// A0CalibrationSketch
// ────────────────────────────────────────────────────────────
// Trimmed-down calibration streamer. Continuously outputs
// "<mm>, <a0_raw>" lines over serial, where <mm> is the
// position measured by one of the two known sensors (A2, A3)
// and <a0_raw> is the raw ADC count from the unknown sensor
// on A0. A companion Python notebook buckets these points
// into 0.05mm intervals and exports an Excel calibration.
//
// Handshake protocol (restartable without re-uploading):
//   Wake byte : 'S'  Python -> Arduino  (start/restart streaming)
//   Stop byte : 'X'  Python -> Arduino  (stop streaming, return to idle)
//
//   Idle state  : Arduino waits for 'S', ignores all other bytes.
//   Stream state: Arduino emits data lines, watches for 'X'.
//                 On 'X' it returns to idle state.
//
//   Python connect sequence (works regardless of current Arduino state):
//     1. Send 'X'             (stops streaming if running; no-op if idle)
//     2. sleep 200 ms + flush (drain any in-flight data lines)
//     3. Send 'S'             (Arduino: 1 s settle, then prints #READY)
//     4. Wait for '#READY'
//
//   Data lines: <float_mm>, <int_a0_raw>\n  — no other output ever.
// ────────────────────────────────────────────────────────────

#include <Arduino.h>
#include <util/atomic.h>

// ── ADC Interrupt-driven 3-channel read (A2, A3, A0) ─────────
// Channel index: 0 → A2 (sensor 0), 1 → A3 (sensor 1), 2 → A0 (unknown)
static const uint8_t adc_mux[3] = {2, 3, 1};

volatile uint16_t adc_result[3] = {0, 0, 0};
volatile bool     adc_ready[3]  = {false, false, false};
volatile uint8_t  adc_channel   = 0;

void setupADC() {
  ADMUX = (1 << REFS0) | adc_mux[0];   // AVCC ref, start on A2
  ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADPS2);  // /16 prescaler
  ADCSRA |= (1 << ADSC);
}

// ── OOR digital inputs ───────────────────────────────────────
#define OOR_PIN_0  12    // HIGH = out of range, sensor 0 (A2)
#define OOR_PIN_1  13    // HIGH = out of range, sensor 1 (A3)

volatile bool OOR[2];

ISR(ADC_vect) {
  uint16_t sample = ADC;
  uint8_t ch   = adc_channel;
  uint8_t next = (ch + 1) % 3;

  if (ch < 2) {
    OOR[ch] = digitalRead(ch == 0 ? OOR_PIN_0 : OOR_PIN_1);
    if (!OOR[ch]) {
      adc_result[ch] = sample;
      adc_ready[ch]  = true;
    }
  } else {
    // A0: no OOR, always store
    adc_result[2] = sample;
    adc_ready[2]  = true;
  }

  ADMUX = (ADMUX & 0xF0) | adc_mux[next];
  adc_channel = next;
  ADCSRA |= (1 << ADSC);
}

// ── ADC → mm linear mappings (raw range: 16–26 mm) ──────────
// Kept identical to AltSensorTesting.ino so calibration is
// performed in the same position frame.
#define adcToMM0(adc) ((float)map(adc, 178, 895, 1600, 2600) / 100.0f)
#define adcToMM1(adc) ((float)map(adc, 176, 885, 1600, 2600) / 100.0f)

// Mounting offsets so sensor 0 → 0–10 mm, sensor 1 → 10–20 mm
#define OFFSET_MM0  15.6f
#define OFFSET_MM1   6.2f

// ── Streaming state ──────────────────────────────────────────
bool streaming = false;

// Enter idle: wait for the 'S' wake byte (all other bytes ignored).
// Then settle, clear stale ADC flags, announce #READY, and set streaming.
// Can be called from both setup() and loop() for restartable sessions.
void waitForWake() {
  streaming = false;
  while (true) {
    if (Serial.available()) {
      char c = Serial.read();
      if (c == 'S') break;
      // Any other byte (e.g. stray 'X') is silently discarded.
    }
  }
  delay(1000);  // ADC reference settle
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    adc_ready[0] = adc_ready[1] = adc_ready[2] = false;
  }
  Serial.println(F("#READY"));
  streaming = true;
}

// ═════════════════════════════════════════════════════════════
void setup() {
  Serial.begin(2000000);
  pinMode(OOR_PIN_0, INPUT);
  pinMode(OOR_PIN_1, INPUT);
  setupADC();      // ADC runs continuously; emission is gated by streaming flag
  waitForWake();
}

void loop() {
  // Check for stop byte before doing any work.
  if (Serial.available()) {
    char c = Serial.read();
    if (c == 'X') {
      waitForWake();  // returns only after next 'S' + #READY
      return;
    }
  }

  if (!streaming) return;

  uint16_t val[3];
  bool ready[3];
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    for (uint8_t i = 0; i < 3; i++) {
      ready[i]     = adc_ready[i];
      val[i]       = adc_result[i];
      adc_ready[i] = false;
    }
  }

  if (!ready[0] && !ready[1]) return;

  // Emit one line per in-range sensor sample, paired with the
  // most recent A0 raw ADC count (val[2] is always fresh).
  if (ready[0]) {
    float mm = adcToMM0(val[0]) - OFFSET_MM0;
    Serial.print(mm, 3);
    Serial.print(F(", "));
    Serial.println(val[2]);
  }
  if (ready[1]) {
    float mm = adcToMM1(val[1]) - OFFSET_MM1;
    Serial.print(mm, 3);
    Serial.print(F(", "));
    Serial.println(val[2]);
  }
}