/*
  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.
  • Configurable stroke window and offset in millimeters within the 50 mm travel.
  • Optimized for fast response (EMA filter; no blocking delays; concise DEBUG).
*/

#define DEBUG  // uncomment to enable verbose serial logging

// --- Pins ---
const uint8_t POT_PIN = A0;     // Blue wire (wiper) from LA10P potentiometer
const uint8_t S1_PIN  = 2;
const uint8_t S2_PIN  = 3;
const uint8_t S3_PIN  = 4;
const uint8_t S4_PIN  = 5;

// --- POT wiring (confirmed) ---
// White  -> +5 V reference
// Yellow -> GND (common with ECU & Arduino)
// Blue   -> Wiper → A0

// --- Calibration: raw ADC at physical ends (from your measurements) ---
const float CAL_MIN_RAW = 420.0f; // raw at 0 mm (fully extended)
const float CAL_MAX_RAW = 596.0f; // raw at 50 mm (fully retracted)
//const float CAL_MIN_RAW = 0.0f; // raw at 0 mm (fully extended)
//const float CAL_MAX_RAW = 1023.0f; // raw at 50 mm (fully retracted)
const float FULL_STROKE_MM = 50.0f;

// --- Desired window (configure these) ---
// Example: 30 mm window starting at 10 mm → maps 10..40 mm to modes 1..9.
float desired_start_mm  = 0.0f;  // offset from fully extended
float desired_length_mm = 50.0f; // window length

// --- Sampling / filtering (fast, non-blocking) ---
float emaRaw = 0.0f;           // EMA state
const float EMA_ALPHA = 0.45f; // 0..1; higher = faster response, less smoothing

// --- Hysteresis ---
const int HYST = 4; // ADC counts (~0.4% FS). Tune for your noise level.

// --- Mode table (mask semantics: 1=open/HIGH, 0=closed/LOW) ---
struct Mode { uint8_t sMask; const char* name; };
Mode modes[] = {
  {0b1110, "Left Stop"},
  {0b1010, "Left of Hi Mode"},
  {0b0010, "Hi Mode"},
  {0b0000, "Right of Hi Mode"},
  {0b1000, "Zone 1"},
  {0b1001, "Neutral Mode"},
  {0b0001, "Zone 2"},
  {0b0101, "Lo Mode"},
  {0b0100, "Right Stop"}
};
const uint8_t NUM_MODES = sizeof(modes)/sizeof(modes[0]);

// Computed thresholds (raw ADC) delimiting each mode within the window
int thresholdsRaw[9]; // NUM_MODES entries; each is the MAX reading for that mode

// --- State ---
int lastModeIdx = -1;

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

// Map millimeters (0..50) → raw ADC using linear calibration
int mmToRaw(float mm) {
  mm = clampf(mm, 0.0f, FULL_STROKE_MM);
  float raw = CAL_MIN_RAW + (mm / FULL_STROKE_MM) * (CAL_MAX_RAW - CAL_MIN_RAW);
  return (int)(raw + 0.5f);
}

// Compute thresholdsRaw[] from desired window; by default even segmentation
void computeThresholds() {
  float start  = clampf(desired_start_mm,  0.0f, FULL_STROKE_MM);
  float length = clampf(desired_length_mm, 1.0f, FULL_STROKE_MM);
  if (start + length > FULL_STROKE_MM) length = FULL_STROKE_MM - start;

  for (uint8_t i = 0; i < NUM_MODES; i++) {
    float edge_mm = start + ((float)(i + 1) / (float)NUM_MODES) * length; // upper edge of bin i
    thresholdsRaw[i] = mmToRaw(edge_mm);
  }
  for (uint8_t i = 1; i < NUM_MODES; i++) {
    if (thresholdsRaw[i] <= thresholdsRaw[i - 1]) thresholdsRaw[i] = thresholdsRaw[i - 1] + 1;
  }
}

// Fast read + EMA (non-blocking)
int readRawFast() {
  int v = analogRead(POT_PIN);          // ~110 µs @ default ADC prescaler
  emaRaw = (EMA_ALPHA * v) + (1.0f - EMA_ALPHA) * emaRaw;
  return (int)(emaRaw + 0.5f);
}

uint8_t findModeIndex_fromRaw(int reading) {
  if (reading <= thresholdsRaw[0]) return 0;
  for (uint8_t i = 1; i < NUM_MODES; i++) {
    if (reading <= thresholdsRaw[i]) 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 setup() {
#ifdef DEBUG
  Serial.begin(115200);
  Serial.println(F("LA10P POT → ECU mode mapper (FAST, no LED)"));
  Serial.print(F("Calibration raw min/max: ")); Serial.print(CAL_MIN_RAW); Serial.print(F(" / ")); Serial.println(CAL_MAX_RAW);
  Serial.print(F("Window mm (start,len): ")); Serial.print(desired_start_mm); Serial.print(F(", ")); Serial.println(desired_length_mm);
#endif

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

  // Prime EMA with a first reading to avoid a long settling time
  emaRaw = analogRead(POT_PIN);

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

  computeThresholds();

#ifdef DEBUG
  Serial.println(F("Thresholds (raw ADC) per mode:"));
  for (uint8_t i = 0; i < NUM_MODES; i++) {
    Serial.print(i + 1); Serial.print(F(": ")); Serial.println(thresholdsRaw[i]);
  }
#endif
}

void loop() {
  int raw = readRawFast();

  uint8_t idx = findModeIndex_fromRaw(raw);
  if (lastModeIdx != (int)idx && lastModeIdx >= 0) {
    // Hysteresis relative to the adjacent boundary
    if (idx > lastModeIdx) {
      int boundary = thresholdsRaw[lastModeIdx];
      if (raw < boundary + HYST) idx = lastModeIdx;
    } else {
      int boundary = thresholdsRaw[idx];
      if (raw > boundary - HYST) idx = lastModeIdx;
    }
  }

  if (idx != lastModeIdx) {
#ifdef DEBUG
    // Immediate print on mode change
    Serial.print(F("raw:")); Serial.print(raw);
    Serial.print(F(" | mm:"));
    float mm = FULL_STROKE_MM * (raw - CAL_MIN_RAW) / (CAL_MAX_RAW - CAL_MIN_RAW);
    Serial.print(mm, 1);
    Serial.print(F(" | Mode:")); Serial.print(modes[idx].name);
    Serial.print(F(" | Mask:")); Serial.println(modes[idx].sMask, BIN);
#endif
  }

  lastModeIdx = idx;
  driveOutputs(modes[idx].sMask);

#ifdef DEBUG
  // Periodic print (every ~200 ms) even without mode change
  static uint32_t lastPrint = 0;
  if (millis() - lastPrint > 200) {
    lastPrint = millis();
    Serial.print(F("raw:")); Serial.print(raw);
    Serial.print(F(" | mm:"));
    float mm = FULL_STROKE_MM * (raw - CAL_MIN_RAW) / (CAL_MAX_RAW - CAL_MIN_RAW);
    Serial.print(mm, 1);
    Serial.print(F(" | Mode:")); Serial.print(modes[idx].name);
    Serial.print(F(" | Mask:")); Serial.println(modes[idx].sMask, BIN);
  }
#endif
}

/*
Notes:
- For even faster response, you can raise EMA_ALPHA (e.g., 0.6) and/or reduce HYST.
- Disabling DEBUG removes serial overhead and yields the tightest loop time.
- If needed, we can switch to ADC free-running + ISR for >10 kSamples/s.
*/