#include <Arduino.h>  // <-- ensure uint16_t, etc. are defined before any use

/*
  Transfer Case Encoder (Encoder-Only) + Boot Emulators + Rich Debug
  ------------------------------------------------------------------
  • A0 POT → S1..S4 (active-LOW relays) using DESCENDING ADC mapping
  • Wider end margins (3 mm), 3 mm Hi/Lo windows, Neutral centered at 25 mm
  • Fast EMA, higher hysteresis, dwell & no-skip guard
  • A0 float-probe fallback → safe Hi Mode
  • Boot emulators:
      (A) Auto sweep @70 mm/s forward & back (drives relays, prints transitions, warns on skip)
      (B) Manual step-through on 'n' (drives relays, prints mm/raw windows)
  • Debug prints include actual S1–S4 pin states

  Relay logic (to ECU):
    - bit0=S1, bit1=S2, bit2=S3, bit3=S4
    - mask bit=1 → drive pin LOW (relay ON → switch CLOSED → logic 0 at ECU)
    - Wire each relay: COM → ECU pin (S1..S4), NO → GND, NC unused
*/

//////////////////// USER TOGGLES ////////////////////
// Run emulators at boot
#define RUN_EMULATOR_ON_BOOT      1   // 1=run, 0=skip all emulators
#define RUN_AUTO_SWEEP            1   // 1=70 mm/s forward & back sweep
#define RUN_MANUAL_AFTER_SWEEP    1   // 1=step 'n' per mode after sweep

// Tables at boot (outside emulators)
#define PRINT_THRESHOLDS_ON_BOOT  1

// Encoder debug: 0=off, 1=on-change, 2=periodic
#define ENC_DEBUG_LEVEL           1
const unsigned long ENC_PERIOD_MS = 150; // when ENC_DEBUG_LEVEL==2

// Auto-sweep parameters
const float    SWEEP_SPEED_MM_S = 70.0f; // actuator spec
const uint16_t SWEEP_STEP_MS    = 10;    // sweep print resolution
//////////////////////////////////////////////////////

// ---------------- Pins ----------------
const uint8_t POT_PIN = A0;
const uint8_t S1_PIN  = 2;
const uint8_t S2_PIN  = 3;
const uint8_t S3_PIN  = 4;
const uint8_t S4_PIN  = 5;
const uint8_t LED_PIN = 13;

// ---------------- Calibration ----------------
// Raw ADC at physical stops (desc: extended→smaller; retracted→larger)
const int16_t CAL_MIN_RAW = 420 + 3;   // fully extended + margin
const int16_t CAL_MAX_RAW = 596 - 3;   // fully retracted - margin

// Stroke geometry (tenths of mm; 50.0 mm → 500)
const int16_t FULL_STROKE_TMM = 500;
// Windows: 3 mm end margins, 3 mm Hi/Lo; Neutral centered at 25 mm
// Layout (tmm): 0–30 Stop, 30–60 LHi, 60–90 Hi, 90–220 RHi, 220–250 Z1,
//               250–280 Neutral, 280–390 Z2, 390–420 Lo, 420–500 Right Stop

// ---------------- S1..S4 masks (VERIFIED) ----------------
struct Mode { uint8_t sMask; const char* name; };
const Mode modes[] = {
  {0b0111, "Left Stop"},          // S1..S4 = 1110
  {0b0101, "Left of Hi Mode"},    // S1..S4 = 1010
  {0b0100, "Hi Mode"},            // S1..S4 = 0010
  {0b0000, "Right of Hi Mode"},   // S1..S4 = 0000
  {0b0001, "Zone 1"},             // S1..S4 = 1000
  {0b1001, "Neutral Mode"},       // S1..S4 = 1001
  {0b1000, "Zone 2"},             // S1..S4 = 0001
  {0b1010, "Lo Mode"},            // S1..S4 = 0101
  {0b0010, "Right Stop"}          // S1..S4 = 0100
};
const uint8_t NUM_MODES = sizeof(modes)/sizeof(modes[0]);

// ---------------- Encoder internals ----------------
int16_t edgesTmm[10];     // 0..9 edges (tenths of mm)
int16_t thresholdsRaw[9]; // upper-edge raw per mode (DESCENDING)

// EMA, hysteresis, dwell, etc.
const uint8_t  EMA_SHIFT = 3;        // alpha ≈ 1/8
int16_t        emaRaw    = 0;
const int16_t  HYST      = 10;       // raw counts
const uint16_t MODE_DWELL_MS = 120;  // ms to commit

inline uint8_t clampStep(uint8_t candidate, int8_t last) {
  if (last < 0) return candidate;
  if ((int8_t)candidate > last + 1) return (uint8_t)(last + 1);
  if ((int8_t)candidate < last - 1) return (uint8_t)(last - 1);
  return candidate;
}

int8_t         lastModeIdx  = -1;
int8_t         pendingIdx   = -1;
unsigned long  pendingStart = 0;

// A0 float detect + fallback
const int RAW_GUARD_LOW  = (int)CAL_MIN_RAW - 10;
const int RAW_GUARD_HIGH = (int)CAL_MAX_RAW + 10;
bool potConnected = true;
unsigned long potWarnLastMs = 0;
const unsigned long POT_WARN_MS = 1000;

// ---------------- Helpers ----------------
inline void driveOutputs(uint8_t mask) {
  // Active-LOW: 1 bit → LOW (relay ON → closed → 0 at ECU)
  digitalWrite(S1_PIN, (mask & 0b0001) ? LOW : HIGH);
  digitalWrite(S2_PIN, (mask & 0b0010) ? LOW : HIGH);
  digitalWrite(S3_PIN, (mask & 0b0100) ? LOW : HIGH);
  digitalWrite(S4_PIN, (mask & 0b1000) ? LOW : HIGH);
}

inline int16_t clampi(int16_t v, int16_t lo, int16_t hi) {
  return v < lo ? lo : (v > hi ? hi : v);
}

int16_t mmTenthToRaw(int16_t tmm) {
  tmm = clampi(tmm, 0, FULL_STROKE_TMM);
  float t = (float)tmm / (float)FULL_STROKE_TMM;
  float raw = CAL_MAX_RAW - t * (CAL_MAX_RAW - CAL_MIN_RAW); // DESCENDING
  return (int16_t)(raw + 0.5f);
}

void buildModeEdgesAndThresholds() {
  // Place windows per the layout above:
  // 0      30     60     90       220   250   280     390   420   500  (tmm)
  // |------|------|------|--------|-----|-----|-------|-----|-----|
  // Stop  LHi    Hi     RHi      Z1   Neu   Z2      Lo    Stop
  edgesTmm[0] =   0;    // 0.0
  edgesTmm[1] =  30;    // 3.0  Left Stop end
  edgesTmm[2] =  60;    // 6.0  Left of Hi end
  edgesTmm[3] =  90;    // 9.0  Hi end (3 mm)
  edgesTmm[4] = 220;    // 22.0 Right of Hi end
  edgesTmm[5] = 250;    // 25.0 Zone 1 end
  edgesTmm[6] = 280;    // 28.0 Neutral end (25–28)
  edgesTmm[7] = 390;    // 39.0 Zone 2 end
  edgesTmm[8] = 420;    // 42.0 Lo end (39–42)
  edgesTmm[9] = 500;    // 50.0 Right Stop end

  for (uint8_t i = 0; i < 9; i++) {
    thresholdsRaw[i] = mmTenthToRaw(edgesTmm[i + 1]);
  }
  // ensure strictly descending
  for (uint8_t i = 1; i < 9; i++) {
    if (thresholdsRaw[i] >= thresholdsRaw[i - 1]) thresholdsRaw[i] = thresholdsRaw[i - 1] - 1;
  }

#if PRINT_THRESHOLDS_ON_BOOT
  Serial.println(F("Idx | Mode Name           | thrUpper(raw) | Code(S1..S4)"));
  for (uint8_t i = 0; i < NUM_MODES; i++) {
    Serial.print(i); Serial.print(F("   | "));
    Serial.print(modes[i].name);
    int pad = 20 - (int)strlen(modes[i].name);
    while (pad-- > 0) Serial.print(' ');
    Serial.print(F(" | "));
    Serial.print(thresholdsRaw[i]);
    Serial.print(F("          | "));
    Serial.print((modes[i].sMask & 0b0001)?'1':'0');
    Serial.print((modes[i].sMask & 0b0010)?'1':'0');
    Serial.print((modes[i].sMask & 0b0100)?'1':'0');
    Serial.print((modes[i].sMask & 0b1000)?'1':'0');
    Serial.println();
  }
#endif
}

// POT safe read with brief INPUT_PULLUP probe
int readPotSafe() {
  pinMode(POT_PIN, INPUT);
  int v1 = analogRead(POT_PIN);
  if (v1 >= RAW_GUARD_LOW && v1 <= RAW_GUARD_HIGH) { potConnected = true; return v1; }
  pinMode(POT_PIN, INPUT_PULLUP);
  (void)analogRead(POT_PIN);           // throwaway
  int v2 = analogRead(POT_PIN);
  pinMode(POT_PIN, INPUT);
  if (v2 > 1000) { potConnected = false; return (int)CAL_MIN_RAW; }
  potConnected = true; return v1;
}

// Mode classification from raw
inline uint8_t modeFromRaw(int16_t raw) {
  if (raw >= thresholdsRaw[0]) return 0;
  for (uint8_t i = 1; i < 9; i++) if (raw >= thresholdsRaw[i]) return i;
  return 8;
}

// -------- Boot Emulator A: 70 mm/s forward & back sweep --------
void runAutoSweepOnce() {
#if RUN_EMULATOR_ON_BOOT && RUN_AUTO_SWEEP
  Serial.println(F("\n=== Auto SWEEP Emulator @70 mm/s ==="));
  Serial.println(F("t(ms) | pos(mm) | raw | idx | name                  | code | note"));

  auto codeStr = [](uint8_t mask){
    char s[5];
    s[0]=(mask&0b0001)?'1':'0';
    s[1]=(mask&0b0010)?'1':'0';
    s[2]=(mask&0b0100)?'1':'0';
    s[3]=(mask&0b1000)?'1':'0';
    s[4]='\0'; return String(s);
  };
  auto printLine = [&](uint32_t ms, float posMm, int16_t raw, uint8_t idx, const __FlashStringHelper* note){
    Serial.print(ms); Serial.print(F(" | "));
    Serial.print(posMm,2); Serial.print(F("    | "));
    Serial.print(raw); Serial.print(F(" | "));
    Serial.print(idx); Serial.print(F("   | "));
    Serial.print(modes[idx].name);
    int pad = 22 - (int)strlen(modes[idx].name); while (pad-- > 0) Serial.print(' ');
    Serial.print(F(" | "));
    Serial.print(codeStr(modes[idx].sMask));
    Serial.print(F(" | "));
    if (note) Serial.print(note);
    Serial.println();
  };

  const float  stepMm   = SWEEP_SPEED_MM_S * (SWEEP_STEP_MS / 1000.0f); // ≈0.7 mm/10ms
  const int16_t stepTmm = (int16_t)(stepMm * 10.0f + 0.5f);

  // Forward 0 → 50
  {
    uint32_t t0 = millis(), nextMs = t0;
    int16_t posTmm = 0;
    int8_t lastIdx = -1;

    while (posTmm <= FULL_STROKE_TMM) {
      uint32_t now = millis();
      if ((int32_t)(now - nextMs) >= 0) {
        nextMs += SWEEP_STEP_MS;
        int16_t raw = mmTenthToRaw(posTmm);
        uint8_t idx = modeFromRaw(raw);
        driveOutputs(modes[idx].sMask);

        if (idx != lastIdx) {
          if (lastIdx >= 0 && idx != (uint8_t)(lastIdx + 1)) {
            printLine(now - t0, posTmm/10.0f, raw, idx, F("SKIP!"));
          } else {
            printLine(now - t0, posTmm/10.0f, raw, idx, F("→ mode change"));
          }
          lastIdx = idx;
        }
        posTmm += stepTmm;
      }
    }
  }

  Serial.println(F("--- reverse ---"));

  // Backward 50 → 0
  {
    uint32_t t0 = millis(), nextMs = t0;
    int16_t posTmm = FULL_STROKE_TMM;
    int8_t lastIdx = -1;

    while (posTmm >= 0) {
      uint32_t now = millis();
      if ((int32_t)(now - nextMs) >= 0) {
        nextMs += SWEEP_STEP_MS;
        int16_t raw = mmTenthToRaw(posTmm);
        uint8_t idx = modeFromRaw(raw);
        driveOutputs(modes[idx].sMask);

        if (idx != lastIdx) {
          if (lastIdx >= 0 && idx + 1 != (uint8_t)lastIdx) {
            printLine(now - t0, posTmm/10.0f, raw, idx, F("SKIP!"));
          } else {
            printLine(now - t0, posTmm/10.0f, raw, idx, F("→ mode change"));
          }
          lastIdx = idx;
        }
        posTmm -= stepTmm;
      }
    }
  }
  Serial.println(F("=== Auto SWEEP done ===\n"));
#endif
}

// -------- Boot Emulator B: manual 'n' step-through ----------
void runManualEmulatorOnce() {
#if RUN_EMULATOR_ON_BOOT && RUN_MANUAL_AFTER_SWEEP
  Serial.println(F("Manual step-through: press 'n' + Enter to advance.\n"));
  Serial.println(F("Idx | Mode                 | Code | mmL–mmU  | mid(mm) | rawL–rawU | rawMid"));

  // Build the same edge table used above
  int16_t mmL_t, mmU_t;

  for (uint8_t i = 0; i < NUM_MODES; i++) {
    mmL_t = edgesTmm[i];
    mmU_t = edgesTmm[i + 1];
    float   mmL   = mmL_t / 10.0f;
    float   mmU   = mmU_t / 10.0f;
    float   midMm = (mmL + mmU) * 0.5f;

    int16_t rawA = mmTenthToRaw(mmL_t);
    int16_t rawB = mmTenthToRaw(mmU_t);
    int16_t rawL = (rawA < rawB) ? rawA : rawB;
    int16_t rawU = (rawA < rawB) ? rawB : rawA;
    int16_t rawMid = (rawL + rawU) / 2;

    driveOutputs(modes[i].sMask);

    char code[5];
    code[0]=(modes[i].sMask & 0b0001)?'1':'0';
    code[1]=(modes[i].sMask & 0b0010)?'1':'0';
    code[2]=(modes[i].sMask & 0b0100)?'1':'0';
    code[3]=(modes[i].sMask & 0b1000)?'1':'0';
    code[4]='\0';

    Serial.print(i); Serial.print(F("   | "));
    Serial.print(modes[i].name);
    int pad = 20 - (int)strlen(modes[i].name); while (pad-- > 0) Serial.print(' ');
    Serial.print(F(" | "));
    Serial.print(code);
    Serial.print(F(" | "));
    Serial.print(mmL, 1); Serial.print('-'); Serial.print(mmU, 1);
    Serial.print(F(" | "));
    Serial.print(midMm, 2);
    Serial.print(F("  | "));
    Serial.print(rawL); Serial.print('-'); Serial.print(rawU);
    Serial.print(F(" | "));
    Serial.println(rawMid);

    Serial.println(F("Waiting for 'n'..."));
    while (true) {
      if (Serial.available()) {
        char c = Serial.read();
        if (c == 'n' || c == 'N') break;
      }
    }
    Serial.println(F("→ Next\n"));
  }
  Serial.println(F("=== Manual emulator done ===\n"));
#endif
}

// ================== Setup ==================
void setup() {
  Serial.begin(115200);
  Serial.println(F("Boot: Transfer Case Encoder + Emulators + Debug"));

  // Safe relay init (avoid glitches): write HIGH before OUTPUT
  digitalWrite(S1_PIN, HIGH); pinMode(S1_PIN, OUTPUT);
  digitalWrite(S2_PIN, HIGH); pinMode(S2_PIN, OUTPUT);
  digitalWrite(S3_PIN, HIGH); pinMode(S3_PIN, OUTPUT);
  digitalWrite(S4_PIN, HIGH); pinMode(S4_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  buildModeEdgesAndThresholds();
  emaRaw = readPotSafe();  // prime EMA

#if RUN_EMULATOR_ON_BOOT
  if (RUN_AUTO_SWEEP)         runAutoSweepOnce();
  if (RUN_MANUAL_AFTER_SWEEP) runManualEmulatorOnce();
#endif
}

// ================== Loop (non-blocking) ===================
void loop() {
  // --- Read POT safely, smooth, clamp ---
  int rawSample = readPotSafe();

  if (!potConnected && millis() - potWarnLastMs >= POT_WARN_MS) {
    potWarnLastMs = millis();
    Serial.println(F("[ENC/POT] WARNING: Pot disconnected – fallback = HI MODE"));
  }

  emaRaw += (int16_t)((rawSample - emaRaw) >> EMA_SHIFT);
  int16_t raw = emaRaw;
  if (raw < CAL_MIN_RAW) raw = CAL_MIN_RAW;
  if (raw > CAL_MAX_RAW) raw = CAL_MAX_RAW;

  if (!potConnected) {
    // Safe fallback when A0 floating
    driveOutputs(modes[2].sMask); // Hi Mode
  } else {
    // --- Classify with hysteresis + dwell + no-skip ---
    uint8_t idx = modeFromRaw(raw);

    // Hysteresis against boundary (DESCENDING thresholds)
    if (lastModeIdx >= 0 && idx != (uint8_t)lastModeIdx) {
      if (idx > (uint8_t)lastModeIdx) {
        int16_t boundary = thresholdsRaw[lastModeIdx];
        if (raw > boundary - HYST) idx = lastModeIdx;
      } else {
        int16_t boundary = thresholdsRaw[idx];
        if (raw < boundary + HYST) idx = lastModeIdx;
      }
    }

    // No-skip guard
    idx = clampStep(idx, lastModeIdx);

    // Dwell commit
    if (idx != (uint8_t)lastModeIdx) {
      if (idx != (uint8_t)pendingIdx) {
        pendingIdx = idx; pendingStart = millis();
      } else if (millis() - pendingStart >= MODE_DWELL_MS) {
        lastModeIdx = idx; pendingIdx = -1;
        driveOutputs(modes[idx].sMask);

#if ENC_DEBUG_LEVEL>=1
        // Build actual S1–S4 pin state string (readbacks)
        char s[5];
        s[0] = digitalRead(S1_PIN) ? '1' : '0';
        s[1] = digitalRead(S2_PIN) ? '1' : '0';
        s[2] = digitalRead(S3_PIN) ? '1' : '0';
        s[3] = digitalRead(S4_PIN) ? '1' : '0';
        s[4] = '\0';

        Serial.print(F("[ENC] idx=")); Serial.print(idx);
        Serial.print(F(" name=")); Serial.print(modes[idx].name);
        Serial.print(F(" raw="));  Serial.print(raw);
        Serial.print(F(" | S1–S4=")); Serial.println(s);
#endif
      }
    } else {
      pendingIdx = -1;
      driveOutputs(modes[lastModeIdx < 0 ? 0 : lastModeIdx].sMask);
    }

#if ENC_DEBUG_LEVEL==2
    static unsigned long encLast = 0;
    if (millis() - encLast >= ENC_PERIOD_MS) {
      encLast = millis();
      uint8_t show = (lastModeIdx < 0) ? 0 : lastModeIdx;
      char s[5];
      s[0]=digitalRead(S1_PIN)?'1':'0';
      s[1]=digitalRead(S2_PIN)?'1':'0';
      s[2]=digitalRead(S3_PIN)?'1':'0';
      s[3]=digitalRead(S4_PIN)?'1':'0';
      s[4]='\0';

      Serial.print(F("[ENC2] idx=")); Serial.print(show);
      Serial.print(F(" name=")); Serial.print(modes[show].name);
      Serial.print(F(" raw="));  Serial.print(raw);
      Serial.print(F(" thrU=")); Serial.print(thresholdsRaw[show]);
      Serial.print(F(" | S1–S4=")); Serial.println(s);
    }
#endif
  }

  // --- Onboard LED blink code (non-blocking): (index+1) flashes, 3 s pause ---
  static uint32_t ledTimer=0, ledPauseStart=0;
  static uint8_t  ledBlinkCount=0;
  static bool     ledState=false, inPause=false;
  static int      lastShownIdx=-1;

  int showIdx = (lastModeIdx < 0) ? 0 : lastModeIdx;
  if (showIdx != lastShownIdx){
    ledBlinkCount = showIdx + 1;
    ledState=false; inPause=false; ledTimer=millis();
    digitalWrite(LED_PIN, LOW);
    lastShownIdx = showIdx;
  }
  if (!inPause){
    if (ledBlinkCount > 0){
      if (millis() - ledTimer >= 250){
        ledState = !ledState;
        digitalWrite(LED_PIN, ledState ? HIGH : LOW);
        ledTimer = millis();
        if (!ledState) ledBlinkCount--;
      }
    }else{
      inPause = true; ledPauseStart = millis(); digitalWrite(LED_PIN, LOW);
    }
  }else{
    if (millis() - ledPauseStart >= 3000){
      ledBlinkCount = showIdx + 1; inPause = false;
    }
  }
}