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

// ── ADC Interrupt-driven single-channel read (A0) ────────────
volatile uint16_t adc_result = 0;
volatile bool     adc_ready  = false;

void setupADC() {
  // Reference = AVCC (5 V)
  ADMUX = (1 << REFS0);

  // Channel = A0 (MUX[3:0] = 0000)
  ADMUX &= 0xF0;

  // Prescaler = 16  → 16 MHz / 16 = 1 MHz ADC clock
  // Each conversion ≈ 13 ADC clocks → ~76.9 kHz sample rate
  ADCSRA = (1 << ADEN)  | (1 << ADIE)
         | (1 << ADPS2);               // ADPS = 100 → /16

  // Start first conversion
  ADCSRA |= (1 << ADSC);
}

// ── OOR digital input ────────────────────────────────────────
#define OOR_PIN 2          // digital pin 2 — HIGH = out of range

volatile bool OOR;

ISR(ADC_vect) {
  uint16_t sample = ADC;
  // Discard if OOR pin (PD2) is HIGH
  OOR = (digitalRead(OOR_PIN));
  if (!OOR) {
    adc_result = sample;
    adc_ready  = true;
  }
  ADCSRA |= (1 << ADSC); // kick off next conversion immediately
}

// ── ADC → mm linear mapping ─────────────────────────────────
// ADC 185 → 16 mm,  ADC 900 → 26 mm
// #define adcToMM(adc) (16.0f + (float)((adc) - 185) * (10.0f / 715.0f))
#define adcToMM(adc) (0.0f + (float)((adc) - 0) * (10.0f / 1024.0f))

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

// Insert val into a sorted-ascending array of up to TRACK_N entries
// keeping only the N smallest values seen so far.
static void trackLowest(uint16_t val) {
  if (lowestCount < TRACK_N) {
    // Array not full — insert in sorted position
    uint8_t i = lowestCount;
    while (i > 0 && lowestVals[i - 1] > val) {
      lowestVals[i] = lowestVals[i - 1];
      i--;
    }
    lowestVals[i] = val;
    lowestCount++;
  } else if (val < lowestVals[TRACK_N - 1]) {
    // Replace the current largest of the "lowest" set
    uint8_t i = TRACK_N - 1;
    while (i > 0 && lowestVals[i - 1] > val) {
      lowestVals[i] = lowestVals[i - 1];
      i--;
    }
    lowestVals[i] = val;
  }
}

// Insert val into a sorted-descending array of up to TRACK_N entries
// keeping only the N largest values seen so far.
static void trackHighest(uint16_t val) {
  if (highestCount < TRACK_N) {
    uint8_t i = highestCount;
    while (i > 0 && highestVals[i - 1] < val) {
      highestVals[i] = highestVals[i - 1];
      i--;
    }
    highestVals[i] = val;
    highestCount++;
  } else if (val > highestVals[TRACK_N - 1]) {
    uint8_t i = TRACK_N - 1;
    while (i > 0 && highestVals[i - 1] < val) {
      highestVals[i] = highestVals[i - 1];
      i--;
    }
    highestVals[i] = val;
  }
}

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

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

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

// ═════════════════════════════════════════════════════════════
void setup() {
  Serial.begin(2000000);
  pinMode(OOR_PIN, 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;

  // Grab the latest ADC value atomically
  uint16_t val;
  bool ready;
  bool newOOR;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    ready = adc_ready;
    val   = adc_result;
    adc_ready = false;
    newOOR = OOR;
  }

  if (!ready) return;  // nothing new — come back next iteration

  if (rawMode) {
    long mm_x100 = map(val, 178, 895, 1600, 2600);
    Serial.print(mm_x100 / 100);
    Serial.print('.');
    long frac = mm_x100 % 100;
    if (frac < 10) Serial.print('0');
    Serial.println(frac);
    return;
  }

  // All values here are in-range (OOR filtered in ISR)
  trackLowest(val);
  trackHighest(val);

  float mm = adcToMM(val);
  Serial.print(val);
  Serial.print(", ");
  Serial.print(mm, 2);
  Serial.print(" mm, ");
  Serial.println(newOOR ? "out of range" : "in range");
}