/*
  ESP32 Komfort- og styringsboks – Mercedes-Benz C126 (v1, robust)
  - Power-cut: ESP mister strøm når ESP_HOLD slippes (ingen deep sleep)
  - 3x MCP23017:
    IN  (0x20): inputs (K1..K4, LIGHTSW, WIPER_RAW, BLINK_H/V, IGN sense på GPB0)
    OUT (0x21): misc outputs + VF/HF motor-coils (pins 12-15)
    MOT (0x22): VB/HB/FLAP1/FLAP2 motor-coils (pins 0-7) + INSTR_ENABLE på pin 8
  - MCP3208: CH0-3 ruder, CH4-5 spjæld, CH6 light, CH7 rain
  - I2C: 100kHz + timeout + bus recovery
  - DRV_EN (GPIO25): hardware coil-supply enable (BOOT-sikring: LOW under boot, HIGH når alt er sikkert)
  - PowerHold aktivitet markeres på reelle output-ændringer (outputsApply)
  - K4 latches i state (RemoteSM), ikke i outApplied/outWanted
  - K1 LOCK: stop hvis motorer kører, ellers (IGN OFF) DEFA lock + komfortluk (vinduer->spjæld) + sluk gulv/under
  - Visker: kræver IGN ON + WIPER_RAW + RAIN >= threshold
  - Ingen delay() i drift (kun delayMicroseconds i I2C recovery)

  ÆNDRINGER (2026-02-12):
  - IGN OFF (faldende flank) slukker gulv + underglow automatisk.
  - Underglow kan stadig tændes manuelt efter IGN OFF via fjernbetjening (K4 hold).
  - FEJL-LED (GPIO17) tilføjet: HIGH i fatalHalt().

  LED-OVERSIGT (hardware, til dokumentation):
  - PWR_12V   (Grøn): 12V_IN efter intern sikring -> (2k2–3k3) -> LED -> GND (viser 12V i boksen)
  - LOGIC_3V3 (Blå): 3V3 rail nær ESP32 -> (680–1k) -> LED -> GND (viser 3.3V rail/ESP oppe)
  - HOLD_RUN  (Gul): ESP_HOLD_REAL efter transistor/opto/OR (ikke direkte på GPIO) -> (1k–2k2) -> LED -> GND (viser boksen holder sig vågen)
  - FAULT     (Rød): GPIO17 -> (1k) -> LED -> GND (tændes kun ved fatalHalt, latched)
*/

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_MCP23X17.h>
#include <math.h>
#include <string.h>  // memset

// ----------------------------
// Tidskonstanter
// ----------------------------
static constexpr uint32_t POLL_INPUT_MS       = 20;
static constexpr uint32_t DEBOUNCE_MS         = 50;
static constexpr uint32_t HOLD_MS             = 500;
static constexpr uint32_t IGNORE_K1K2_MS      = 2000;
static constexpr uint32_t DEFA_PULSE_MS       = 300;

static constexpr uint32_t MOTOR_MAX_MS        = 15000;
static constexpr uint32_t MOTOR_REV_DEAD_MS   = 50;
static constexpr uint32_t MOTOR_SOFTIGNORE_MS = 300;

static constexpr uint32_t ADC_POLL_MS         = 50;
static constexpr uint8_t  ADC_AVG_N           = 5;

static constexpr uint32_t POWER_CUT_MS        = 5UL * 60UL * 1000UL; // 5 min

static constexpr uint32_t BLINK_LOCK_ON_MS    = 90;
static constexpr uint32_t BLINK_UL_ON1_MS     = 30;
static constexpr uint32_t BLINK_UL_OFF_MS     = 30;
static constexpr uint32_t BLINK_UL_ON2_MS     = 30;

static constexpr uint32_t WIPER_PULSE_MS      = 150;
static constexpr uint32_t WIPER_INTERVAL_MS   = 2000; // TOTAL cyklus: 150ms ON + 1850ms WAIT

static constexpr uint32_t INSTR_PWM_FREQ_HZ   = 20000;
static constexpr float    INSTR_PWM_MIN_DUTY  = 0.30f;
static constexpr uint32_t INSTR_UPDATE_MS     = 100;
static constexpr float    INSTR_HYST          = 0.03f;

// ----------------------------
// ESP32 pins (strap-sikre)
// ----------------------------
static constexpr int PIN_I2C_SDA      = 21;
static constexpr int PIN_I2C_SCL      = 22;

static constexpr int PIN_SPI_SCK      = 18;
static constexpr int PIN_SPI_MISO     = 19;
static constexpr int PIN_SPI_MOSI     = 23;
static constexpr int PIN_MCP3208_CS   = 16;  // ikke strap

static constexpr int PIN_ESP_HOLD     = 26;  // holder komfortrelæ OR-siden (via transistor/opto)
static constexpr int PIN_INSTR_PWM    = 27;  // PWM til MOSFET (instr)
static constexpr int PIN_WAKE_RTC     = 33;  // WAKE_ALL (doc)

// Driver-enable signal (skal realiseres i hardware som coil-supply enable)
static constexpr int PIN_DRV_EN       = 25;  // HIGH = coil-supply enabled

// Control / fault LED
static constexpr int PIN_LED_FAULT    = 17;  // HIGH i fatalHalt (latched mens ESP hænger)

// ----------------------------
// MCP adresser
// ----------------------------
static constexpr uint8_t MCP_IN_ADDR  = 0x20; // MCP23017_1
static constexpr uint8_t MCP_OUT_ADDR = 0x21; // MCP23017_2
static constexpr uint8_t MCP_MOT_ADDR = 0x22; // MCP23017_3

Adafruit_MCP23X17 mcpIn;
Adafruit_MCP23X17 mcpOut;
Adafruit_MCP23X17 mcpMot;

// ----------------------------
// Helpers
// ----------------------------
static inline float clampf(float v, float lo, float hi) { return (v < lo) ? lo : (v > hi) ? hi : v; }

// ----------------------------
// I2C recovery + init robust
// ----------------------------
static bool i2cRecoverBus(int sdaPin, int sclPin) {
  pinMode(sdaPin, INPUT_PULLUP);
  pinMode(sclPin, INPUT_PULLUP);

  if (digitalRead(sdaPin) == HIGH && digitalRead(sclPin) == HIGH) return true;

  pinMode(sclPin, OUTPUT_OPEN_DRAIN);
  digitalWrite(sclPin, HIGH);

  for (int i = 0; i < 9; i++) {
    digitalWrite(sclPin, LOW);
    delayMicroseconds(5);
    digitalWrite(sclPin, HIGH);
    delayMicroseconds(5);
  }

  // STOP-condition
  pinMode(sdaPin, OUTPUT_OPEN_DRAIN);
  digitalWrite(sdaPin, LOW);
  delayMicroseconds(5);
  digitalWrite(sclPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(sdaPin, HIGH);
  delayMicroseconds(5);

  pinMode(sdaPin, INPUT_PULLUP);
  pinMode(sclPin, INPUT_PULLUP);

  return (digitalRead(sdaPin) == HIGH && digitalRead(sclPin) == HIGH);
}

static void i2cInitRobust(int sdaPin, int sclPin) {
  i2cRecoverBus(sdaPin, sclPin);
  Wire.begin(sdaPin, sclPin);
  Wire.setClock(100000);
  Wire.setTimeOut(50);
}

// ----------------------------
// Global aktivitet / power hold
// ----------------------------
static uint32_t g_lastActivity = 0;
static inline void markActivity(uint32_t now) { g_lastActivity = now; }

// ----------------------------
// Output-lag (LOGISK)
// ----------------------------
enum Out : uint8_t {
  OUT_LOW_BEAM_INJ = 0,
  OUT_POS_L_INJ,
  OUT_POS_R_INJ,
  OUT_TAIL_L_INJ,
  OUT_TAIL_R_INJ,
  OUT_FLOOR_LED,
  OUT_UNDERGLOW,
  OUT_BLINK_H_INJ,
  OUT_BLINK_V_INJ,
  OUT_DEFA_LOCK,
  OUT_DEFA_UNLOCK,
  OUT_INSTR_ENABLE,   // på mcpMot pin 8
  OUT_WIPER_LOW_SEL,  // ejerskab: KUN WiperSM i v1
  OUT_COUNT
};

// MCP_OUT pin mapping (0x21): pins 0..11 til “misc”
enum OutPin : uint8_t {
  P_LOW_BEAM_INJ = 0,
  P_POS_L_INJ    = 1,
  P_POS_R_INJ    = 2,
  P_TAIL_L_INJ   = 3,
  P_TAIL_R_INJ   = 4,
  P_FLOOR_LED    = 5,
  P_UNDERGLOW    = 6,
  P_BLINK_H_INJ  = 7,
  P_BLINK_V_INJ  = 8,
  P_DEFA_LOCK    = 9,
  P_DEFA_UNLOCK  = 10,
  P_WIPER_SEL    = 11,

  // mcpOut 12..15 er reserveret til VF/HF coils:
  P_MTR_VF_FWD   = 12,
  P_MTR_VF_REV   = 13,
  P_MTR_HF_FWD   = 14,
  P_MTR_HF_REV   = 15
};

// MCP_MOT pin mapping (0x22): VB/HB/FLAP1/FLAP2 coils + INSTR_ENABLE
enum MotPin : uint8_t {
  P_MTR_VB_FWD   = 0,
  P_MTR_VB_REV   = 1,
  P_MTR_HB_FWD   = 2,
  P_MTR_HB_REV   = 3,
  P_MTR_F1_FWD   = 4,
  P_MTR_F1_REV   = 5,
  P_MTR_F2_FWD   = 6,
  P_MTR_F2_REV   = 7,
  P_INSTR_EN     = 8
};

struct OutputsWanted {
  bool  level[OUT_COUNT];
  float instrDuty;
};
struct OutputsApplied {
  bool  level[OUT_COUNT];
  float instrDuty;
};

static OutputsWanted outWanted;
static OutputsApplied outApplied;

static void outputsDefault() {
  memset(outWanted.level, 0, sizeof(outWanted.level));
  outWanted.instrDuty = 1.0f;
}

static inline void mcpWrite(Adafruit_MCP23X17 &m, uint8_t pin, bool on) {
  m.digitalWrite(pin, on ? HIGH : LOW);
}

static void writeOutLogical(Out o, bool on) {
  switch (o) {
    case OUT_LOW_BEAM_INJ:  mcpWrite(mcpOut, P_LOW_BEAM_INJ, on); break;
    case OUT_POS_L_INJ:     mcpWrite(mcpOut, P_POS_L_INJ, on);    break;
    case OUT_POS_R_INJ:     mcpWrite(mcpOut, P_POS_R_INJ, on);    break;
    case OUT_TAIL_L_INJ:    mcpWrite(mcpOut, P_TAIL_L_INJ, on);   break;
    case OUT_TAIL_R_INJ:    mcpWrite(mcpOut, P_TAIL_R_INJ, on);   break;
    case OUT_FLOOR_LED:     mcpWrite(mcpOut, P_FLOOR_LED, on);    break;
    case OUT_UNDERGLOW:     mcpWrite(mcpOut, P_UNDERGLOW, on);    break;
    case OUT_BLINK_H_INJ:   mcpWrite(mcpOut, P_BLINK_H_INJ, on);  break;
    case OUT_BLINK_V_INJ:   mcpWrite(mcpOut, P_BLINK_V_INJ, on);  break;
    case OUT_DEFA_LOCK:     mcpWrite(mcpOut, P_DEFA_LOCK, on);    break;
    case OUT_DEFA_UNLOCK:   mcpWrite(mcpOut, P_DEFA_UNLOCK, on);  break;
    case OUT_WIPER_LOW_SEL: mcpWrite(mcpOut, P_WIPER_SEL, on);    break;
    case OUT_INSTR_ENABLE:  mcpWrite(mcpMot, P_INSTR_EN, on);     break;
    default: break;
  }
}

static void writeInstrPwm(float duty) {
  duty = clampf(duty, 0.0f, 1.0f);
  uint32_t val = (uint32_t)lroundf(duty * 255.0f);
  ledcWrite(0, val);
}

static void outputsApply(uint32_t now) {
  for (int i = 0; i < OUT_COUNT; i++) {
    if (outWanted.level[i] != outApplied.level[i]) {
      writeOutLogical((Out)i, outWanted.level[i]);
      outApplied.level[i] = outWanted.level[i];
      markActivity(now);
    }
  }
  if (fabsf(outWanted.instrDuty - outApplied.instrDuty) > 0.0001f) {
    writeInstrPwm(outWanted.instrDuty);
    outApplied.instrDuty = outWanted.instrDuty;
    markActivity(now);
  }
}

static void setAllOutputsSafe() {
  for (uint8_t p = 0; p < 16; p++) mcpWrite(mcpOut, p, false);
  for (uint8_t p = 0; p < 16; p++) mcpWrite(mcpMot, p, false);

  memset(outApplied.level, 0, sizeof(outApplied.level));
  outApplied.instrDuty = 1.0f;
  memset(outWanted.level, 0, sizeof(outWanted.level));
  outWanted.instrDuty = 1.0f;

  writeInstrPwm(1.0f);
}

// ----------------------------
// Buttons (debounce + short/hold)
// ----------------------------
struct Btn {
  bool raw = false;
  bool stable = false;
  bool lastStable = false;

  bool pressedEvent = false;
  bool releasedEvent = false;
  bool shortEvent = false;
  bool holdEvent = false;
  bool holding = false;

  uint32_t tRawChange = 0;
  uint32_t tPress = 0;

  void update(bool newRaw, uint32_t now) {
    pressedEvent = releasedEvent = shortEvent = holdEvent = false;

    if (newRaw != raw) {
      raw = newRaw;
      tRawChange = now;
    }

    if ((now - tRawChange) >= DEBOUNCE_MS && stable != raw) {
      lastStable = stable;
      stable = raw;

      pressedEvent  = (!lastStable && stable);
      releasedEvent = ( lastStable && !stable);

      if (pressedEvent) {
        tPress = now;
        holding = false;
      }
      if (releasedEvent) {
        uint32_t dt = now - tPress;
        if (!holding && dt < HOLD_MS) shortEvent = true;
        holding = false;
      }
    }

    if (stable && !holding && (now - tPress) >= HOLD_MS) {
      holding = true;
      holdEvent = true;
    }
  }
};

struct DebouncedLevel {
  bool raw=false, stable=false;
  uint32_t tRawChange=0;

  void update(bool newRaw, uint32_t now) {
    if (newRaw != raw) { raw=newRaw; tRawChange=now; }
    if ((now - tRawChange) >= DEBOUNCE_MS) stable = raw;
  }
};

// ----------------------------
// Inputs på MCP_IN (0x20), active HIGH
// ----------------------------
static inline bool mcpInReadA(uint8_t pin) { return mcpIn.digitalRead(pin) == HIGH; }
static inline bool mcpInReadB(uint8_t pin) { return mcpIn.digitalRead(8 + pin) == HIGH; }

// ----------------------------
// MCP3208 ADC
// ----------------------------
static SPISettings adcSpi(1000000, MSBFIRST, SPI_MODE0);

static uint16_t mcp3208_read_raw(uint8_t ch) {
  ch &= 0x07;
  uint8_t b0 = 0x06 | (ch >> 2);
  uint8_t b1 = (uint8_t)((ch & 0x03) << 6);

  digitalWrite(PIN_MCP3208_CS, HIGH);
  digitalWrite(PIN_MCP3208_CS, LOW);

  (void)SPI.transfer(b0);
  uint8_t r1 = SPI.transfer(b1);
  uint8_t r2 = SPI.transfer(0x00);

  digitalWrite(PIN_MCP3208_CS, HIGH);
  return (uint16_t)(((r1 & 0x0F) << 8) | r2);
}

enum AdcCh : uint8_t {
  ADC_WIN_VF = 0,
  ADC_WIN_HF = 1,
  ADC_WIN_VB = 2,
  ADC_WIN_HB = 3,
  ADC_FLAP1  = 4,
  ADC_FLAP2  = 5,
  ADC_LIGHT  = 6,
  ADC_RAIN   = 7
};

struct AdcAvg {
  uint16_t buf[ADC_AVG_N]{};
  uint8_t  idx = 0;
  bool     filled = false;

  void push(uint16_t v) {
    buf[idx++] = v;
    if (idx >= ADC_AVG_N) { idx = 0; filled = true; }
  }

  uint16_t mean() const {
    uint32_t sum = 0;
    uint8_t n = filled ? ADC_AVG_N : idx;
    if (n == 0) return 0;
    for (uint8_t i = 0; i < n; i++) sum += buf[i];
    return (uint16_t)(sum / n);
  }
};

static AdcAvg adcAvg[8];

// ----------------------------
// Motorstyring (2 coils pr motor)
// STOP = begge OFF (friløb), aldrig begge ON
// ----------------------------
enum MotorId : uint8_t {
  MTR_WIN_VF = 0,
  MTR_WIN_HF,
  MTR_WIN_VB,
  MTR_WIN_HB,
  MTR_FLAP1,
  MTR_FLAP2,
  MTR_COUNT
};

enum Dir : uint8_t { DIR_STOP=0, DIR_FWD=1, DIR_REV=2 };
enum CoilBus : uint8_t { BUS_OUT, BUS_MOT };

struct CoilPin {
  CoilBus bus;
  uint8_t pin;
};

static inline void writeCoil(const CoilPin &c, bool on) {
  if (c.bus == BUS_OUT) mcpWrite(mcpOut, c.pin, on);
  else                  mcpWrite(mcpMot, c.pin, on);
}

struct Motor {
  CoilPin fwd;
  CoilPin rev;

  Dir want = DIR_STOP;
  Dir applied = DIR_STOP;
  Dir pending = DIR_STOP;

  uint32_t tStart = 0;
  uint32_t tLastAppliedChange = 0;
  uint32_t tIgnoreUntil = 0;

  bool running() const { return applied != DIR_STOP; }

  void init(CoilPin f, CoilPin r) { fwd = f; rev = r; }

  void command(Dir d, uint32_t now) { want = d; markActivity(now); }
  void stop(uint32_t now) { want = DIR_STOP; pending = DIR_STOP; markActivity(now); }

  void apply(Dir d) {
    // interlock: aldrig begge ON
    writeCoil(fwd, d == DIR_FWD);
    writeCoil(rev, d == DIR_REV);
  }

  void tick(uint32_t now) {
    if (want == DIR_STOP) {
      if (applied != DIR_STOP) {
        apply(DIR_STOP);
        applied = DIR_STOP;
        tLastAppliedChange = now;
      }
      pending = DIR_STOP;
      return;
    }

    if (applied == DIR_STOP && pending == DIR_STOP) {
      apply(want);
      applied = want;
      tLastAppliedChange = now;
      tStart = now;
      tIgnoreUntil = now + MOTOR_SOFTIGNORE_MS;
      return;
    }

    if (applied == want) return;

    if (applied != DIR_STOP && applied != want) {
      apply(DIR_STOP);
      applied = DIR_STOP;
      tLastAppliedChange = now;
      pending = want;
      return;
    }

    if (applied == DIR_STOP && pending != DIR_STOP) {
      if ((now - tLastAppliedChange) >= MOTOR_REV_DEAD_MS) {
        apply(pending);
        applied = pending;
        pending = DIR_STOP;
        tLastAppliedChange = now;
        tStart = now;
        tIgnoreUntil = now + MOTOR_SOFTIGNORE_MS;
      }
    }
  }
};

static Motor motor[MTR_COUNT];

// ============================
// INDIVIDUELLE OVERSTRØM-THRESHOLDS (KALIBRERES PÅ BILEN)
// ============================
// NOTE: Rå ADC-counts (0..4095) fra MCP3208.
static uint16_t TH_WIN_VF_RAW = 4090;
static uint16_t TH_WIN_HF_RAW = 4090;
static uint16_t TH_WIN_VB_RAW = 4090;
static uint16_t TH_WIN_HB_RAW = 4090;

static uint16_t TH_FLAP1_RAW  = 4090;
static uint16_t TH_FLAP2_RAW  = 4090;

static bool motorOverCurrent(MotorId id, uint32_t now) {
  if (!motor[id].running()) return false;
  if (now < motor[id].tIgnoreUntil) return false;

  switch (id) {
    case MTR_WIN_VF: return adcAvg[ADC_WIN_VF].mean() >= TH_WIN_VF_RAW;
    case MTR_WIN_HF: return adcAvg[ADC_WIN_HF].mean() >= TH_WIN_HF_RAW;
    case MTR_WIN_VB: return adcAvg[ADC_WIN_VB].mean() >= TH_WIN_VB_RAW;
    case MTR_WIN_HB: return adcAvg[ADC_WIN_HB].mean() >= TH_WIN_HB_RAW;
    case MTR_FLAP1:  return adcAvg[ADC_FLAP1 ].mean() >= TH_FLAP1_RAW;
    case MTR_FLAP2:  return adcAvg[ADC_FLAP2 ].mean() >= TH_FLAP2_RAW;
    default:         return false;
  }
}

// ----------------------------
// Puls-hjælper (DEFA pulses)
// ----------------------------
struct OneShotPulse {
  bool active = false;
  uint32_t tEnd = 0;
  Out out;

  void start(Out o, uint32_t now, uint32_t widthMs) {
    out = o;
    active = true;
    tEnd = now + widthMs;
    markActivity(now);
  }

  void tick(uint32_t now) {
    if (!active) return;
    outWanted.level[out] = true;
    if (now >= tEnd) {
      outWanted.level[out] = false;
      active = false;
    }
  }
};

static OneShotPulse defaLockPulse;
static OneShotPulse defaUnlockPulse;

// ----------------------------
// BlinkSM (ejer OUT_BLINK_*)
// ----------------------------
struct BlinkSM {
  enum Mode : uint8_t { BL_NONE, BL_LOCK, BL_UNLOCK };
  Mode mode = BL_NONE;
  uint32_t tNext = 0;
  uint8_t step = 0;

  void startLock(uint32_t now)  { mode = BL_LOCK;   step = 0; tNext = now; markActivity(now); }
  void startUnlock(uint32_t now){ mode = BL_UNLOCK; step = 0; tNext = now; markActivity(now); }

  void tick(uint32_t now) {
    if (mode == BL_NONE) return;

    if (mode == BL_LOCK) {
      if (step == 0 && now >= tNext) {
        outWanted.level[OUT_BLINK_H_INJ] = true;
        outWanted.level[OUT_BLINK_V_INJ] = true;
        tNext = now + BLINK_LOCK_ON_MS;
        step = 1;
      } else if (step == 1 && now >= tNext) {
        outWanted.level[OUT_BLINK_H_INJ] = false;
        outWanted.level[OUT_BLINK_V_INJ] = false;
        mode = BL_NONE;
      }
      return;
    }

    if (step == 0 && now >= tNext) {
      outWanted.level[OUT_BLINK_H_INJ] = true;
      outWanted.level[OUT_BLINK_V_INJ] = true;
      tNext = now + BLINK_UL_ON1_MS;
      step = 1;
    } else if (step == 1 && now >= tNext) {
      outWanted.level[OUT_BLINK_H_INJ] = false;
      outWanted.level[OUT_BLINK_V_INJ] = false;
      tNext = now + BLINK_UL_OFF_MS;
      step = 2;
    } else if (step == 2 && now >= tNext) {
      outWanted.level[OUT_BLINK_H_INJ] = true;
      outWanted.level[OUT_BLINK_V_INJ] = true;
      tNext = now + BLINK_UL_ON2_MS;
      step = 3;
    } else if (step == 3 && now >= tNext) {
      outWanted.level[OUT_BLINK_H_INJ] = false;
      outWanted.level[OUT_BLINK_V_INJ] = false;
      mode = BL_NONE;
    }
  }
};

static BlinkSM blinkSM;

// ----------------------------
// WiperSM (ejer OUT_WIPER_LOW_SEL)
// Krav: IGN ON + WIPER_RAW + regn over tærskel
// ----------------------------
struct WiperSM {
  enum Phase : uint8_t { PH_IDLE, PH_PULSE_ON, PH_WAIT };
  Phase phase = PH_IDLE;
  uint32_t tPhaseEnd = 0;

  uint16_t rainTh = 1800; // placeholder (kalibrér)
  bool active = false;

  void tick(uint32_t now, bool ignOn, bool wiperRawStable, uint16_t rainAdc) {
    if (!ignOn || !wiperRawStable || rainAdc < rainTh) {
      active = false;
      phase = PH_IDLE;
      outWanted.level[OUT_WIPER_LOW_SEL] = false;
      return;
    }

    active = true;

    switch (phase) {
      case PH_IDLE:
        outWanted.level[OUT_WIPER_LOW_SEL] = true;
        phase = PH_PULSE_ON;
        tPhaseEnd = now + WIPER_PULSE_MS;
        markActivity(now);
        break;

      case PH_PULSE_ON:
        outWanted.level[OUT_WIPER_LOW_SEL] = true;
        if (now >= tPhaseEnd) {
          outWanted.level[OUT_WIPER_LOW_SEL] = false;
          phase = PH_WAIT;
          uint32_t rest = (WIPER_INTERVAL_MS > WIPER_PULSE_MS) ? (WIPER_INTERVAL_MS - WIPER_PULSE_MS) : 1;
          tPhaseEnd = now + rest;
        }
        break;

      case PH_WAIT:
        outWanted.level[OUT_WIPER_LOW_SEL] = false;
        if (now >= tPhaseEnd) phase = PH_IDLE;
        break;
    }
  }
};

static WiperSM wiperSM;

// ----------------------------
// RemoteSM (K1-K4), ejer floor/under latches
// ----------------------------
struct RemoteSM {
  Btn k1, k2, k3, k4;

  bool floorLatched = false;
  bool underLatched = false;

  bool comfortActive = false;
  bool comfortFlapsDone = false;

  uint32_t tIgnoreK1K2Until = 0;

  bool lastIgnOn = false; // bruges til IGN faldende flank

  bool windowsRunning() const {
    return motor[MTR_WIN_VF].running() || motor[MTR_WIN_HF].running() ||
           motor[MTR_WIN_VB].running() || motor[MTR_WIN_HB].running();
  }
  bool flapsRunning() const { return motor[MTR_FLAP1].running() || motor[MTR_FLAP2].running(); }

  void stopAllMotors(uint32_t now) {
    for (int i = 0; i < MTR_COUNT; i++) motor[i].stop(now);
    comfortActive = false;
    comfortFlapsDone = false;
    markActivity(now);
  }

  void startComfortClose(uint32_t now) {
    motor[MTR_WIN_VF].command(DIR_FWD, now);
    motor[MTR_WIN_HF].command(DIR_FWD, now);
    motor[MTR_WIN_VB].command(DIR_FWD, now);
    motor[MTR_WIN_HB].command(DIR_FWD, now);

    comfortActive = true;
    comfortFlapsDone = false;

    // LOCK-sekvens slukker gulv/under
    floorLatched = false;
    underLatched = false;

    markActivity(now);
  }

  void tick(uint32_t now, bool ignOn) {
    // IGN OFF -> sluk alt lys (kun faldende flank)
    if (lastIgnOn && !ignOn) {
      floorLatched = false;
      underLatched = false;
      markActivity(now);
    }
    lastIgnOn = ignOn;

    // K4 latches
    if (k4.shortEvent) { floorLatched = !floorLatched; markActivity(now); }
    if (k4.holdEvent)  { underLatched = !underLatched; markActivity(now); }

    bool allowK1K2 = (now >= tIgnoreK1K2Until);

    // K1 LOCK: short OR hold
    if ((k1.shortEvent || k1.holdEvent) && allowK1K2) {
      if (windowsRunning() || flapsRunning()) {
        stopAllMotors(now);
      } else if (!ignOn) {
        defaLockPulse.start(OUT_DEFA_LOCK, now, DEFA_PULSE_MS);
        blinkSM.startLock(now);
        startComfortClose(now);
        tIgnoreK1K2Until = now + IGNORE_K1K2_MS;
      }
    }

    // K2 UNLOCK: short OR hold
    if ((k2.shortEvent || k2.holdEvent) && allowK1K2) {
      if (windowsRunning() || flapsRunning()) {
        stopAllMotors(now);
      } else {
        defaUnlockPulse.start(OUT_DEFA_UNLOCK, now, DEFA_PULSE_MS);
        blinkSM.startUnlock(now);
        tIgnoreK1K2Until = now + IGNORE_K1K2_MS;
        markActivity(now);
      }
    }

    // K3 spjæld: kort=luk(FWD), hold=åbn(REV)
    if (k3.shortEvent) {
      comfortActive = false;
      motor[MTR_FLAP1].stop(now); motor[MTR_FLAP2].stop(now);
      motor[MTR_FLAP1].command(DIR_FWD, now);
      motor[MTR_FLAP2].command(DIR_FWD, now);
      markActivity(now);
    }
    if (k3.holdEvent) {
      comfortActive = false;
      motor[MTR_FLAP1].stop(now); motor[MTR_FLAP2].stop(now);
      motor[MTR_FLAP1].command(DIR_REV, now);
      motor[MTR_FLAP2].command(DIR_REV, now);
      markActivity(now);
    }

    // Komfortluk: vinduer -> spjæld
    if (comfortActive) {
      if (!windowsRunning() && !comfortFlapsDone) {
        motor[MTR_FLAP1].command(DIR_FWD, now);
        motor[MTR_FLAP2].command(DIR_FWD, now);
        comfortFlapsDone = true;
        markActivity(now);
      }
      if (comfortFlapsDone && !flapsRunning()) {
        comfortActive = false;
      }
    }

    outWanted.level[OUT_FLOOR_LED] = floorLatched;
    outWanted.level[OUT_UNDERGLOW] = underLatched;
  }

  bool lightsLatchedOn() const { return floorLatched || underLatched; }
};

static RemoteSM remoteSM;

// ----------------------------
// InstrSM
// - OUT_INSTR_ENABLE holdes ON (relay/switch til instr feed), PWM duty styrer MOSFET-stel
// - lightswOn: bypass = 100%
// ----------------------------
struct InstrSM {
  uint32_t tLastUpd = 0;
  float avg = 1.0f;
  float duty = 1.0f;

  void tick(uint32_t now, bool lightswOn, uint16_t lightAdc) {
    outWanted.level[OUT_INSTR_ENABLE] = true; // altid enable i v1

    if (lightswOn) {
      outWanted.instrDuty = 1.0f;
      avg = duty = 1.0f;
      return;
    }

    float x = (float)lightAdc / 4095.0f;
    float target = 1.0f - x;
    target = clampf(target, INSTR_PWM_MIN_DUTY, 1.0f);

    if (now - tLastUpd >= INSTR_UPDATE_MS) {
      float alpha = 0.1f;
      avg = avg + alpha * (target - avg);

      if (fabsf(avg - duty) >= INSTR_HYST) duty = avg;
      outWanted.instrDuty = duty;
      tLastUpd = now;
    } else {
      outWanted.instrDuty = duty;
    }
  }
};

static InstrSM instrSM;

// ----------------------------
// PowerHold
// ----------------------------
static void powerHoldTick(uint32_t now, bool ignOn) {
  bool keepAlive = false;

  if (ignOn) keepAlive = true;

  for (int i = 0; i < MTR_COUNT; i++) {
    if (motor[i].running()) { keepAlive = true; break; }
  }

  if (wiperSM.active) keepAlive = true;
  if (remoteSM.lightsLatchedOn()) keepAlive = true; // underglow/floor holder altid systemet vågent

  if ((now - g_lastActivity) < POWER_CUT_MS) keepAlive = true;

  digitalWrite(PIN_ESP_HOLD, keepAlive ? HIGH : LOW);
}

// ----------------------------
// Poll inputs/adc
// ----------------------------
static uint32_t tPollIn = 0;
static uint32_t tPollAdc = 0;

static DebouncedLevel wiperRawDeb;
static DebouncedLevel ignDeb;
static DebouncedLevel lightSwDeb;

static void pollInputs(uint32_t now) {
  if (now - tPollIn < POLL_INPUT_MS) return;
  tPollIn = now;

  remoteSM.k1.update(mcpInReadA(0), now);
  remoteSM.k2.update(mcpInReadA(1), now);
  remoteSM.k3.update(mcpInReadA(2), now);
  remoteSM.k4.update(mcpInReadA(3), now);

  lightSwDeb.update(mcpInReadA(4), now);
  wiperRawDeb.update(mcpInReadA(5), now);
  ignDeb.update(mcpInReadB(0), now);

  if (remoteSM.k1.shortEvent || remoteSM.k1.holdEvent) markActivity(now);
  if (remoteSM.k2.shortEvent || remoteSM.k2.holdEvent) markActivity(now);
  if (remoteSM.k3.shortEvent || remoteSM.k3.holdEvent) markActivity(now);
  if (remoteSM.k4.shortEvent || remoteSM.k4.holdEvent) markActivity(now);
}

static void pollAdc(uint32_t now) {
  if (now - tPollAdc < ADC_POLL_MS) return;
  tPollAdc = now;

  SPI.beginTransaction(adcSpi);
  for (uint8_t ch = 0; ch < 8; ch++) {
    adcAvg[ch].push(mcp3208_read_raw(ch));
  }
  SPI.endTransaction();
}

// ----------------------------
// MCP init
// ----------------------------
static bool initMcpAll() {
  if (!mcpIn.begin_I2C(MCP_IN_ADDR, &Wire)) return false;
  if (!mcpOut.begin_I2C(MCP_OUT_ADDR, &Wire)) return false;
  if (!mcpMot.begin_I2C(MCP_MOT_ADDR, &Wire)) return false;

  // Inputs active HIGH (du har input-relæ, så ingen pullups her)
  for (uint8_t p = 0; p < 16; p++) {
    mcpIn.pinMode(p, INPUT);
    mcpIn.pullUp(p, LOW);
  }

  // Outputs default LOW
  for (uint8_t p = 0; p < 16; p++) {
    mcpOut.pinMode(p, OUTPUT);
    mcpOut.digitalWrite(p, LOW);
  }

  for (uint8_t p = 0; p < 16; p++) {
    mcpMot.pinMode(p, OUTPUT);
    mcpMot.digitalWrite(p, LOW);
  }

  return true;
}

static void initMotors() {
  // VF/HF på mcpOut pins 12-15
  motor[MTR_WIN_VF].init({BUS_OUT, P_MTR_VF_FWD}, {BUS_OUT, P_MTR_VF_REV});
  motor[MTR_WIN_HF].init({BUS_OUT, P_MTR_HF_FWD}, {BUS_OUT, P_MTR_HF_REV});

  // resten på mcpMot 0..7
  motor[MTR_WIN_VB].init({BUS_MOT, P_MTR_VB_FWD}, {BUS_MOT, P_MTR_VB_REV});
  motor[MTR_WIN_HB].init({BUS_MOT, P_MTR_HB_FWD}, {BUS_MOT, P_MTR_HB_REV});
  motor[MTR_FLAP1 ].init({BUS_MOT, P_MTR_F1_FWD}, {BUS_MOT, P_MTR_F1_REV});
  motor[MTR_FLAP2 ].init({BUS_MOT, P_MTR_F2_FWD}, {BUS_MOT, P_MTR_F2_REV});
}

// ----------------------------
// Fatal
// ----------------------------
static void fatalHalt() {
  pinMode(PIN_ESP_HOLD, OUTPUT);
  digitalWrite(PIN_ESP_HOLD, LOW);
  pinMode(PIN_DRV_EN, OUTPUT);
  digitalWrite(PIN_DRV_EN, LOW);

  pinMode(PIN_LED_FAULT, OUTPUT);
  digitalWrite(PIN_LED_FAULT, HIGH);

  while (true) { }
}

// ----------------------------
// Setup / Loop
// ----------------------------
void setup() {
  // FEJL-LED default OFF (tændes kun i fatalHalt)
  pinMode(PIN_LED_FAULT, OUTPUT);
  digitalWrite(PIN_LED_FAULT, LOW);

  // 1) Spoler fysisk låst ude under boot
  pinMode(PIN_DRV_EN, OUTPUT);
  digitalWrite(PIN_DRV_EN, LOW);

  // 2) Hold logik-12V vågen tidligt (så wake på K1/K2 med IGN OFF ikke dropper)
  pinMode(PIN_ESP_HOLD, OUTPUT);
  digitalWrite(PIN_ESP_HOLD, HIGH);

  // PWM
  ledcSetup(0, INSTR_PWM_FREQ_HZ, 8);
  ledcAttachPin(PIN_INSTR_PWM, 0);
  writeInstrPwm(1.0f);

  // SPI
  pinMode(PIN_MCP3208_CS, OUTPUT);
  digitalWrite(PIN_MCP3208_CS, HIGH);
  SPI.begin(PIN_SPI_SCK, PIN_SPI_MISO, PIN_SPI_MOSI);

  // I2C robust init
  i2cInitRobust(PIN_I2C_SDA, PIN_I2C_SCL);

  // Init MCPs
  if (!initMcpAll()) fatalHalt();

  initMotors();

  // Safe outputs (ALT LOW)
  setAllOutputsSafe();

  // 3) Først nu: coil-supply enable
  digitalWrite(PIN_DRV_EN, HIGH);

  g_lastActivity = millis();
}

void loop() {
  uint32_t now = millis();

  outputsDefault();

  pollInputs(now);
  pollAdc(now);

  bool ignOn     = ignDeb.stable;
  bool lightswOn = lightSwDeb.stable;
  bool wiperRaw  = wiperRawDeb.stable;

  // Remote
  remoteSM.tick(now, ignOn);

  // Pulsers / blink
  defaLockPulse.tick(now);
  defaUnlockPulse.tick(now);
  blinkSM.tick(now);

  // Instr
  instrSM.tick(now, lightswOn, adcAvg[ADC_LIGHT].mean());

  // Wiper
  wiperSM.tick(now, ignOn, wiperRaw, adcAvg[ADC_RAIN].mean());

  // Motor stop: max tid + overstrøm
  for (uint8_t i = 0; i < MTR_COUNT; i++) {
    if (motor[i].running()) {
      if ((now - motor[i].tStart) >= MOTOR_MAX_MS) motor[i].stop(now);
      else if (motorOverCurrent((MotorId)i, now))  motor[i].stop(now);
    }
  }

  // Motor tick (coils)
  for (uint8_t i = 0; i < MTR_COUNT; i++) motor[i].tick(now);

  // Apply outputs (misc)
  outputsApply(now);

  // Hold / power-cut
  powerHoldTick(now, ignOn);
}