/*  FINAL UPDATED: ESP32 FreeRTOS Alarm System + Keypad 4x4 + Password
    - Single WatchdogTask
    - First-run: require set password before alarm/sensors can arm/trigger
    - After successful password entry, keypad unlocked for 2 minutes (no password)
    - A/B/C/D: if unlocked -> execute action immediately; otherwise request password
    - '#' confirm, '*' long-press -> change PW flow
    - LCD updates only on change; backlight on keypad/remote for 20s
    - Password stored in EEPROM (max 8 digits)
*/

#include <Arduino.h>
#include <EEPROM.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "esp_system.h"
#include "Keypad.h"

// --- hardware settings ---
#define NUM_SENSORS 3
#define NUM_REMOTE_KEYS 4

const int sensorPins[NUM_SENSORS] = {2, 3, 4};
const int remotePins[NUM_REMOTE_KEYS] = {6, 7, 14, 15};
const int buzzerPin = 5;
const int ledArmedPin = 10;
const int ledDisarmedPin = 11;
const int ledSilentPin = 12;
const int ledSensorPins[NUM_SENSORS] = {34, 35, 36};

const int batteryRelayPin = 33;
const int batteryAnalogPin = 18; // ADC1_CH0 (adjust if needed)

// battery thresholds and divider
const float batteryStartCharge = 12.4;
const float batteryStopCharge  = 12.6;
const float batteryDividerRatio = 2.6;

// EEPROM addresses
#define EEPROM_ADDR_ARMED 0
#define EEPROM_ADDR_SILENT 1
#define EEPROM_MAGIC_ADDR 10
#define EEPROM_MAGIC_VALUE 0xAB
#define EEPROM_ADDR_SENSOR_LATCHED 20

// password storage
#define EEPROM_ADDR_PW_MAGIC 50
#define EEPROM_PW_MAGIC 0x5A
#define EEPROM_ADDR_PW 60
#define MAX_PW_LEN 8

// system state (shared)
volatile bool systemArmed = false;
volatile bool silentMode = false;
volatile bool buzzerActive = false;
volatile bool sensorTriggered = false;
volatile bool sensorLatched[NUM_SENSORS] = {false};
volatile bool remoteLock[NUM_REMOTE_KEYS] = {false};
volatile bool isCharging = false;
unsigned long lastMotionTime = 0;
const unsigned long alarmDuration = 20000; // ms

// PIR filtering
int pirHitCount[NUM_SENSORS] = {0};
unsigned long pirLastWindow[NUM_SENSORS] = {0};
const unsigned long PIR_WINDOW = 600;
const int PIR_REQUIRED_HITS = 2;

// beep request (from remote/actions)
volatile bool beepRequested = false;
volatile int beepCount = 0;

// LCD & backlight
LiquidCrystal_I2C lcd(0x27, 16, 2); // change if needed
volatile unsigned long lcdBacklightUntil = 0; // millis until backlight stays on

// shared debugging/status values
volatile int batteryPercent = -1;
volatile int lastSensorIndex = -1; // last sensor that triggered (1..N), -1 none

// synchronization
portMUX_TYPE stateMux = portMUX_INITIALIZER_UNLOCKED;

// --- Keypad unlock (2 minutes after successful PW) ---
volatile unsigned long keypadUnlockedUntil = 0; // millis

inline bool isKeypadUnlocked() {
  return (millis() < keypadUnlockedUntil);
}

// --- Heartbeats for watchdog (timestamps in millis) ---
volatile unsigned long hb_LCD = 0;
volatile unsigned long hb_Remote = 0;
volatile unsigned long hb_Sensor = 0;
volatile unsigned long hb_Buzzer = 0;
volatile unsigned long hb_Battery = 0;
volatile unsigned long hb_Status = 0;
volatile unsigned long hb_Keypad = 0;

// watchdog thresholds (ms)
const unsigned long WATCHDOG_THRESHOLD = 10000UL; // 10 seconds
const unsigned long WATCHDOG_CHECK_PERIOD = 2000UL; // check every 2s

// forward decls
void saveSystemState();
void loadSystemState();
void saveSensorLatchState();
void loadSensorLatchState();
bool isReliablePIR(int index);
void requestBeep(int times);
void updateStatusLEDs();
void resetSensorLEDs();
void handleRemoteCommand(int key);
int readBatteryAverage();
void checkBattery();

// ---------------- Keypad setup ----------------
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};

// pins (as you requested: from 37 up; adjust if hardware requires)
byte rowPins[ROWS] = {37, 38, 39, 40};   // outputs (note: check hardware pin capabilities)
byte colPins[COLS] = {19, 20, 21, 26};   // inputs (with internal pullups)

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// password runtime state
bool pw_exists = false;
char pw_buf[MAX_PW_LEN+1];
int pw_len = 0;

// attempt counters / lockout
int pw_fail_count = 0;
bool pw_lockout = false;
unsigned long pw_lockout_until = 0; // millis

// states for keypad flows
enum KPMode { KP_IDLE, KP_ENTER_FOR_ACTION, KP_SETTING_NEW, KP_WAIT_OLD_FOR_CHANGE, KP_ENTER_NEW_FOR_CHANGE };
KPMode kpMode = KP_IDLE;
char pendingActionKey = 0; // key that user requested action for (exec after successful pw)
char input_buffer[16];
int input_len = 0;
unsigned long star_press_time = 0; // for long-press detect

// ---------------- EEPROM helpers ----------------
void safeEEPROMWrite(int addr, byte value) {
  if (EEPROM.read(addr) != value) EEPROM.write(addr, value);
}

void saveSystemState() {
  portENTER_CRITICAL(&stateMux);
  safeEEPROMWrite(EEPROM_ADDR_ARMED, systemArmed ? 1 : 0);
  safeEEPROMWrite(EEPROM_ADDR_SILENT, silentMode ? 1 : 0);
  safeEEPROMWrite(EEPROM_MAGIC_ADDR, EEPROM_MAGIC_VALUE);
  EEPROM.commit();
  portEXIT_CRITICAL(&stateMux);
}

void loadSystemState() {
  byte magic = EEPROM.read(EEPROM_MAGIC_ADDR);
  if (magic == EEPROM_MAGIC_VALUE) {
    systemArmed = EEPROM.read(EEPROM_ADDR_ARMED);
    silentMode = EEPROM.read(EEPROM_ADDR_SILENT);
    Serial.println(systemArmed ? (silentMode ? "✅ ARMED (SILENT)" : "✅ ARMED") : "❌ DISARMED");
  } else {
    systemArmed = false;
    silentMode = false;
    saveSystemState();
    Serial.println("⚠ EEPROM corrupted, defaults loaded");
  }
}

void saveSensorLatchState() {
  portENTER_CRITICAL(&stateMux);
  for (int i = 0; i < NUM_SENSORS; i++) {
    safeEEPROMWrite(EEPROM_ADDR_SENSOR_LATCHED + i, sensorLatched[i] ? 1 : 0);
  }
  EEPROM.commit();
  portEXIT_CRITICAL(&stateMux);
}

void loadSensorLatchState() {
  for (int i = 0; i < NUM_SENSORS; i++) {
    sensorLatched[i] = EEPROM.read(EEPROM_ADDR_SENSOR_LATCHED + i);
  }
}

// ---------------- password EEPROM ----------------
void savePasswordToEEPROM(const char* pw, int len) {
  portENTER_CRITICAL(&stateMux);
  EEPROM.write(EEPROM_ADDR_PW_MAGIC, EEPROM_PW_MAGIC);
  EEPROM.write(EEPROM_ADDR_PW, len);
  for (int i=0;i<len && i<MAX_PW_LEN;i++) EEPROM.write(EEPROM_ADDR_PW + 1 + i, pw[i]);
  EEPROM.commit();
  portEXIT_CRITICAL(&stateMux);
}

int loadPasswordFromEEPROM(char* out_pw) {
  byte magic = EEPROM.read(EEPROM_ADDR_PW_MAGIC);
  if (magic != EEPROM_PW_MAGIC) return 0;
  int len = EEPROM.read(EEPROM_ADDR_PW);
  if (len <=0 || len > MAX_PW_LEN) return 0;
  for (int i=0;i<len;i++) out_pw[i] = (char)EEPROM.read(EEPROM_ADDR_PW + 1 + i);
  out_pw[len]=0;
  return len;
}

bool checkPassword(const char* candidate, int len) {
  char stored[MAX_PW_LEN+1];
  int stored_len = loadPasswordFromEEPROM(stored);
  if (stored_len==0) return false;
  if (stored_len != len) return false;
  for (int i=0;i<len;i++) if (stored[i] != candidate[i]) return false;
  return true;
}

// ---------------- PIR reliability ----------------
bool isReliablePIR(int index) {
  if (digitalRead(sensorPins[index]) == HIGH) {
    unsigned long now = millis();
    if (now - pirLastWindow[index] > PIR_WINDOW) {
      pirHitCount[index] = 1;
      pirLastWindow[index] = now;
    } else {
      pirHitCount[index]++;
      pirLastWindow[index] = now;
      if (pirHitCount[index] >= PIR_REQUIRED_HITS) {
        pirHitCount[index] = 0;
        return true;
      }
    }
  }
  return false;
}

// ---------------- beep ----------------
void requestBeep(int times) {
  portENTER_CRITICAL(&stateMux);
  if (!beepRequested) {
    beepCount = times * 2;
    beepRequested = true;
  }
  // also wake/display LCD on remote press
  lcdBacklightUntil = millis() + 20000UL;
  portEXIT_CRITICAL(&stateMux);
}

// ---------------- high-level helpers ----------------
void activateAlarm() {
  portENTER_CRITICAL(&stateMux);
  if (!silentMode) {
    buzzerActive = true;
    digitalWrite(buzzerPin, HIGH);
  } else {
    buzzerActive = false;
    digitalWrite(buzzerPin, LOW);
  }
  portEXIT_CRITICAL(&stateMux);
}

void updateStatusLEDs() {
  portENTER_CRITICAL(&stateMux);
  digitalWrite(ledArmedPin, systemArmed && !silentMode);
  digitalWrite(ledSilentPin, systemArmed && silentMode);
  digitalWrite(ledDisarmedPin, !systemArmed);
  for (int i = 0; i < NUM_SENSORS; i++) {
    digitalWrite(ledSensorPins[i], sensorLatched[i]);
  }
  portEXIT_CRITICAL(&stateMux);
}

void resetSensorLEDs() {
  portENTER_CRITICAL(&stateMux);
  for (int i = 0; i < NUM_SENSORS; i++) {
    sensorLatched[i] = false;
    digitalWrite(ledSensorPins[i], LOW);
  }
  saveSensorLatchState();
  portEXIT_CRITICAL(&stateMux);
}

// If password not set yet, block remote arming and prompt to set PW first
void handleRemoteCommand(int key) {
  portENTER_CRITICAL(&stateMux);
  // ensure LCD backlight on for 20s
  lcdBacklightUntil = millis() + 20000UL;

  if (!pw_exists) {
    // Block actions until password is set
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Set PIN first");
    lcd.setCursor(0,1);
    lcd.print("# on keypad");
    portEXIT_CRITICAL(&stateMux);
    return;
  }

  switch (key) {
    case 0:
      if (!systemArmed || silentMode) {
        systemArmed = true; silentMode = false; sensorTriggered = false;
        resetSensorLEDs(); saveSystemState(); requestBeep(1);
        Serial.println("✅ ARMED (normal)");
      }
      break;
    case 1:
      if (systemArmed) {
        systemArmed = false; silentMode = false; buzzerActive = false;
        sensorTriggered = false;
        digitalWrite(buzzerPin, LOW); saveSystemState(); requestBeep(1);
        Serial.println("❌ DISARMED");
      }
      break;
    case 2:
      if (!systemArmed || !silentMode) {
        systemArmed = true; silentMode = true; sensorTriggered = false;
        resetSensorLEDs(); saveSystemState(); requestBeep(1);
        Serial.println("🔇 ARMED (SILENT)");
      }
      break;
    case 3:
      if (buzzerActive) {
        buzzerActive = false;
        digitalWrite(buzzerPin, LOW);
        requestBeep(1);
        Serial.println("🛑 Alarm manually stopped");
      }
      break;
  }
  updateStatusLEDs();
  portEXIT_CRITICAL(&stateMux);
}

// ---------------- battery helpers ----------------
int readBatteryAverage() {
  long sum = 0;
  for (int i = 0; i < 10; i++) {
    sum += analogRead(batteryAnalogPin);
    vTaskDelay(5 / portTICK_PERIOD_MS);
  }
  return sum / 10;
}

void checkBattery() {
  static bool lastChargingState = false;
  static int lastBatteryPercent = -1;

  int rawBattery = readBatteryAverage();
  // ADC on ESP32 is 0..4095; using 3.3V ref
  float voltageBattery = (rawBattery * 3.3f / 4095.0f) * batteryDividerRatio;

  portENTER_CRITICAL(&stateMux);
  if (!isCharging && voltageBattery < batteryStartCharge) {
    digitalWrite(batteryRelayPin, HIGH);
    isCharging = true;
  } else if (isCharging && voltageBattery >= batteryStopCharge) {
    digitalWrite(batteryRelayPin, LOW);
    isCharging = false;
  }
  portEXIT_CRITICAL(&stateMux);

  if (isCharging != lastChargingState) {
    if (isCharging) Serial.println("🔋 Charging started");
    else Serial.println("✅ Charging stopped");
    lastChargingState = isCharging;
  }

  float percentage = ((voltageBattery - 11.0f) / (12.6f - 11.0f)) * 100.0f;
  percentage = constrain(percentage, 0, 100);
  int percentInt = round(percentage);

  portENTER_CRITICAL(&stateMux);
  batteryPercent = percentInt;
  portEXIT_CRITICAL(&stateMux);

  if (percentInt != lastBatteryPercent) {
    lastBatteryPercent = percentInt;
    Serial.print("🔋 Battery: ");
    Serial.print(percentInt);
    Serial.println(" %");
  }
}

// ---------------- Tasks ----------------

// LCDTask
void LCDTask(void *pvParameters) {
  (void) pvParameters;
  lcd.init();
  lcd.backlight();
  vTaskDelay(200 / portTICK_PERIOD_MS);

  bool lastBuzzer = false;
  bool lastSystemArmed = false;
  bool lastSilent = false;
  int  lastBattery = -1;
  int  lastSensor = -1;
  bool forceUpdate = true;

  const TickType_t xPeriod = 500 / portTICK_PERIOD_MS;
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    hb_LCD = millis();

    portENTER_CRITICAL(&stateMux);
    bool localBuzzer = buzzerActive;
    bool localSystemArmed = systemArmed;
    bool localSilent = silentMode;
    int localBattery = batteryPercent;
    int localSensor = lastSensorIndex;
    unsigned long localBacklightUntil = lcdBacklightUntil;
    bool localPwExists = pw_exists;
    portEXIT_CRITICAL(&stateMux);

    unsigned long now = millis();

    // backlight
    if (localBacklightUntil > now) lcd.backlight();
    else lcd.noBacklight();

    // if no password set -> show prompt (forceUpdate to true so message appears)
    if (!localPwExists) {
      if (!forceUpdate) { // ensure it shows at least once
        lcd.clear();
        lcd.setCursor(0,0); lcd.print("FIRST RUN");
        lcd.setCursor(0,1); lcd.print("Set PIN -> #");
        forceUpdate = true;
      }
    } else if (localBuzzer != lastBuzzer ||
        localSystemArmed != lastSystemArmed ||
        localSilent != lastSilent ||
        localBattery != lastBattery ||
        localSensor != lastSensor ||
        forceUpdate) {

      lcd.clear();

      if (localBuzzer) {
        lcd.setCursor(0, 0);
        lcd.print("ALARM ACTIVE   ");
        lcd.setCursor(0, 1);
        if (localSensor >= 1 && localSensor <= NUM_SENSORS) {
          lcd.print("Sensor ");
          lcd.print(localSensor);
          lcd.print("        ");
        } else {
          lcd.print("Sensor ?       ");
        }
      } else {
        lcd.setCursor(0, 0);
        if (!localSystemArmed) lcd.print("MOOD : DISARMED");
        else if (localSilent) lcd.print("MOOD : SILENT ");
        else lcd.print("MOOD : ARMED   ");

        lcd.setCursor(0, 1);
        if (localBattery >= 0) {
          lcd.print("Battery: ");
          if (localBattery < 10) lcd.print(' ');
          lcd.print(localBattery);
          lcd.print("%          ");
        } else {
          lcd.print("Battery: --%   ");
        }
      }

      lastBuzzer = localBuzzer;
      lastSystemArmed = localSystemArmed;
      lastSilent = localSilent;
      lastBattery = localBattery;
      lastSensor = localSensor;
      forceUpdate = false;
    }

    // CPU freq optimization
    static unsigned long lastActivity = 0;
    if (systemArmed || buzzerActive || (millis() - lastMotionTime) < 5000UL) {
      lastActivity = millis();
      setCpuFrequencyMhz(240);
    } else {
      if (millis() - lastActivity > 10000UL) {
        setCpuFrequencyMhz(80);
      }
    }

    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

// RemoteTask
void RemoteTask(void *pvParameters) {
  (void) pvParameters;
  const TickType_t xPeriod = 50 / portTICK_PERIOD_MS;
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    hb_Remote = millis();

    for (int i = 0; i < NUM_REMOTE_KEYS; i++) {
      if (digitalRead(remotePins[i]) == HIGH && !remoteLock[i]) {
        vTaskDelay(30 / portTICK_PERIOD_MS); // debounce
        if (digitalRead(remotePins[i]) == HIGH) {
          handleRemoteCommand(i);
          remoteLock[i] = true;
        }
      } else if (digitalRead(remotePins[i]) == LOW) {
        remoteLock[i] = false;
      }
    }

    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

// SensorTask
void SensorTask(void *pvParameters) {
  (void) pvParameters;
  const TickType_t xPeriod = 50 / portTICK_PERIOD_MS;
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    hb_Sensor = millis();

    // Block sensors if password not set yet
    if (!pw_exists) {
      vTaskDelayUntil(&xLastWakeTime, xPeriod);
      continue;
    }

    if (systemArmed) {
      for (int i = 0; i < NUM_SENSORS; i++) {
        if (isReliablePIR(i)) {
          portENTER_CRITICAL(&stateMux);
          sensorTriggered = true;
          lastMotionTime = millis();
          activateAlarm();
          if (!sensorLatched[i]) {
            sensorLatched[i] = true;
            saveSensorLatchState();
            Serial.print("🚨 Triggered sensor ");
            Serial.println(i + 1);
            lastSensorIndex = i + 1; // save 1-based index for LCD
            updateStatusLEDs();
          } else {
            lastSensorIndex = i + 1;
          }
          portEXIT_CRITICAL(&stateMux);
        }
      }
    }

    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

// BuzzerTask
void BuzzerTask(void *pvParameters) {
  (void) pvParameters;
  bool buzState = false;
  int intervalCounter = 0;

  for (;;) {
    hb_Buzzer = millis();

    portENTER_CRITICAL(&stateMux);
    if (beepRequested && beepCount > 0) {
      if (intervalCounter == 0) {
        buzState = !buzState;
        digitalWrite(buzzerPin, buzState);
        if (!buzState) {
          beepCount--;
          if (beepCount == 0) beepRequested = false;
        }
        intervalCounter = 2;
      } else {
        intervalCounter--;
      }
    } else {
      if (buzzerActive) digitalWrite(buzzerPin, HIGH);
      else digitalWrite(buzzerPin, LOW);
    }
    portEXIT_CRITICAL(&stateMux);

    vTaskDelay(10 / portTICK_PERIOD_MS);
  }
}

// AlarmTask
void AlarmTask(void *pvParameters) {
  (void) pvParameters;
  const TickType_t xPeriod = 500 / portTICK_PERIOD_MS;
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    portENTER_CRITICAL(&stateMux);
    if (buzzerActive && (millis() - lastMotionTime > alarmDuration)) {
      buzzerActive = false;
      digitalWrite(buzzerPin, LOW);
      Serial.println("🔕 Alarm auto-stopped");
    }
    portEXIT_CRITICAL(&stateMux);

    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

// BatteryTask
void BatteryTask(void *pvParameters) {
  (void) pvParameters;
  const TickType_t xPeriod = 1000 / portTICK_PERIOD_MS; // every 1s
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    hb_Battery = millis();
    checkBattery();
    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

// StatusTask
void StatusTask(void *pvParameters) {
  (void) pvParameters;
  const TickType_t xPeriod = 1000 / portTICK_PERIOD_MS;
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    hb_Status = millis();
    updateStatusLEDs();
    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

// ---------------- Keypad helpers / flows ----------------

void startPasswordEntryForAction(char actionKey) {
  kpMode = KP_ENTER_FOR_ACTION;
  pendingActionKey = actionKey;
  input_len = 0;
  input_buffer[0]=0;
  lcd.clear();
  lcd.setCursor(0,0); lcd.print("Enter Password");
  lcd.setCursor(0,1); lcd.print("Press # to OK");
  lcdBacklightUntil = millis() + 20000UL;
}

void startSettingNewPassword() {
  kpMode = KP_SETTING_NEW;
  input_len = 0; input_buffer[0]=0;
  lcd.clear();
  lcd.setCursor(0,0); lcd.print("SET NEW PASSWORD");
  lcd.setCursor(0,1); lcd.print("Enter digits + #");
  lcdBacklightUntil = millis() + 20000UL;
}

void startChangePasswordFlow() {
  kpMode = KP_WAIT_OLD_FOR_CHANGE;
  input_len = 0; input_buffer[0]=0;
  lcd.clear();
  lcd.setCursor(0,0); lcd.print("Change password?");
  lcd.setCursor(0,1); lcd.print("Press # to start");
  lcdBacklightUntil = millis() + 20000UL;
}

void grantKeypadUnlockTemporary() {
  // grant 2 minutes of unlocked keypad use
  keypadUnlockedUntil = millis() + 120000UL; // 120000 ms = 2 minutes
}

void handlePasswordEntryConfirm() {
  if (kpMode == KP_SETTING_NEW) {
    if (input_len < 4) {
      lcd.clear(); lcd.setCursor(0,0); lcd.print("Min 4 digits!");
      vTaskDelay(1000/portTICK_PERIOD_MS);
      startSettingNewPassword();
      return;
    }
    // save
    savePasswordToEEPROM(input_buffer, input_len);
    pw_exists = true;
    lcd.clear(); lcd.setCursor(0,0); lcd.print("Password Saved");
    vTaskDelay(1200/portTICK_PERIOD_MS);
    // grant temporary unlock so user can use keypad/remote for 2 minutes
    grantKeypadUnlockTemporary();
    kpMode = KP_IDLE;
    return;
  } else if (kpMode == KP_ENTER_FOR_ACTION) {
    if (pw_lockout && millis() < pw_lockout_until) {
      lcd.clear(); lcd.setCursor(0,0); lcd.print("ACCESS LOCKED");
      lcd.setCursor(0,1); lcd.print("Try later");
      vTaskDelay(1200/portTICK_PERIOD_MS);
      kpMode = KP_IDLE;
      return;
    }
    if (checkPassword(input_buffer, input_len)) {
      // success -> perform action
      lcd.clear(); lcd.setCursor(0,0); lcd.print("OK");
      vTaskDelay(400/portTICK_PERIOD_MS);
      // grant temporary unlocking
      grantKeypadUnlockTemporary();
      // map pendingActionKey to action:
      char k = pendingActionKey;
      if (k == 'A') handleRemoteCommand(0);
      else if (k == 'B') handleRemoteCommand(1);
      else if (k == 'C') handleRemoteCommand(2);
      else if (k == 'D') handleRemoteCommand(3);
      pw_fail_count = 0;
      kpMode = KP_IDLE;
      return;
    } else {
      pw_fail_count++;
      lcd.clear(); lcd.setCursor(0,0); lcd.print("Wrong Password");
      lcd.setCursor(0,1); lcd.print("Attempts:");
      lcd.print(pw_fail_count);
      vTaskDelay(900/portTICK_PERIOD_MS);
      if (pw_fail_count >= 3) {
        // lockout
        pw_lockout = true;
        pw_lockout_until = millis() + 30000UL; // 30s lock
        // short alarm
        portENTER_CRITICAL(&stateMux);
        buzzerActive = true;
        digitalWrite(buzzerPin, HIGH);
        portEXIT_CRITICAL(&stateMux);
        lcd.clear(); lcd.setCursor(0,0); lcd.print("ACCESS DENIED");
        lcd.setCursor(0,1); lcd.print("Locked 30s");
        vTaskDelay(1200/portTICK_PERIOD_MS);
        // stop buzzer
        portENTER_CRITICAL(&stateMux);
        buzzerActive = false; digitalWrite(buzzerPin, LOW);
        portEXIT_CRITICAL(&stateMux);
      }
      kpMode = KP_IDLE;
      return;
    }
  } else if (kpMode == KP_WAIT_OLD_FOR_CHANGE) {
    // check old password
    if (checkPassword(input_buffer, input_len)) {
      // ask for new
      kpMode = KP_ENTER_NEW_FOR_CHANGE;
      input_len = 0; input_buffer[0]=0;
      lcd.clear(); lcd.setCursor(0,0); lcd.print("Enter new pass");
      lcd.setCursor(0,1); lcd.print("then press OK");
      lcdBacklightUntil = millis() + 20000UL;
      return;
    } else {
      pw_fail_count++;
      lcd.clear(); lcd.setCursor(0,0); lcd.print("Wrong Password");
      vTaskDelay(900/portTICK_PERIOD_MS);
      if (pw_fail_count >= 3) {
        pw_lockout = true;
        pw_lockout_until = millis() + 30000UL;
        lcd.clear(); lcd.setCursor(0,0); lcd.print("ACCESS DENIED");
        lcd.setCursor(0,1); lcd.print("Locked 30s");
        vTaskDelay(1200/portTICK_PERIOD_MS);
      }
      kpMode = KP_IDLE;
      return;
    }
  } else if (kpMode == KP_ENTER_NEW_FOR_CHANGE) {
    if (input_len < 4) {
      lcd.clear(); lcd.setCursor(0,0); lcd.print("Min 4 digits!");
      vTaskDelay(900/portTICK_PERIOD_MS);
      kpMode = KP_IDLE;
      return;
    }
    // save
    savePasswordToEEPROM(input_buffer, input_len);
    lcd.clear(); lcd.setCursor(0,0); lcd.print("Password Changed");
    vTaskDelay(1000/portTICK_PERIOD_MS);
    // grant temporary unlock after change
    grantKeypadUnlockTemporary();
    kpMode = KP_IDLE;
    return;
  }
}

// Keypad task
void KeypadTask(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    hb_Keypad = millis();
    char key = keypad.getKey();
    if (key) {
      // any key wakes backlight
      lcdBacklightUntil = millis() + 20000UL;

      // if currently locked due to fail, ignore keys except to display message
      if (pw_lockout && millis() < pw_lockout_until) {
        lcd.clear(); lcd.setCursor(0,0); lcd.print("ACCESS LOCKED");
        unsigned long tleft = (pw_lockout_until - millis())/1000;
        lcd.setCursor(0,1); lcd.print("Wait ");
        lcd.print(tleft);
        lcd.print("s     ");
        vTaskDelay(800/portTICK_PERIOD_MS);
        continue;
      } else if (pw_lockout && millis() >= pw_lockout_until) {
        pw_lockout = false; pw_fail_count = 0;
      }

      // detect '*' long-press
      if (key == '*') {
        star_press_time = millis();
        while (keypad.getState() == PRESSED && keypad.getKey() == '*') {
          if (millis() - star_press_time >= 3000) {
            startChangePasswordFlow();
            break;
          }
          vTaskDelay(50/portTICK_PERIOD_MS);
        }
        // if not long press, continue to handle as short press below
      }

      // If in a password-entry mode, collect digits and respond to '#'
      if (kpMode == KP_ENTER_FOR_ACTION || kpMode == KP_SETTING_NEW || kpMode == KP_WAIT_OLD_FOR_CHANGE || kpMode == KP_ENTER_NEW_FOR_CHANGE) {
        if (key >= '0' && key <= '9') {
          if (input_len < (int)sizeof(input_buffer)-1) {
            input_buffer[input_len++] = key;
            input_buffer[input_len]=0;
            lcd.setCursor(0,1); lcd.print("Entered: ");
            for (int i=0;i<input_len;i++) lcd.print('*');
            for (int i=input_len;i<6;i++) lcd.print(' ');
          }
        } else if (key == '#') {
          handlePasswordEntryConfirm();
        } else if (key == '*') {
          if (input_len>0) { input_len--; input_buffer[input_len]=0; }
        } else {
          // ignore A-D while entering
        }
        vTaskDelay(120/portTICK_PERIOD_MS);
        continue;
      }

      // Normal idle behavior:
      // A-D: if keypad unlocked -> execute, else start password entry
      if (key == 'A' || key == 'B' || key == 'C' || key == 'D') {
        if (isKeypadUnlocked() && pw_exists) {
          // direct execute
          if (key == 'A') handleRemoteCommand(0);
          else if (key == 'B') handleRemoteCommand(1);
          else if (key == 'C') handleRemoteCommand(2);
          else if (key == 'D') handleRemoteCommand(3);
        } else {
          // request password for that action
          startPasswordEntryForAction(key);
        }
        continue;
      }

      // Numeric keys in idle -> wake backlight + offer to enter password for numeric action
      if ((key >= '0' && key <= '9')) {
        lcd.clear();
        lcd.setCursor(0,0); lcd.print("Enter Password");
        kpMode = KP_ENTER_FOR_ACTION;
        input_len = 0; input_buffer[0]=0;
        pendingActionKey = key;
        continue;
      }

      // '#' pressed in idle -> if no pw set, start set password
      if (key == '#') {
        if (!pw_exists) {
          startSettingNewPassword();
          continue;
        } else {
          lcd.clear(); lcd.setCursor(0,0); lcd.print("Press digit/key");
          lcd.setCursor(0,1); lcd.print("to use keypad");
          vTaskDelay(900/portTICK_PERIOD_MS);
        }
      }

      // '*' short press: hint
      if (key == '*') {
        lcd.clear(); lcd.setCursor(0,0); lcd.print("Hold * to change");
        lcd.setCursor(0,1); lcd.print("password");
        vTaskDelay(900/portTICK_PERIOD_MS);
        lcdBacklightUntil = millis() + 20000UL;
        continue;
      }
    }

    vTaskDelay(30 / portTICK_PERIOD_MS);
  }
}

// WatchdogTask (single implementation)
void WatchdogTask(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    unsigned long now = millis();

    if ((now - hb_LCD) > WATCHDOG_THRESHOLD ||
        (now - hb_Remote) > WATCHDOG_THRESHOLD ||
        (now - hb_Sensor) > WATCHDOG_THRESHOLD ||
        (now - hb_Buzzer) > WATCHDOG_THRESHOLD ||
        (now - hb_Battery) > WATCHDOG_THRESHOLD ||
        (now - hb_Status) > WATCHDOG_THRESHOLD ||
        (now - hb_Keypad) > WATCHDOG_THRESHOLD) {

      Serial.println("!!! Watchdog: task unresponsive, restarting ...");
      delay(200);
      esp_restart();
    }

    vTaskDelay(WATCHDOG_CHECK_PERIOD / portTICK_PERIOD_MS);
  }
}

// ---------------- setup & main ----------------
TaskHandle_t remoteHandle = NULL;
TaskHandle_t sensorHandle = NULL;
TaskHandle_t buzzerHandle = NULL;
TaskHandle_t alarmHandle = NULL;
TaskHandle_t batteryHandle = NULL;
TaskHandle_t statusHandle = NULL;
TaskHandle_t lcdHandle = NULL;
TaskHandle_t watchdogHandle = NULL;
TaskHandle_t keypadHandle = NULL;

void setup() {
  Serial.begin(115200);
  delay(500);
  Serial.println("🛡️ Alarm (FreeRTOS + LCD + Keypad) starting...");

  // pins
  for (int i = 0; i < NUM_SENSORS; i++) {
    pinMode(sensorPins[i], INPUT);
    pinMode(ledSensorPins[i], OUTPUT);
    digitalWrite(ledSensorPins[i], LOW);
  }
  for (int i = 0; i < NUM_REMOTE_KEYS; i++) pinMode(remotePins[i], INPUT);

  pinMode(buzzerPin, OUTPUT);
  pinMode(ledArmedPin, OUTPUT);
  pinMode(ledDisarmedPin, OUTPUT);
  pinMode(ledSilentPin, OUTPUT);
  pinMode(batteryRelayPin, OUTPUT);
  digitalWrite(batteryRelayPin, LOW);
  digitalWrite(buzzerPin, LOW);

  // keypad pin modes: rows as OUTPUT, cols as INPUT_PULLUP
  for (byte i=0;i<ROWS;i++) pinMode(rowPins[i], OUTPUT);
  for (byte j=0;j<COLS;j++) pinMode(colPins[j], INPUT_PULLUP);

  // EEPROM init
  if (!EEPROM.begin(512)) {
    Serial.println("⚠ EEPROM init failed");
  }

  loadSystemState();
  loadSensorLatchState();
  if (!systemArmed) resetSensorLEDs();
  updateStatusLEDs();

  // load pw existence
  char tmp_pw[MAX_PW_LEN+1];
  int l = loadPasswordFromEEPROM(tmp_pw);
  if (l>0) { pw_exists = true; Serial.println("Password loaded"); }
  else {
    pw_exists = false;
    // first-boot: ask to set password
    lcd.init(); lcd.backlight();
    lcd.clear();
    lcd.setCursor(0,0); lcd.print("FIRST RUN");
    lcd.setCursor(0,1); lcd.print("Set Password ");
    // Start setting flow
    kpMode = KP_SETTING_NEW;
    input_len = 0; input_buffer[0]=0;
    lcdBacklightUntil = millis() + 20000UL;

    // Ensure alarm won't arm until password set
    systemArmed = false;
  }

  // initialize heartbeats to now
  unsigned long now = millis();
  hb_LCD = hb_Remote = hb_Sensor = hb_Buzzer = hb_Battery = hb_Status = hb_Keypad = now;

  // create tasks
  xTaskCreate(LCDTask,    "LCD",    4096, NULL, 4, &lcdHandle);
  xTaskCreate(KeypadTask, "Keypad", 4096, NULL, 4, &keypadHandle); // high priority with LCD
  xTaskCreate(RemoteTask, "Remote", 2048, NULL, 3, &remoteHandle);
  xTaskCreate(SensorTask, "Sensor", 4096, NULL, 3, &sensorHandle);
  xTaskCreate(BuzzerTask, "Buzzer", 2048, NULL, 2, &buzzerHandle);
  xTaskCreate(AlarmTask,  "Alarm",  2048, NULL, 2, &alarmHandle);
  xTaskCreate(BatteryTask,"Battery",4096, NULL, 1, &batteryHandle);
  xTaskCreate(StatusTask, "Status", 2048, NULL, 1, &statusHandle);

  // Watchdog task (low priority)
  xTaskCreate(WatchdogTask,"Watchdog",2048, NULL, 1, &watchdogHandle);

  Serial.println("Tasks created.");
}

void loop() {
  vTaskDelay(portMAX_DELAY);
}