#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <ArduinoJson.h>
#include <Preferences.h>
#include <mbedtls/gcm.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>
#include <LittleFS.h>
#include <time.h>
#include <esp_task_wdt.h>
#include <esp_system.h>
#include <esp_heap_caps.h>

#define RELAI_BUKA            18
#define RELAI_TUTUP           19
#define PWM_MOTOR             23
#define CURRENT_SENSOR        34
#define STATUS_LED            2
#define BUZZER_PIN            4

#define TOMBOL_MULAI          13
#define SENSOR_PHOTOCELL      27
#define LIMIT_BUKA            32
#define LIMIT_TUTUP           33
#define TOMBOL_DARURAT        14

#define KANAL_PWM             0
#define FREKUENSI_PWM         5000
#define RESOLUSI_PWM          8

static const int PWM_PELAN = 140;
static const int PWM_CEPAT = 255;
static const int CURRENT_THRESHOLD = 1850;
static const int CURRENT_SPIKE_LIMIT = 2600;
static const int CURRENT_FILTER_SAMPLES = 12;

static const unsigned long DURASI_RAMP = 1200;
static const unsigned long DURASI_TERBUKA = 8000;
static const unsigned long BATAS_WAKTU_BUKA = 14500;
static const unsigned long BATAS_WAKTU_TUTUP = 14500;
static const unsigned long DURASI_DEADTIME = 280;
static const unsigned long DIRECTION_DEADTIME_MS = 180;
static const unsigned long DEBOUNCE_DELAY = 50;
static const unsigned long HOMING_TIMEOUT = 28000;
static const unsigned long PHOTOCELL_STUCK_TIMEOUT = 25000;
static const unsigned long WS_INTERVAL = 200;
static const unsigned long WIFI_CHECK_INTERVAL = 5000;
static const unsigned long ANTI_JEPIT_RETRY_LIMIT = 3;
static const unsigned long BOOT_FAULT_HOLD_MS = 1500;
static const unsigned long WIFI_BACKOFF_BASE_MS = 5000;
static const unsigned long WIFI_BACKOFF_MAX_MS = 30000;
static const unsigned long HEAP_LOG_INTERVAL_MS = 60000;

static const char* AP_NAME = "SlidingGate_Setup";
static const char* AUTH_USER = "admin";
static const char* AUTH_PASS = "gate1234";
static const char* NTP_SERVER = "pool.ntp.org";
static const long GMT_OFFSET_SEC = 7 * 3600;

enum LangkahSistem {
  LANGKAH_VALIDASI_AWAL,
  LANGKAH_HOMING_INIT,
  LANGKAH_HOMING_WAIT,
  LANGKAH_HOMING_RUNNING,
  LANGKAH_SIAGA,
  LANGKAH_BUKA_PELAN,
  LANGKAH_BUKA_CEPAT,
  LANGKAH_BUKA_AKHIR,
  LANGKAH_TERBUKA,
  LANGKAH_DEADTIME_BUKA,
  LANGKAH_TUTUP_PELAN,
  LANGKAH_TUTUP_CEPAT,
  LANGKAH_TUTUP_AKHIR,
  LANGKAH_PAUSE_MANUAL,
  LANGKAH_GANGGUAN,
  LANGKAH_REVERSING_ANTI_JEPIT,
  LANGKAH_ANTI_JEPIT_WAIT
};

enum CommandType { CMD_BUKA, CMD_TUTUP, CMD_STOP, CMD_RESET, CMD_EMERGENCY };
enum FaultSeverity { FAULT_NONE, FAULT_WARNING, FAULT_RECOVERABLE, FAULT_CRITICAL };

struct Command { CommandType type; };
struct LogEntry { char message[192]; };
struct Debounce { bool raw; bool stable; unsigned long lastChange; };

struct SystemSnapshot {
  char state[24];
  char fault[96];
  char resetReason[32];
  uint32_t logDropped;
  uint32_t cmdDropped;
  uint32_t freeHeap;
  uint32_t largestBlock;
  bool emergency;
  int current;
  uint8_t retry;
  FaultSeverity severity;
};

AsyncWebServer server(80);
AsyncWebSocket ws("/ws");
Preferences preferences;
QueueHandle_t commandQueue;
QueueHandle_t logQueue;
SemaphoreHandle_t stateMutex;
SemaphoreHandle_t faultMutex;
SemaphoreHandle_t telemetryMutex;

LangkahSistem langkahAktif = LANGKAH_VALIDASI_AWAL;
FaultSeverity faultSeverity = FAULT_NONE;
Debounce inputs[5] = {};
int currentBuffer[CURRENT_FILTER_SAMPLES] = {0};
uint8_t currentIndex = 0;

char phoneNumber[24] = "6281234567890";
const uint8_t AES_KEY[16] = {'S','l','i','d','i','n','g','G','a','t','e','2','0','2','6','!'};

bool fsReady = false;
bool emergencyLatch = false;
bool antiJepitHandling = false;
bool ntpSynced = false;

unsigned long waktuLangkah = 0;
unsigned long waktuPhotocellStart = 0;
unsigned long lastLedToggle = 0;
unsigned long buzzerEndTime = 0;
unsigned long deadtimeTimer = 0;
unsigned long lastWsPush = 0;
unsigned long lastWifiCheck = 0;
unsigned long bootHoldUntil = 0;
unsigned long directionDeadtimeUntil = 0;
unsigned long wifiBackoffUntil = 0;
unsigned long heapLogTimer = 0;

bool relaiBukaAktif = false;
bool relaiTutupAktif = false;
bool buzzerState = false;
bool pendingOpen = false;
bool pendingClose = false;

int pwmMotor = 0;
int pendingPwm = 0;
int currentReading = 0;
int ledPattern = 0;
uint8_t antiJepitRetry = 0;
uint32_t logDropped = 0;
uint32_t cmdDropped = 0;
uint32_t rebootCount = 0;
String lastError = "";
char lastResetReason[32] = "UNKNOWN";
bool wifiConnecting = false;

const char dashboard_html[] PROGMEM = R"rawliteral(
<!DOCTYPE html>
<html lang="id">
<head>
  <meta charset="UTF-8">
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  <title>Sliding Gate v5.1</title>
  <style>
    body{font-family:Arial,sans-serif;background:#0f172a;color:#e2e8f0;margin:0;padding:20px;}
    .container{max-width:1100px;margin:auto;}
    h1{color:#60a5fa;text-align:center;}
    .card{background:#1e2937;border-radius:12px;padding:20px;margin:15px 0;box-shadow:0 4px 6px rgba(0,0,0,0.3);}
    button{padding:12px 24px;margin:5px;font-size:1.1rem;border:none;border-radius:8px;cursor:pointer;}
    .btn-buka{background:#22c55e;color:white;}
    .btn-tutup{background:#ef4444;color:white;}
    .btn-stop{background:#eab308;color:black;}
    .meta{font-size:0.95rem;opacity:0.9;}
  </style>
</head>
<body>
  <div class="container">
    <h1>🚪 Sliding Gate Controller v5.1</h1>
    <div class="card">
      <h2>Status: <span id="state">LOADING...</span></h2>
      <p>Arus Motor: <span id="current">0</span> mA</p>
      <p>Emergency: <span id="emergency">NORMAL</span></p>
      <p>Fault: <span id="fault">-</span></p>
      <p class="meta">Reset: <span id="reset_reason">-</span></p>
      <p class="meta">Log Drop: <span id="log_dropped">0</span></p>
      <p class="meta">Cmd Drop: <span id="cmd_dropped">0</span></p>
      <p class="meta">Heap Free: <span id="heap">0</span></p>
      <p class="meta">Largest Block: <span id="largest_block">0</span></p>
      <button class="btn-buka" onclick="sendCmd('buka')">BUKA GATE</button>
      <button class="btn-tutup" onclick="sendCmd('tutup')">TUTUP GATE</button>
      <button class="btn-stop" onclick="sendCmd('stop')">STOP</button>
      <button onclick="sendCmd('reset')">RESET FAULT</button>
    </div>
  </div>
  <script>
    const ws = new WebSocket('ws://' + location.hostname + '/ws');
    ws.onmessage = function(evt) {
      const d = JSON.parse(evt.data);
      document.getElementById('state').textContent = d.state;
      document.getElementById('current').textContent = d.current;
      document.getElementById('emergency').textContent = d.emergency ? 'AKTIF ⚠️' : 'NORMAL';
      document.getElementById('fault').textContent = d.fault || '-';
      document.getElementById('reset_reason').textContent = d.reset_reason || '-';
      document.getElementById('log_dropped').textContent = d.log_dropped || 0;
      document.getElementById('cmd_dropped').textContent = d.cmd_dropped || 0;
      document.getElementById('heap').textContent = d.free_heap || 0;
      document.getElementById('largest_block').textContent = d.largest_block || 0;
    };
    function sendCmd(cmd){ fetch(`/aksi?cmd=${cmd}`, {credentials:'include'}); }
  </script>
</body>
</html>
)rawliteral";

String stateName(LangkahSistem s){
  switch(s){
    case LANGKAH_VALIDASI_AWAL:return "VALIDASI_AWAL";
    case LANGKAH_HOMING_INIT:return "HOMING_INIT";
    case LANGKAH_HOMING_WAIT:return "HOMING_WAIT";
    case LANGKAH_HOMING_RUNNING:return "HOMING_RUNNING";
    case LANGKAH_SIAGA:return "SIAGA";
    case LANGKAH_BUKA_PELAN:return "BUKA_PELAN";
    case LANGKAH_BUKA_CEPAT:return "BUKA_CEPAT";
    case LANGKAH_BUKA_AKHIR:return "BUKA_AKHIR";
    case LANGKAH_TERBUKA:return "TERBUKA";
    case LANGKAH_DEADTIME_BUKA:return "DEADTIME_BUKA";
    case LANGKAH_TUTUP_PELAN:return "TUTUP_PELAN";
    case LANGKAH_TUTUP_CEPAT:return "TUTUP_CEPAT";
    case LANGKAH_TUTUP_AKHIR:return "TUTUP_AKHIR";
    case LANGKAH_PAUSE_MANUAL:return "PAUSE_MANUAL";
    case LANGKAH_GANGGUAN:return "GANGGUAN";
    case LANGKAH_REVERSING_ANTI_JEPIT:return "REVERSING_ANTI_JEPIT";
    case LANGKAH_ANTI_JEPIT_WAIT:return "ANTI_JEPIT_WAIT";
    default:return "UNKNOWN";
  }
}

void setStateSafe(LangkahSistem s){
  if(stateMutex) xSemaphoreTake(stateMutex, portMAX_DELAY);
  langkahAktif = s;
  waktuLangkah = millis();
  if(stateMutex) xSemaphoreGive(stateMutex);
}

void setFaultSafe(const char* msg, FaultSeverity sev){
  if(faultMutex) xSemaphoreTake(faultMutex, portMAX_DELAY);
  lastError = msg;
  faultSeverity = sev;
  if(faultMutex) xSemaphoreGive(faultMutex);
}

void logEvent(const char* level, const char* message);
void transitionTo(LangkahSistem next);
void masukGangguan(const char* pesan, FaultSeverity sev = FAULT_CRITICAL);
void perbaruiKeluaran();

void snapshotSystem(SystemSnapshot& s){
  if(stateMutex) xSemaphoreTake(stateMutex, portMAX_DELAY);
  if(faultMutex) xSemaphoreTake(faultMutex, portMAX_DELAY);
  strncpy(s.state, stateName(langkahAktif).c_str(), sizeof(s.state));
  s.state[sizeof(s.state)-1] = 0;
  strncpy(s.fault, lastError.c_str(), sizeof(s.fault));
  s.fault[sizeof(s.fault)-1] = 0;
  strncpy(s.resetReason, lastResetReason, sizeof(s.resetReason));
  s.resetReason[sizeof(s.resetReason)-1] = 0;
  s.logDropped = logDropped;
  s.cmdDropped = cmdDropped;
  s.emergency = emergencyLatch;
  s.current = currentReading;
  s.retry = antiJepitRetry;
  s.severity = faultSeverity;
  if(faultMutex) xSemaphoreGive(faultMutex);
  if(stateMutex) xSemaphoreGive(stateMutex);
  s.freeHeap = esp_get_free_heap_size();
  s.largestBlock = heap_caps_get_largest_free_block(MALLOC_CAP_8BIT);
}

void sendStatusToWs(){
  SystemSnapshot s;
  snapshotSystem(s);
  StaticJsonDocument<384> doc;
  doc["state"] = s.state;
  doc["current"] = s.current;
  doc["emergency"] = s.emergency;
  doc["fault"] = s.fault;
  doc["reset_reason"] = s.resetReason;
  doc["retry"] = s.retry;
  doc["log_dropped"] = s.logDropped;
  doc["cmd_dropped"] = s.cmdDropped;
  doc["free_heap"] = s.freeHeap;
  doc["largest_block"] = s.largestBlock;
  doc["severity"] = (int)s.severity;
  String out;
  serializeJson(doc, out);
  ws.textAll(out);
}

void onWsEvent(AsyncWebSocket *server, AsyncWebSocketClient *client, AwsEventType type, void *arg, uint8_t *data, size_t len){
  if(type == WS_EVT_CONNECT){
    client->text("{"hello":true}");
    sendStatusToWs();
  } else if(type == WS_EVT_DATA){
    char msg[96];
    size_t n = len < sizeof(msg) - 1 ? len : sizeof(msg) - 1;
    memcpy(msg, data, n);
    msg[n] = 0;
    for(size_t i = 0; i < n; i++) if(msg[i] >= 'A' && msg[i] <= 'Z') msg[i] = msg[i] - 'A' + 'a';
    Command cmd;
    bool ok = true;
    if(strstr(msg, "buka")) cmd.type = CMD_BUKA;
    else if(strstr(msg, "tutup")) cmd.type = CMD_TUTUP;
    else if(strstr(msg, "stop")) cmd.type = CMD_STOP;
    else if(strstr(msg, "reset")) cmd.type = CMD_RESET;
    else if(strstr(msg, "emergency")) cmd.type = CMD_EMERGENCY;
    else ok = false;
    if(ok){
      if(xQueueSend(commandQueue, &cmd, 0) != pdPASS) cmdDropped++;
    }
  }
}

void setResetReason(){
  esp_reset_reason_t r = esp_reset_reason();
  switch(r){
    case ESP_RST_POWERON: strncpy(lastResetReason, "POWERON_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_EXT: strncpy(lastResetReason, "EXT_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_SW: strncpy(lastResetReason, "SW_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_PANIC: strncpy(lastResetReason, "PANIC_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_INT_WDT: strncpy(lastResetReason, "INT_WDT_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_TASK_WDT: strncpy(lastResetReason, "TASK_WDT_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_WDT: strncpy(lastResetReason, "OTHER_WDT_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_BROWNOUT: strncpy(lastResetReason, "BROWNOUT_RESET", sizeof(lastResetReason)); break;
    case ESP_RST_SDIO: strncpy(lastResetReason, "SDIO_RESET", sizeof(lastResetReason)); break;
    default: strncpy(lastResetReason, "UNKNOWN_RESET", sizeof(lastResetReason)); break;
  }
  lastResetReason[sizeof(lastResetReason)-1] = 0;
}

String aesGcmEncrypt(const char* plain) {
  mbedtls_gcm_context gcm;
  mbedtls_gcm_init(&gcm);
  uint8_t iv[12];
  for(int i=0;i<12;i++) iv[i] = esp_random() & 0xFF;
  uint8_t output[256] = {0};
  uint8_t tag[16] = {0};
  size_t plen = strlen(plain);
  if(plen > 192){ mbedtls_gcm_free(&gcm); return ""; }
  mbedtls_gcm_setkey(&gcm, MBEDTLS_CIPHER_ID_AES, AES_KEY, 128);
  if(mbedtls_gcm_crypt_and_tag(&gcm, MBEDTLS_GCM_ENCRYPT, plen, iv, 12, NULL, 0, (const uint8_t*)plain, output, 16, tag) != 0){
    mbedtls_gcm_free(&gcm);
    return "";
  }
  mbedtls_gcm_free(&gcm);
  char result[512];
  size_t pos = 0;
  for(int i=0;i<12;i++) pos += snprintf(result + pos, sizeof(result) - pos, "%02X", iv[i]);
  for(size_t i=0;i<plen;i++) pos += snprintf(result + pos, sizeof(result) - pos, "%02X", output[i]);
  for(int i=0;i<16;i++) pos += snprintf(result + pos, sizeof(result) - pos, "%02X", tag[i]);
  result[sizeof(result)-1] = 0;
  return String(result);
}

String aesGcmDecrypt(const char* hex) {
  size_t hlen = strlen(hex);
  if(hlen < 56 || (hlen % 2) != 0) return "";
  mbedtls_gcm_context gcm;
  mbedtls_gcm_init(&gcm);
  uint8_t iv[12], tag[16], input[256] = {0}, output[256] = {0};
  for(int i=0;i<12;i++){
    char tmp[3] = {hex[i*2], hex[i*2+1], 0};
    sscanf(tmp, "%02hhX", &iv[i]);
  }
  size_t clen = (hlen - 56) / 2;
  if(clen > 192){ mbedtls_gcm_free(&gcm); return ""; }
  for(size_t i=0;i<clen;i++){
    char tmp[3] = {hex[24 + i*2], hex[24 + i*2 + 1], 0};
    sscanf(tmp, "%02hhX", &input[i]);
  }
  for(int i=0;i<16;i++){
    char tmp[3] = {hex[24 + clen*2 + i*2], hex[24 + clen*2 + i*2 + 1], 0};
    sscanf(tmp, "%02hhX", &tag[i]);
  }
  mbedtls_gcm_setkey(&gcm, MBEDTLS_CIPHER_ID_AES, AES_KEY, 128);
  int ret = mbedtls_gcm_auth_decrypt(&gcm, clen, iv, 12, NULL, 0, tag, 16, input, output);
  mbedtls_gcm_free(&gcm);
  if(ret != 0) return "";
  char result[128];
  size_t pos = 0;
  for(size_t i=0;i<clen && pos < sizeof(result)-1;i++){
    if(output[i] == 0) break;
    result[pos++] = (char)output[i];
  }
  result[pos] = 0;
  return String(result);
}

void loadPhoneNumber() {
  preferences.begin("gate", false);
  String enc = preferences.getString("phone_enc", "");
  if(enc.length() == 0){
    strncpy(phoneNumber, "6281234567890", sizeof(phoneNumber));
    phoneNumber[sizeof(phoneNumber)-1] = 0;
    String e = aesGcmEncrypt(phoneNumber);
    if(e.length()) preferences.putString("phone_enc", e);
  } else {
    String dec = aesGcmDecrypt(enc.c_str());
    if(dec.length() > 0){
      strncpy(phoneNumber, dec.c_str(), sizeof(phoneNumber));
      phoneNumber[sizeof(phoneNumber)-1] = 0;
    } else {
      strncpy(phoneNumber, "6281234567890", sizeof(phoneNumber));
      phoneNumber[sizeof(phoneNumber)-1] = 0;
    }
  }
  preferences.end();
}

void rotateLogIfNeeded(){
  if(!fsReady) return;
  File file = LittleFS.open("/gate.log", "r");
  if(!file) return;
  size_t size = file.size();
  file.close();
  if(size > 51200){
    LittleFS.remove("/gate.log.old");
    LittleFS.rename("/gate.log", "/gate.log.old");
    File nf = LittleFS.open("/gate.log", "w");
    if(nf) nf.close();
  }
}

void loggerTask(void *pvParameters) {
  esp_task_wdt_add(NULL);
  LogEntry entry;
  while(true){
    esp_task_wdt_reset();
    if(xQueueReceive(logQueue, &entry, pdMS_TO_TICKS(100)) == pdPASS && fsReady){
      File file = LittleFS.open("/gate.log", "a");
      if(file){
        time_t now;
        struct tm timeinfo;
        time(&now);
        localtime_r(&now, &timeinfo);
        char ts[32];
        strftime(ts, sizeof(ts), "%Y-%m-%d %H:%M:%S", &timeinfo);
        file.printf("[%s] %s
", ts, entry.message);
        file.close();
        rotateLogIfNeeded();
      }
    }
  }
}

void logEvent(const char* level, const char* message) {
  LogEntry entry;
  snprintf(entry.message, sizeof(entry.message), "%s | %s | I=%d | S=%d", level, message, currentReading, (int)langkahAktif);
  if(xQueueSend(logQueue, &entry, 0) != pdPASS) logDropped++;
}

void printHeapTelemetry(){
  char buf[180];
  snprintf(buf, sizeof(buf), "HEAP | free=%u | largest=%u | min=%u", 
           (unsigned)esp_get_free_heap_size(),
           (unsigned)heap_caps_get_largest_free_block(MALLOC_CAP_8BIT),
           (unsigned)esp_get_minimum_free_heap_size());
  logEvent("INFO", buf);
}

void beepPattern(int duration, int times = 1, int interval = 100) {
  buzzerEndTime = millis() + (duration * times) + (interval * (times - 1));
  buzzerState = true;
  digitalWrite(BUZZER_PIN, HIGH);
}

void updateBuzzer() {
  if(buzzerState && millis() >= buzzerEndTime){
    digitalWrite(BUZZER_PIN, LOW);
    buzzerState = false;
  }
}

void updateLEDStatus() {
  unsigned long now = millis();
  switch(ledPattern){
    case 1: digitalWrite(STATUS_LED, HIGH); break;
    case 2: if(now - lastLedToggle > 500){ digitalWrite(STATUS_LED, !digitalRead(STATUS_LED)); lastLedToggle = now; } break;
    case 3: if(now - lastLedToggle > 250){ digitalWrite(STATUS_LED, !digitalRead(STATUS_LED)); lastLedToggle = now; } break;
    case 4: if(now - lastLedToggle > 100){ digitalWrite(STATUS_LED, !digitalRead(STATUS_LED)); lastLedToggle = now; } break;
    default: digitalWrite(STATUS_LED, LOW);
  }
}

int bacaArusFiltered() {
  int mv = analogReadMilliVolts(CURRENT_SENSOR);
  int mA = abs((mv - 1650) * 3);
  currentBuffer[currentIndex] = mA;
  currentIndex = (currentIndex + 1) % CURRENT_FILTER_SAMPLES;
  long sum = 0;
  int maxv = 0, minv = 9999;
  for(int i=0;i<CURRENT_FILTER_SAMPLES;i++){
    sum += currentBuffer[i];
    if(currentBuffer[i] > maxv) maxv = currentBuffer[i];
    if(currentBuffer[i] < minv) minv = currentBuffer[i];
  }
  return (sum - maxv - minv) / (CURRENT_FILTER_SAMPLES - 2);
}

bool readDebounced(int pin, uint8_t idx) {
  bool raw = (digitalRead(pin) == LOW);
  unsigned long now = millis();
  if(raw != inputs[idx].raw){
    inputs[idx].raw = raw;
    inputs[idx].lastChange = now;
  }
  if((now - inputs[idx].lastChange) >= DEBOUNCE_DELAY) inputs[idx].stable = raw;
  return inputs[idx].stable;
}

void matikanMotor() {
  relaiBukaAktif = false;
  relaiTutupAktif = false;
  pwmMotor = 0;
  pendingPwm = 0;
}

void requestDirectionChange(bool toOpen, int pwm){
  matikanMotor();
  pendingOpen = toOpen;
  pendingClose = !toOpen;
  pendingPwm = pwm;
  directionDeadtimeUntil = millis() + DIRECTION_DEADTIME_MS;
}

void applyDirectionAfterDeadtime(){
  if(directionDeadtimeUntil == 0) return;
  if(millis() < directionDeadtimeUntil) return;
  relaiBukaAktif = pendingOpen;
  relaiTutupAktif = pendingClose;
  pwmMotor = pendingPwm;
  directionDeadtimeUntil = 0;
}

void gerakkanBuka(int pwm) {
  if(directionDeadtimeUntil == 0 && (!relaiBukaAktif || relaiTutupAktif)){
    requestDirectionChange(true, pwm);
    return;
  }
  relaiBukaAktif = true;
  relaiTutupAktif = false;
  pendingPwm = pwm;
  pwmMotor = pwm;
}

void gerakkanTutup(int pwm) {
  if(directionDeadtimeUntil == 0 && (!relaiTutupAktif || relaiBukaAktif)){
    requestDirectionChange(false, pwm);
    return;
  }
  relaiBukaAktif = false;
  relaiTutupAktif = true;
  pendingPwm = pwm;
  pwmMotor = pwm;
}

void perbaruiKeluaran() {
  applyDirectionAfterDeadtime();
  digitalWrite(RELAI_BUKA, relaiBukaAktif ? LOW : HIGH);
  digitalWrite(RELAI_TUTUP, relaiTutupAktif ? LOW : HIGH);
  ledcWrite(KANAL_PWM, pwmMotor);
}

void transitionTo(LangkahSistem next) {
  if(stateMutex) xSemaphoreTake(stateMutex, portMAX_DELAY);
  if(langkahAktif == next){
    if(stateMutex) xSemaphoreGive(stateMutex);
    return;
  }
  langkahAktif = next;
  waktuLangkah = millis();
  if(stateMutex) xSemaphoreGive(stateMutex);
  char buf[64];
  snprintf(buf, sizeof(buf), "State -> %s", stateName(next).c_str());
  logEvent("INFO", buf);
}

void masukGangguan(const char* pesan, FaultSeverity sev) {
  if(faultMutex) xSemaphoreTake(faultMutex, portMAX_DELAY);
  faultSeverity = sev;
  if(faultMutex) xSemaphoreGive(faultMutex);

  if(stateMutex) xSemaphoreTake(stateMutex, portMAX_DELAY);
  langkahAktif = LANGKAH_GANGGUAN;
  if(stateMutex) xSemaphoreGive(stateMutex);

  matikanMotor();
  lastError = pesan;
  emergencyLatch = true;
  antiJepitHandling = false;
  logEvent("ERROR", pesan);
  ledPattern = 4;
  beepPattern(100, 5, 80);

  preferences.begin("fault", false);
  preferences.putString("last_fault", pesan);
  preferences.putUInt("reboot_count", rebootCount);
  preferences.end();
}

void handleAntiJepit() {
  if(antiJepitHandling) return;
  if(langkahAktif == LANGKAH_ANTI_JEPIT_WAIT || langkahAktif == LANGKAH_REVERSING_ANTI_JEPIT) return;
  antiJepitHandling = true;
  matikanMotor();
  antiJepitRetry++;
  if(antiJepitRetry >= ANTI_JEPIT_RETRY_LIMIT){
    masukGangguan("ANTI-JEPIT RETRY EXCEEDED", FAULT_CRITICAL);
  } else {
    deadtimeTimer = millis();
    transitionTo(LANGKAH_ANTI_JEPIT_WAIT);
    logEvent("WARN", "Anti-Jepit - Deadtime then Reverse");
  }
}

void processCommand(CommandType type){
  switch(type){
    case CMD_BUKA:
      if(langkahAktif == LANGKAH_SIAGA || langkahAktif == LANGKAH_PAUSE_MANUAL) transitionTo(LANGKAH_BUKA_PELAN);
      break;
    case CMD_TUTUP:
      if(langkahAktif == LANGKAH_TERBUKA) transitionTo(LANGKAH_DEADTIME_BUKA);
      break;
    case CMD_STOP:
      matikanMotor();
      transitionTo(LANGKAH_PAUSE_MANUAL);
      break;
    case CMD_RESET:
      matikanMotor();
      perbaruiKeluaran();
      vTaskDelay(pdMS_TO_TICKS(20));
      emergencyLatch = false;
      if(faultMutex) xSemaphoreTake(faultMutex, portMAX_DELAY);
      lastError = "";
      faultSeverity = FAULT_NONE;
      if(faultMutex) xSemaphoreGive(faultMutex);
      antiJepitRetry = 0;
      antiJepitHandling = false;
      directionDeadtimeUntil = 0;
      transitionTo(LANGKAH_VALIDASI_AWAL);
      break;
    case CMD_EMERGENCY:
      masukGangguan("EMERGENCY VIA WEB", FAULT_CRITICAL);
      break;
  }
}

void prosesSekuens(bool tombolMulai, bool photocell, bool limitBuka, bool limitTutup) {
  unsigned long now = millis();
  currentReading = bacaArusFiltered();

  if(photocell){
    if(waktuPhotocellStart == 0) waktuPhotocellStart = now;
    if(now - waktuPhotocellStart > PHOTOCELL_STUCK_TIMEOUT) handleAntiJepit();
  } else {
    waktuPhotocellStart = 0;
  }

  if(currentReading > CURRENT_SPIKE_LIMIT) masukGangguan("OVERCURRENT", FAULT_CRITICAL);
  if(limitBuka && limitTutup) masukGangguan("LIMIT KONTRADIKSI", FAULT_CRITICAL);

  if(langkahAktif == LANGKAH_ANTI_JEPIT_WAIT){
    if(now - deadtimeTimer > DURASI_DEADTIME){
      antiJepitHandling = false;
      gerakkanBuka(PWM_PELAN);
      transitionTo(LANGKAH_REVERSING_ANTI_JEPIT);
    }
    return;
  }

  switch(langkahAktif){
    case LANGKAH_VALIDASI_AWAL: {
      bool lb = readDebounced(LIMIT_BUKA, 1);
      bool lt = readDebounced(LIMIT_TUTUP, 2);
      if(lb && !lt) transitionTo(LANGKAH_TERBUKA);
      else if(lt && !lb) transitionTo(LANGKAH_SIAGA);
      else transitionTo(LANGKAH_HOMING_INIT);
    } break;

    case LANGKAH_HOMING_INIT:
      matikanMotor();
      transitionTo(LANGKAH_HOMING_WAIT);
      break;

    case LANGKAH_HOMING_WAIT:
      matikanMotor();
      if(now - waktuLangkah > 500) transitionTo(LANGKAH_HOMING_RUNNING);
      break;

    case LANGKAH_HOMING_RUNNING:
      gerakkanTutup(PWM_PELAN);
      if(limitTutup) transitionTo(LANGKAH_SIAGA);
      if(now - waktuLangkah > HOMING_TIMEOUT) masukGangguan("HOMING TIMEOUT", FAULT_CRITICAL);
      break;

    case LANGKAH_SIAGA:
      matikanMotor();
      if(tombolMulai) transitionTo(LANGKAH_BUKA_PELAN);
      break;

    case LANGKAH_BUKA_PELAN:
      gerakkanBuka(PWM_PELAN);
      if(now - waktuLangkah > DURASI_RAMP) transitionTo(LANGKAH_BUKA_CEPAT);
      break;

    case LANGKAH_BUKA_CEPAT:
      gerakkanBuka(PWM_CEPAT);
      if(limitBuka) transitionTo(LANGKAH_BUKA_AKHIR);
      if(now - waktuLangkah > BATAS_WAKTU_BUKA) masukGangguan("TIMEOUT BUKA", FAULT_CRITICAL);
      break;

    case LANGKAH_BUKA_AKHIR:
      gerakkanBuka(PWM_PELAN);
      if(now - waktuLangkah > 800) transitionTo(LANGKAH_TERBUKA);
      break;

    case LANGKAH_TERBUKA:
      matikanMotor();
      if(now - waktuLangkah >= DURASI_TERBUKA) transitionTo(LANGKAH_DEADTIME_BUKA);
      break;

    case LANGKAH_DEADTIME_BUKA:
      matikanMotor();
      if(now - waktuLangkah > DURASI_DEADTIME) transitionTo(LANGKAH_TUTUP_PELAN);
      break;

    case LANGKAH_TUTUP_PELAN:
      gerakkanTutup(PWM_PELAN);
      if(now - waktuLangkah > DURASI_RAMP) transitionTo(LANGKAH_TUTUP_CEPAT);
      break;

    case LANGKAH_TUTUP_CEPAT:
      gerakkanTutup(PWM_CEPAT);
      if(limitTutup) transitionTo(LANGKAH_TUTUP_AKHIR);
      if(now - waktuLangkah > BATAS_WAKTU_TUTUP) masukGangguan("TIMEOUT TUTUP", FAULT_CRITICAL);
      if(photocell || currentReading > CURRENT_THRESHOLD) handleAntiJepit();
      break;

    case LANGKAH_TUTUP_AKHIR:
      gerakkanTutup(PWM_PELAN);
      if(now - waktuLangkah > 800) transitionTo(LANGKAH_SIAGA);
      break;

    case LANGKAH_PAUSE_MANUAL:
      matikanMotor();
      if(tombolMulai) transitionTo(LANGKAH_BUKA_PELAN);
      break;

    case LANGKAH_GANGGUAN:
      matikanMotor();
      break;

    case LANGKAH_REVERSING_ANTI_JEPIT:
      if(now - waktuLangkah > 1500){
        transitionTo(LANGKAH_TUTUP_PELAN);
        antiJepitRetry = 0;
      }
      break;

    default:
      break;
  }
}

void pushStatusIfNeeded(bool force = false){
  unsigned long now = millis();
  if(force || now - lastWsPush >= WS_INTERVAL){
    lastWsPush = now;
    sendStatusToWs();
  }
}

void realtimeTask(void *pvParameters) {
  esp_task_wdt_add(NULL);
  TickType_t xLastWakeTime = xTaskGetTickCount();
  const TickType_t xFrequency = pdMS_TO_TICKS(8);
  while(true){
    vTaskDelayUntil(&xLastWakeTime, xFrequency);
    esp_task_wdt_reset();
    updateBuzzer();
    updateLEDStatus();

    if(millis() < bootHoldUntil){
      matikanMotor();
      perbaruiKeluaran();
      continue;
    }

    bool tombolMulai = readDebounced(TOMBOL_MULAI, 3);
    bool photocell   = readDebounced(SENSOR_PHOTOCELL, 4);
    bool limitBuka   = readDebounced(LIMIT_BUKA, 1);
    bool limitTutup  = readDebounced(LIMIT_TUTUP, 2);
    bool darurat     = readDebounced(TOMBOL_DARURAT, 0);

    if(darurat) masukGangguan("TOMBOL DARURAT", FAULT_CRITICAL);

    Command cmd;
    if(xQueueReceive(commandQueue, &cmd, 0) == pdPASS){
      processCommand(cmd.type);
    }

    prosesSekuens(tombolMulai, photocell, limitBuka, limitTutup);
    perbaruiKeluaran();
    pushStatusIfNeeded();

    if(millis() - heapLogTimer >= HEAP_LOG_INTERVAL_MS){
      heapLogTimer = millis();
      printHeapTelemetry();
    }
  }
}

void wifiReconnectCheck(){
  if(WiFi.status() == WL_CONNECTED){
    wifiConnecting = false;
    wifiRetryCount = 0;
    return;
  }

  if(millis() < wifiBackoffUntil) return;
  if(millis() - lastWifiCheck < WIFI_CHECK_INTERVAL) return;

  lastWifiCheck = millis();
  WiFi.reconnect();
  wifiRetryCount++;
  wifiBackoffUntil = millis() + min((unsigned long)(WIFI_BACKOFF_BASE_MS * wifiRetryCount), WIFI_BACKOFF_MAX_MS);
}

void setup() {
  Serial.begin(115200);
  delay(500);

  stateMutex = xSemaphoreCreateMutex();
  faultMutex = xSemaphoreCreateMutex();
  telemetryMutex = xSemaphoreCreateMutex();

  setResetReason();

  pinMode(RELAI_BUKA, OUTPUT);
  pinMode(RELAI_TUTUP, OUTPUT);
  pinMode(STATUS_LED, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(RELAI_BUKA, HIGH);
  digitalWrite(RELAI_TUTUP, HIGH);
  digitalWrite(STATUS_LED, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  pinMode(TOMBOL_MULAI, INPUT_PULLUP);
  pinMode(SENSOR_PHOTOCELL, INPUT_PULLUP);
  pinMode(LIMIT_BUKA, INPUT_PULLUP);
  pinMode(LIMIT_TUTUP, INPUT_PULLUP);
  pinMode(TOMBOL_DARURAT, INPUT_PULLUP);

  analogReadResolution(12);
  analogSetPinAttenuation(CURRENT_SENSOR, ADC_11db);

  fsReady = LittleFS.begin(true);
  logQueue = xQueueCreate(48, sizeof(LogEntry));
  commandQueue = xQueueCreate(16, sizeof(Command));

  preferences.begin("fault", false);
  rebootCount = preferences.getUInt("reboot_count", 0) + 1;
  preferences.putUInt("reboot_count", rebootCount);
  lastError = preferences.getString("last_fault", "");
  preferences.end();

  if(fsReady) logEvent("INFO", "System Boot v4 Hardened");

  loadPhoneNumber();

  WiFi.mode(WIFI_STA);
  WiFi.setAutoReconnect(true);
  WiFi.persistent(true);

  WiFiManager wm;
  wm.setConfigPortalTimeout(240);
  bool forceConfig = (digitalRead(TOMBOL_DARURAT) == LOW);

  if(forceConfig){
    wm.startConfigPortal(AP_NAME);
  } else {
    if(!wm.autoConnect(AP_NAME)){
      ESP.restart();
    }
  }

  configTime(GMT_OFFSET_SEC, 0, NTP_SERVER);
  ntpSynced = true;

  ledcSetup(KANAL_PWM, FREKUENSI_PWM, RESOLUSI_PWM);
  ledcAttachPin(PWM_MOTOR, KANAL_PWM);

  ws.onEvent(onWsEvent);
  server.addHandler(&ws);

  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send_P(200, "text/html", dashboard_html);
  });

  server.on("/aksi", HTTP_GET, [](AsyncWebServerRequest *request){
    if(!request->authenticate(AUTH_USER, AUTH_PASS)) return request->requestAuthentication();
    if(request->hasParam("cmd")){
      String c = request->getParam("cmd")->value();
      c.toLowerCase();
      Command cmd;
      bool ok = true;
      if(c == "buka") cmd.type = CMD_BUKA;
      else if(c == "tutup") cmd.type = CMD_TUTUP;
      else if(c == "stop") cmd.type = CMD_STOP;
      else if(c == "reset") cmd.type = CMD_RESET;
      else if(c == "emergency") cmd.type = CMD_EMERGENCY;
      else ok = false;

      if(!ok){
        request->send(400, "text/plain", "BAD CMD");
        return;
      }

      if(xQueueSend(commandQueue, &cmd, 0) != pdPASS){
        cmdDropped++;
      }
      request->send(200, "text/plain", "OK");
    } else {
      request->send(400, "text/plain", "NO CMD");
    }
  });

  server.begin();

  esp_task_wdt_config_t wdt_config = {
    .timeout_ms = 8000,
    .idle_core_mask = (1 << 0) | (1 << 1),
    .trigger_panic = true
  };
  esp_task_wdt_init(&wdt_config);

  bootHoldUntil = millis() + BOOT_FAULT_HOLD_MS;
  xTaskCreatePinnedToCore(loggerTask, "Logger", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(realtimeTask, "Realtime", 8192, NULL, 4, NULL, 1);

  beepPattern(80, 2, 120);
  ledPattern = 1;
  transitionTo(LANGKAH_VALIDASI_AWAL);
  Serial.println("Sliding Gate v4 Hardened Ready");
}

void loop() {
  ws.cleanupClients();
  wifiReconnectCheck();
  vTaskDelay(pdMS_TO_TICKS(50));
}