// OrbitScan — Mega 2560 ultrasonic mapping lab. Project guide: /projects/orbitscan
// Install official Servo and LiquidCrystal libraries. Keep ScanCore.h in this folder.
#include <Servo.h>
#include <LiquidCrystal.h>
#include <util/atomic.h>
#include "ScanCore.h"

const uint8_t TRIG=22,ECHO=18,SERVO=9,PAUSE_BUTTON=24,THRESHOLD_BUTTON=25;
const uint8_t RED=6,GREEN=7,BLUE=8;
const bool COMMON_ANODE=false;
LiquidCrystal lcd(12,11,5,4,3,2);
Servo head;
Scan::Sweep sweep;
Scan::Button pauseButton,thresholdButton;
Scan::Proximity proximity;
volatile bool armed=false,riseSeen=false,echoReady=false;
volatile uint32_t riseUs=0,pulseUs=0;
bool running=true,waiting=false;
uint32_t sentUs=0,lastShotMs=0,movedMs=0,settleMs=500;
uint16_t raw[3]={0,0,0},threshold=40;
uint8_t sample=0;
enum Stage {SETTLE,SAMPLE};Stage stage=SETTLE;
uint16_t mapCm[Scan::BINS]={0}; // 0 means unknown; never free space

void echoChanged(){
  if(!armed)return;
  uint32_t now=micros();
  if(digitalRead(ECHO)){riseUs=now;riseSeen=true;}
  else if(riseSeen){pulseUs=uint32_t(now-riseUs);echoReady=true;armed=false;}
}
void disarm(){ATOMIC_BLOCK(ATOMIC_RESTORESTATE){armed=false;echoReady=false;riseSeen=false;}waiting=false;}
void rgb(bool r,bool g,bool b){
  digitalWrite(RED,r^COMMON_ANODE);digitalWrite(GREEN,g^COMMON_ANODE);digitalWrite(BLUE,b^COMMON_ANODE);
}
void displayResult(Scan::Filtered f){
  lcd.setCursor(0,0);lcd.print(F("                "));lcd.setCursor(0,0);
  lcd.print(sweep.angle());lcd.print(F("deg "));
  if(f.cm){lcd.print(f.cm);lcd.print(F("cm"));}else lcd.print(F("NO ECHO"));
  lcd.setCursor(0,1);lcd.print(F("                "));lcd.setCursor(0,1);
  lcd.print(F("T"));lcd.print(threshold);lcd.print(F(" V"));lcd.print(f.valid);lcd.print(F("/3 "));
  lcd.print(!f.cm?F("UNKNOWN"):proximity.near?F("NEAR"):F("SCAN"));
  if(!f.cm)rgb(true,false,true);else if(proximity.near)rgb(true,false,false);else rgb(false,true,false);
}
void finishBin(uint32_t now){
  Scan::Filtered f=Scan::filter(raw);mapCm[sweep.bin]=f.cm;proximity.update(f.cm,threshold);
  displayResult(f);
  Serial.print(now);Serial.print(',');Serial.print(sweep.passes);Serial.print(',');
  Serial.print(sweep.angle());Serial.print(',');
  if(f.cm)Serial.print(f.cm);else Serial.print(-1);
  Serial.print(',');Serial.print(f.valid);Serial.print(',');Serial.println(f.spread);
  sweep.advance();head.write(sweep.angle());movedMs=now;settleMs=65;stage=SETTLE;sample=0;
}
void takeSample(uint32_t duration,uint32_t now){
  waiting=false;raw[sample++]=Scan::cmFromPulse(duration);
  if(sample==3)finishBin(now);
}
void togglePause(uint32_t now){
  running=!running;disarm();sample=0;stage=SETTLE;movedMs=now;
  if(running){head.write(sweep.angle());settleMs=500;}
  if(!running){lcd.setCursor(0,1);lcd.print(F("PAUSED          "));rgb(false,false,true);}
}
void setup(){
  pinMode(TRIG,OUTPUT);digitalWrite(TRIG,LOW);pinMode(ECHO,INPUT);
  pinMode(PAUSE_BUTTON,INPUT_PULLUP);pinMode(THRESHOLD_BUTTON,INPUT_PULLUP);
  pinMode(RED,OUTPUT);pinMode(GREEN,OUTPUT);pinMode(BLUE,OUTPUT);rgb(false,false,true);
  Serial.begin(115200);Serial.println(F("ms,sweep,angle_deg,distance_cm,valid_samples,spread_cm"));
  lcd.begin(16,2);lcd.print(F("Hello, Ori!"));lcd.setCursor(0,1);lcd.print(F("OrbitScan / init"));
  attachInterrupt(digitalPinToInterrupt(ECHO),echoChanged,CHANGE);
  head.attach(SERVO);head.write(sweep.angle());movedMs=millis();lastShotMs=movedMs;
}
void loop(){
  uint32_t now=millis();
  if(pauseButton.update(digitalRead(PAUSE_BUTTON)==LOW,now))togglePause(now);
  if(thresholdButton.update(digitalRead(THRESHOLD_BUTTON)==LOW,now))threshold=threshold==60?20:threshold+20;
  while(Serial.available()){
    char c=Serial.read();if(c=='p')togglePause(now);
    if(c=='n')threshold=threshold==60?20:threshold+20;
    if(c=='c'){
      running=false;disarm();sample=0;head.write(90);rgb(false,false,true);
      lcd.setCursor(0,0);lcd.print(F("CENTER 90deg    "));lcd.setCursor(0,1);lcd.print(F("PAUSED          "));
    }
  }
  if(!running)return; // servo holds its last angle; never force its shaft
  if(stage==SETTLE){if(!Scan::elapsed(now,movedMs,settleMs))return;stage=SAMPLE;}
  if(waiting){
    bool ready;uint32_t width;
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE){ready=echoReady;width=pulseUs;if(ready)echoReady=false;}
    if(ready){takeSample(width,now);return;}
    if(Scan::elapsed(micros(),sentUs,Scan::TIMEOUT_US)){disarm();takeSample(0,now);}
    return;
  }
  if(!Scan::elapsed(now,lastShotMs,Scan::SHOT_GAP_MS))return;
  // If Echo is stuck HIGH, count an invalid sample rather than manufacturing distance.
  lastShotMs=now;
  if(digitalRead(ECHO)==HIGH){takeSample(0,now);return;}
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE){echoReady=false;riseSeen=false;armed=true;}
  sentUs=micros();waiting=true;
  digitalWrite(TRIG,HIGH);delayMicroseconds(10);digitalWrite(TRIG,LOW);
}