/*
  LA10P Linear Actuator (POT feedback) → ECU mode mapper + Stroke Windowing (FAST, no LED)
  ----------------------------------------------------------------
  • ECU drives actuator; Arduino only reads the potentiometer and asserts S1..S4.
  • Mode/Zone windows updated to custom 3 mm modes, Neutral centered at 25 mm.
  • Includes optional simulation mode and full relay state debug printout.
*/

#define DEBUG           // uncomment to enable verbose serial logging
#define SIMULATION true // set true to enable internal raw sweep test

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

// --- Calibration ---
const float CAL_MIN_RAW = 420.0f; // fully extended
const float CAL_MAX_RAW = 596.0f; // fully retracted
const float FULL_STROKE_MM = 50.0f;

// --- Custom window edges (mm) ---
struct ModeWindow { const char* name; float start_mm; float end_mm; uint8_t sMask; };
ModeWindow windows[] = {
  {"Left Stop",        0.0f,   3.0f,   0b1110},
  {"Left of Hi Mode",  3.0f,   6.0f,   0b1010},
  {"Hi Mode",          6.0f,   9.0f,   0b0010},
  {"Right of Hi Mode", 9.0f,  12.0f,   0b0000},
  {"Zone 1",          12.0f,  23.5f,   0b1000},
  {"Neutral Mode",    23.5f,  26.5f,   0b1001},
  {"Zone 2",          26.5f,  44.0f,   0b0001},
  {"Lo Mode",         44.0f,  47.0f,   0b0101},
  {"Right Stop",      47.0f,  50.0f,   0b0100}
};
const uint8_t NUM_MODES = sizeof(windows) / sizeof(windows[0]);

// --- Sampling/filtering ---
float emaRaw = 0.0f;
const float EMA_ALPHA = 0.45f;
const int HYST = 4;

// --- Helpers ---
static inline float clampf(float x, float a, float b) { return x < a ? a : (x > b ? b : x); }

float mmFromRaw(int raw) {
  float mm = FULL_STROKE_MM * (raw - CAL_MIN_RAW) / (CAL_MAX_RAW - CAL_MIN_RAW);
  return clampf(mm, 0.0f, FULL_STROKE_MM);
}

int readRawFast() {
#if SIMULATION
  static int rawSim = CAL_MAX_RAW; // start fully retracted
  static int dir = -1;             // move toward extended
  rawSim += dir;
  if (rawSim <= CAL_MIN_RAW) { rawSim = CAL_MIN_RAW; dir = 1; }
  if (rawSim >= CAL_MAX_RAW) { rawSim = CAL_MAX_RAW; dir = -1; }
  delay(20); // simulate slow sweep (~10 s full travel)
  return rawSim;
#else
  int v = analogRead(POT_PIN);
  emaRaw = EMA_ALPHA * v + (1.0f - EMA_ALPHA) * emaRaw;
  return (int)(emaRaw + 0.5f);
#endif
}

uint8_t findWindowIndex(float mm) {
  for (uint8_t i = 0; i < NUM_MODES; i++) {
    if (mm >= windows[i].start_mm && mm < windows[i].end_mm) return i;
  }
  return NUM_MODES - 1;
}

void driveOutputs(uint8_t mask) {
  digitalWrite(S1_PIN, (mask & 0b0001) ? HIGH : LOW);
  digitalWrite(S2_PIN, (mask & 0b0010) ? HIGH : LOW);
  digitalWrite(S3_PIN, (mask & 0b0100) ? HIGH : LOW);
  digitalWrite(S4_PIN, (mask & 0b1000) ? HIGH : LOW);
}

void printRelayStates() {
  Serial.print(F(" | S1:")); Serial.print(digitalRead(S1_PIN));
  Serial.print(F(" S2:")); Serial.print(digitalRead(S2_PIN));
  Serial.print(F(" S3:")); Serial.print(digitalRead(S3_PIN));
  Serial.print(F(" S4:")); Serial.print(digitalRead(S4_PIN));
}

// --- State ---
int lastIdx = -1;

void setup() {
#ifdef DEBUG
  Serial.begin(115200);
  Serial.println(F("LA10P POT → ECU mode mapper (custom spacing + simulation + relay state debug)"));
  Serial.print(F("Calibration raw min/max: ")); Serial.print(CAL_MIN_RAW); Serial.print(F(" / ")); Serial.println(CAL_MAX_RAW);
  if (SIMULATION) Serial.println(F("Running in SIMULATION mode (no analogRead)."));
#endif

  pinMode(S1_PIN, OUTPUT);
  pinMode(S2_PIN, OUTPUT);
  pinMode(S3_PIN, OUTPUT);
  pinMode(S4_PIN, OUTPUT);

  emaRaw = analogRead(POT_PIN);

  digitalWrite(S1_PIN, HIGH);
  digitalWrite(S2_PIN, HIGH);
  digitalWrite(S3_PIN, HIGH);
  digitalWrite(S4_PIN, HIGH);

#ifdef DEBUG
  Serial.println(F("Mode/Zone windows (mm):"));
  for (uint8_t i = 0; i < NUM_MODES; i++) {
    Serial.print(i + 1); Serial.print(F(": "));
    Serial.print(windows[i].name);
    Serial.print(F(" = ")); Serial.print(windows[i].start_mm, 1);
    Serial.print(F("–")); Serial.println(windows[i].end_mm, 1);
  }
#endif
}

void loop() {
  int raw = readRawFast();
  float mm = mmFromRaw(raw);
  uint8_t idx = findWindowIndex(mm);

  if (idx != lastIdx) {
#ifdef DEBUG
    Serial.print(F("raw:")); Serial.print(raw);
    Serial.print(F(" | mm:")); Serial.print(mm, 1);
    Serial.print(F(" | Mode:")); Serial.print(windows[idx].name);
    Serial.print(F(" | Mask:")); Serial.print(windows[idx].sMask, BIN);
    printRelayStates();
    Serial.println();
#endif
    lastIdx = idx;
  }

  driveOutputs(windows[idx].sMask);

#ifdef DEBUG
  static uint32_t lastPrint = 0;
  if (millis() - lastPrint > 200) {
    lastPrint = millis();
    Serial.print(F("raw:")); Serial.print(raw);
    Serial.print(F(" | mm:")); Serial.print(mm, 1);
    Serial.print(F(" | Mode:")); Serial.print(windows[idx].name);
    Serial.print(F(" | Mask:")); Serial.print(windows[idx].sMask, BIN);
    printRelayStates();
    Serial.println();
  }
#endif
}

/*
Notes:
- SIMULATION=true → sweeps virtual potentiometer (CAL_MAX_RAW ↔ CAL_MIN_RAW).
- Debug shows live S1–S4 pin states (1=HIGH, 0=LOW).
- No functional or timing changes to control logic.
*/