/*
  0004_DenshaBekutoru – State Machine implemented
  Target dev board (later): Arduino Pro Mini 5V / 16 MHz (ATmega328P)

  ----------------------------------------------------------------------------
  State machine (direction memory)
  ----------------------------------------------------------------------------
  Inputs derived from v1, v2, VdiffThreshold:

    N (Neutral):  |v1 - v2| <= VdiffThreshold
    D1 (v1>>v2):  (v1 - v2) >  VdiffThreshold
    D2 (v2>>v1):  (v2 - v1) >  VdiffThreshold

  State encoding (kept transfer-friendly, and compatible with earlier pin-mapping):
    0 = NONE
    2 = DIR1
    3 = DIR2

  Transition table:

    Current \ Input |   N (neutral)   |   D1 (v1>>v2) |   D2 (v2>>v1)
    -----------------------------------------------------------------
       0 (NONE)     |      0          |       2       |       3
       2 (DIR1)     |      2          |       2       |       3
       3 (DIR2)     |      3          |       2       |       3

  Mermaid (documentation)
  -----------------------
  stateDiagram-v2
      [*] --> NONE
      state "0 : NONE" as NONE
      state "2 : DIR1 (v1>>v2)" as DIR1
      state "3 : DIR2 (v2>>v1)" as DIR2

      NONE --> NONE : |v1 - v2| <= T
      NONE --> DIR1 : v1 - v2 > T
      NONE --> DIR2 : v2 - v1 > T

      DIR1 --> DIR1 : |v1 - v2| <= T
      DIR1 --> DIR1 : v1 - v2 > T
      DIR1 --> DIR2 : v2 - v1 > T

      DIR2 --> DIR2 : |v1 - v2| <= T
      DIR2 --> DIR2 : v2 - v1 > T
      DIR2 --> DIR1 : v1 - v2 > T
*/

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)

// 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:
// 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; }

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

// Implements the state machine table above
static void updateDirectionFromVoltages()
{
  const float diff = v1 - v2;
  const float absDiff = absf(diff);

  // Neutral region: hold direction (only NONE stays NONE)
  if (absDiff <= VdiffThreshold) {
    // direction remains unchanged (0 stays 0; 2 stays 2; 3 stays 3)
    return;
  }

  // Clear dominance: set direction deterministically
  // diff > 0  => v1 >> v2 => DIR1 (2)
  // diff < 0  => v2 >> v1 => DIR2 (3)
  direction = (diff > 0.0f) ? (byte)2 : (byte)3;
}

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);

  // Initial calibration: compute VdiffThreshold once after boot/reset
  calibrateVdiffThreshold(AverageRepeat);

  // One measurement for initial display
  measureAveragedVoltages();
  updateDirectionFromVoltages();

  if (SerialOutputAllow) {
    Serial.println("=== DenshaBekutoru State Machine and Variable Output (Arduino Pro Mini 5V/16MHz) ===");
    Serial.println("~~~ Init ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
    Serial.print("AverageRepeat: ");
    Serial.println(AverageRepeat);
    Serial.println("~~~ Init ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
  }

  serialPrintAll();
}

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

  serialPrintAll();

  delay(300);
}
