#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 (raw ref)
static const uint8_t adc_mux[3] = {2, 3, 0};

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) {
    // Sensor channels: filter by OOR
    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) ──────────
#define adcToMM0(adc) ((float)map(adc, 178, 895, 1600, 2600) / 100.0f)
#define adcToMM1(adc) ((float)map(adc, 176, 885, 1600, 2600) / 100.0f)

// Subtract mounting offsets so both sensors share the same position frame:
//   Sensor 0 raw 16–26 mm − 16 → 0–10 mm
//   Sensor 1 raw 16–26 mm −  6 → 10–20 mm
#define OFFSET_MM0  15.6f
#define OFFSET_MM1   6.2f

// ── Boundary tracking (in-range only, per sensor) ────────────
#define TRACK_N 10
uint16_t lowestVals[2][TRACK_N];
uint16_t highestVals[2][TRACK_N];
uint8_t  lowestCount[2]  = {0, 0};
uint8_t  highestCount[2] = {0, 0};

static void trackLowest(uint8_t s, uint16_t val) {
  uint16_t *lv = lowestVals[s];
  uint8_t  &lc = lowestCount[s];
  if (lc < TRACK_N) {
    uint8_t i = lc;
    while (i > 0 && lv[i - 1] > val) { lv[i] = lv[i - 1]; i--; }
    lv[i] = val;
    lc++;
  } else if (val < lv[TRACK_N - 1]) {
    uint8_t i = TRACK_N - 1;
    while (i > 0 && lv[i - 1] > val) { lv[i] = lv[i - 1]; i--; }
    lv[i] = val;
  }
}

static void trackHighest(uint8_t s, uint16_t val) {
  uint16_t *hv = highestVals[s];
  uint8_t  &hc = highestCount[s];
  if (hc < TRACK_N) {
    uint8_t i = hc;
    while (i > 0 && hv[i - 1] < val) { hv[i] = hv[i - 1]; i--; }
    hv[i] = val;
    hc++;
  } else if (val > hv[TRACK_N - 1]) {
    uint8_t i = TRACK_N - 1;
    while (i > 0 && hv[i - 1] < val) { hv[i] = hv[i - 1]; i--; }
    hv[i] = val;
  }
}

static void resetTracking() {
  lowestCount[0] = highestCount[0] = 0;
  lowestCount[1] = highestCount[1] = 0;
}

static void printBoundaries() {
  for (uint8_t s = 0; s < 2; s++) {
    Serial.print(F("--- Sensor "));
    Serial.print(s);
    Serial.println(F(": 10 Lowest In-Range ADC Values ---"));
    for (uint8_t i = 0; i < lowestCount[s]; i++) Serial.println(lowestVals[s][i]);
    Serial.print(F("--- Sensor "));
    Serial.print(s);
    Serial.println(F(": 10 Highest In-Range ADC Values ---"));
    for (uint8_t i = 0; i < highestCount[s]; i++) Serial.println(highestVals[s][i]);
  }
}

// ── State ────────────────────────────────────────────────────
bool sampling = false;
bool rawMode  = false;

// ═════════════════════════════════════════════════════════════
void setup() {
  Serial.begin(115200);
  pinMode(OOR_PIN_0, INPUT);
  pinMode(OOR_PIN_1, INPUT);
  setupADC();
  Serial.println(F("Send '1' to start sampling, '0' to stop and print bounds, '2' for raw ADC output."));
}

void loop() {
  // ── Serial command handling ──────────────────────────────
  if (Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();

    if (cmd.charAt(0) == '1') {
      sampling = true;
      rawMode  = false;
      resetTracking();
      Serial.println(F("Sampling started."));
    } else if (cmd.charAt(0) == '2') {
      sampling = true;
      rawMode  = true;
      Serial.println(F("Raw ADC output started."));
    } else if (cmd.charAt(0) == '0') {
      sampling = false;
      rawMode  = false;
      Serial.println(F("Sampling stopped."));
      printBoundaries();
    }
  }

  // ── Main sample path ────────────────────────────────────
  if (!sampling) 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;

  if (rawMode) {
    if (ready[0]) {
      Serial.print(adcToMM0(val[0]) - OFFSET_MM0);
      Serial.print(F(", "));
      Serial.println(val[2]);
    }
    if (ready[1]) {
      Serial.print(adcToMM1(val[1]) - OFFSET_MM1);
      Serial.print(F(", "));
      Serial.println(val[2]);
    }
    return;
  }

  // Apply offset for whichever sensor(s) are in range
  if (ready[0]) {
    float mm = adcToMM0(val[0]) - OFFSET_MM0;
    trackLowest(0, val[0]);
    trackHighest(0, val[0]);
    Serial.print(val[0]);
    Serial.print(F(", "));
    Serial.print(mm, 2);
    Serial.println(F(" mm (s0)"));
  }
  if (ready[1]) {
    float mm = adcToMM1(val[1]) - OFFSET_MM1;
    trackLowest(1, val[1]);
    trackHighest(1, val[1]);
    Serial.print(val[1]);
    Serial.print(F(", "));
    Serial.print(mm, 2);
    Serial.println(F(" mm (s1)"));
  }
}