/*
  0004_DenshaBekutoru – 1st Version PCB
 
  PCB Version      : 2026/02 (Order F016895)
  Target dev board : Arduino Pro Mini 5V / 16 MHz (ATmega328P)
  
  ----------------------------------------------------------------------------
  State machine (direction memory) + Hysteresis (Version A)
  ----------------------------------------------------------------------------
  We use two thresholds:
    T_hold  (smaller): below -> treat as neutral and HOLD state
    T_enter (larger) : above -> allow direction change (set by sign)

  Behavior:
    If |diff| <= T_hold  : hold direction
    If |diff| >= T_enter : set direction by sign
    Else (between)       : hold direction

  Presets (implemented as multipliers of the calibrated VdiffThreshold):
    T_hold  = 1.0 * VdiffThreshold
    T_enter = 2.0 * VdiffThreshold

  Inputs derived from v1, v2: diff = v1 - v2
*/

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 = A0;  // Pro Mini A0 -> (PCB dependend!)
const uint8_t ADC_PIN_2 = A1;  // Pro Mini A1 -> (PCB dependend")

// Direction LEDs (avoid D0/D1 because you may want RX/TX later)
const uint8_t LED_DIR1_PIN = 3;  // D3
const uint8_t LED_DIR2_PIN = 9;  // D9

const unsigned int LED_BLINK_ON_MS  = 150;
const unsigned int LED_BLINK_OFF_MS = 150;

// 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

// ---- Hysteresis presets (multipliers of VdiffThreshold) ----
const float THOLD_MULT  = 1.0f;  // T_hold  = THOLD_MULT  * VdiffThreshold
const float TENTER_MULT = 2.0f;  // T_enter = TENTER_MULT * VdiffThreshold

// 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;  // calibrated base threshold (used to derive T_hold/T_enter)

// Hysteresis thresholds (derived once after calibration)
float T_hold  = 0.0f;
float T_enter = 0.0f;

// Direction encoding:
// 0 = NONE, 2 = DIR1, 3 = DIR2
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; }

// -----------------------------------------------------------------------------
// LED helpers
// -----------------------------------------------------------------------------
static void blinkLED(uint8_t pin, uint8_t times)
{
  for (uint8_t i = 0; i < times; i++) {
    digitalWrite(pin, HIGH);
    delay(LED_BLINK_ON_MS);
    digitalWrite(pin, LOW);
    delay(LED_BLINK_OFF_MS);
  }
}

static void initBlinkSequence()
{
  // LED at Output 2 blinks 2 times
  blinkLED(LED_DIR1_PIN, 2);

  // LED at Output 3 blinks 3 times
  blinkLED(LED_DIR2_PIN, 3);
}

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

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

// -----------------------------------------------------------------------------
// Measurement
// -----------------------------------------------------------------------------
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);
}

// -----------------------------------------------------------------------------
// Median helpers (for calibration)
// -----------------------------------------------------------------------------
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 + VdiffThreshold once at boot/reset
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);

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

  // Derive hysteresis thresholds (Version A)
  T_hold  = THOLD_MULT  * VdiffThreshold;
  T_enter = TENTER_MULT * VdiffThreshold;
}

// -----------------------------------------------------------------------------
// State machine with hysteresis (Version A)
// -----------------------------------------------------------------------------
static void updateDirectionFromVoltages()
{
  const float diff = v1 - v2;
  const float absDiff = absf(diff);

  // 1) Neutral region: HOLD
  if (absDiff <= T_hold) {
    return;
  }

  // 2) Strong region: allow direction change
  if (absDiff >= T_enter) {
    direction = (diff > 0.0f) ? (byte)2 : (byte)3;
    return;
  }

  // 3) Deadband between T_hold and T_enter: HOLD
  return;
}

// -----------------------------------------------------------------------------
// Serial output
// -----------------------------------------------------------------------------
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("VdiffThreshold: ");
  Serial.print(VdiffThreshold, 3);
  Serial.print(" V   |   ");

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

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

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

// -----------------------------------------------------------------------------
// Arduino entry points
// -----------------------------------------------------------------------------
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);

  // Initial calibration (assumes no movement)
  calibrateVdiffThreshold(AverageRepeat);

  // Init blink sequence: 2x on D2, 3x on D3
  initBlinkSequence();

  // One measurement for initial display + initial direction
  measureAveragedVoltages();
  updateDirectionFromVoltages();
  applyDirectionOutputs();

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

  serialPrintAll();
}

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

  serialPrintAll();

  delay(300);
}
