/*
  0004_DenshaBekutoru – Optocoupler Averaging Test
  Target dev board (later): Arduino Pro Mini 5V / 16 MHz (ATmega328P)

  Measures average voltage on:
   - A2 -> later ATtiny85 PB3 (XTAL1, Pin 2)
   - A3 -> later ATtiny85 PB4 (XTAL2, Pin 3)

  Boot calibration:
   - assumes "no movement" at startup
   - measures V1/V2 AverageRepeat times
   - computes di = |V1 - V2| each time
   - VdiffBaseline = median(di)
   - VdiffThreshold = max(VdiffBaseline * VdiffThresholdMargin, VdiffThresholdMinVolts)
*/

const unsigned long SAMPLE_WINDOW_MS = 100;   // averaging window per measurement
const unsigned long SAMPLE_DELAY_US  = 500;   // delay between ADC samples (~2 kHz)

const uint8_t ADC_PIN_1 = A2;  // Pro Mini A2 -> later ATtiny85 PB3 (XTAL1, Pin 2)
const uint8_t ADC_PIN_2 = A3;  // Pro Mini A3 -> later ATtiny85 PB4 (XTAL2, Pin 3)

// LEDs for direction indication (avoid D0/D1)
const uint8_t LED_DIR1_PIN = 2;  // D2
const uint8_t LED_DIR2_PIN = 3;  // D3

// Serial output control (0 = no output, 1 = output)
bool SerialOutputAllow = true;

// ---- Calibration parameters ----
const uint16_t AverageRepeat = 100;          // number of init measurements
const float VdiffThresholdMargin = 1.50f;    // multiplier (e.g., 1.5 = +50% margin)
const float VdiffThresholdMinVolts = 0.03f;  // floor in volts (optional but practical)

// Globals: latest measured averaged voltages
float v1 = 0.0f;
float v2 = 0.0f;

// Calibration globals
float VdiffBaseline = 0.0f;   // median(|V1-V2|) measured at boot (no movement)
float VdiffThreshold = 0.0f;  // final threshold used for direction decision

// Direction encoding requirement:
// 0 = no direction
// 2 = direction 1 (maps to LED pin D2)
// 3 = direction 2 (maps to LED pin D3)
byte direction = 0;

static inline float absf(float x) { return (x >= 0.0f) ? x : -x; }
static inline float maxf(float a, float b) { return (a > b) ? a : b; }

static void applyDirectionOutputs()
{
  digitalWrite(LED_DIR1_PIN, LOW);
  digitalWrite(LED_DIR2_PIN, LOW);

  if (direction == LED_DIR1_PIN) {
    digitalWrite(LED_DIR1_PIN, HIGH);
  } else if (direction == LED_DIR2_PIN) {
    digitalWrite(LED_DIR2_PIN, HIGH);
  }
}

// Measure averaged voltages (updates globals v1/v2)
static void measureAveragedVoltages()
{
  unsigned long startTime = millis();

  unsigned long sum1 = 0;
  unsigned long sum2 = 0;
  unsigned long samples = 0;

  while (millis() - startTime < SAMPLE_WINDOW_MS) {
    sum1 += analogRead(ADC_PIN_1);
    sum2 += analogRead(ADC_PIN_2);
    samples++;
    delayMicroseconds(SAMPLE_DELAY_US);
  }

  float avg1 = (float)sum1 / samples;
  float avg2 = (float)sum2 / samples;

  // Convert ADC value to voltage (default analog reference = Vcc ~ 5V)
  v1 = avg1 * (5.0f / 1023.0f);
  v2 = avg2 * (5.0f / 1023.0f);
}

static void sortFloatArray(float *a, uint16_t n)
{
  // Insertion sort (n=100 is small, OK)
  for (uint16_t i = 1; i < n; i++) {
    float key = a[i];
    int16_t j = (int16_t)i - 1;
    while (j >= 0 && a[j] > key) {
      a[j + 1] = a[j];
      j--;
    }
    a[j + 1] = key;
  }
}

static float medianOfSortedFloatArray(const float *a, uint16_t n)
{
  if (n == 0) return 0.0f;
  if (n & 1) {
    return a[n / 2];
  } else {
    return (a[(n / 2) - 1] + a[n / 2]) * 0.5f;
  }
}

// Calibrate baseline + threshold once at boot/reset
// Parameter must be only AverageRepeat (per your requirement).
static void calibrateVdiffThreshold(uint16_t averageRepeat)
{
  if (averageRepeat < 1) averageRepeat = 1;
  if (averageRepeat > 200) averageRepeat = 200; // RAM safety cap

  float diffs[200];

  for (uint16_t i = 0; i < averageRepeat; i++) {
    measureAveragedVoltages();
    diffs[i] = absf(v1 - v2); // baseline mismatch sample
  }

  sortFloatArray(diffs, averageRepeat);
  VdiffBaseline = medianOfSortedFloatArray(diffs, averageRepeat);

  // Final threshold with margin + floor
  VdiffThreshold = maxf(VdiffBaseline * VdiffThresholdMargin, VdiffThresholdMinVolts);
}

static void updateDirectionFromVoltages()
{
  float diff = v1 - v2;
  float absDiff = absf(diff);

  if (absDiff <= VdiffThreshold) {
    direction = 0;
  } else {
    // Mapping choice:
    // v1 lower than v2 -> direction 1 (D2)
    // v2 lower than v1 -> direction 2 (D3)
    direction = (diff < 0.0f) ? LED_DIR1_PIN : LED_DIR2_PIN;
  }
}

static void serialPrintAll()
{
  if (!SerialOutputAllow) return;

  Serial.print("A2 for PB3  XTAL1, Pin2: ");
  Serial.print(v1, 3);
  Serial.print(" V   |   ");

  Serial.print("A3 for PB4 XTAL2, Pin3: ");
  Serial.print(v2, 3);
  Serial.print(" V   |   ");

  Serial.print("|V1-V2|: ");
  Serial.print(absf(v1 - v2), 3);
  Serial.print(" V   |   ");

  Serial.print("VdiffBaseline: ");
  Serial.print(VdiffBaseline, 3);
  Serial.print(" V   |   ");

  Serial.print("VdiffThresholdMargin: ");
  Serial.print(VdiffThresholdMargin, 2);
  Serial.print(" x   |   ");

  Serial.print("VdiffThresholdMinVolts: ");
  Serial.print(VdiffThresholdMinVolts, 3);
  Serial.print(" V   |   ");

  Serial.print("VdiffThreshold: ");
  Serial.print(VdiffThreshold, 3);
  Serial.print(" V   |   ");

  Serial.print("direction: ");
  Serial.println(direction);
}

void setup() {
  Serial.begin(115200);

  pinMode(ADC_PIN_1, INPUT);
  pinMode(ADC_PIN_2, INPUT);

  pinMode(LED_DIR1_PIN, OUTPUT);
  pinMode(LED_DIR2_PIN, OUTPUT);

  calibrateVdiffThreshold(AverageRepeat);

  // One measurement for initial output + outputs
  measureAveragedVoltages();
  updateDirectionFromVoltages();
  applyDirectionOutputs();

  if (SerialOutputAllow) {
    Serial.println("=== DenshaBekutoru Optocoupler Average Test (Arduino Pro Mini 5V/16MHz) ===");
    Serial.print("AverageRepeat: ");
    Serial.println(AverageRepeat);
  }

  serialPrintAll();
}

void loop() {
  measureAveragedVoltages();
  updateDirectionFromVoltages();
  applyDirectionOutputs();

  serialPrintAll();

  delay(300);
}