/*
 * IoT SmartBin — ESP32 DevKit (version in FIRMWARE_VERSION)
 *
 * v7.0 FIX:
 *   - Sharp IR: handle non-linear close zone (<10cm voltage DROP)
 *     → Peak tracking: if voltage was high then drops, object is CLOSER not farther
 *     → mv > 2400 = very close (force 3cm)
 *     → mv > 2000 = close zone (linear interpolation 3-10cm)
 *   - HC-SR04: retry every 60s even if marked "dead"
 *   - 5 samples US (was 3), 11 samples IR (was 9)
 *   - Both sensors: if both valid, use SMALLER distance (= more full)
 *   - Hardware watchdog 30s
 *
 * Wiring:
 *   HC-SR04:  VCC→3V3 TRIG→D5   ECHO→D18  GND→GND
 *   Sharp IR: VCC→5V  GND→GND   Vo→D34
 *   Traffic Light: R→D25  Y→D26  G→D27  VCC→3.3V
 *   Blue LED: GPIO2 (heartbeat)
 */

#include <Arduino.h>
#include <WiFi.h>
#include <HTTPClient.h>
#include <ArduinoJson.h>
#include "config.h"
#include "soc/rtc_cntl_reg.h"
#include "esp_task_wdt.h"

// ==================== PIN ====================
#define PIN_BLUE  2

// ==================== LED ====================
static bool g_ledActiveLow = (LED_ACTIVE_LOW != 0);

static void ledON(int pin)  { digitalWrite(pin, g_ledActiveLow ? LOW : HIGH); }
static void ledOFF(int pin) { digitalWrite(pin, g_ledActiveLow ? HIGH : LOW); }

static void allLedOff() {
  ledOFF(PIN_LED_RED);
  ledOFF(PIN_LED_YELLOW);
  ledOFF(PIN_LED_GREEN);
}

static void setTrafficLight(const char* status) {
  allLedOff();
  if      (strcmp(status, "FULL") == 0)   ledON(PIN_LED_RED);
  else if (strcmp(status, "MEDIUM") == 0) ledON(PIN_LED_YELLOW);
  else                                    ledON(PIN_LED_GREEN);
}

// ==================== STATE ====================
static float  g_us_cm       = -1.0f;
static float  g_ir_mv       = 0.0f;
static float  g_ir_cm       = -1.0f;
static float  g_distance    = -1.0f;
static int    g_fill        = 0;
static char   g_status[10]  = "EMPTY";
static char   g_led_mode    = 'a';
static int    g_raw_adc     = 0;

static int    g_us_fail     = 0;
static bool   g_us_fault    = false;
static int    g_us_timeouts = 0;
static int    g_us_stuck    = 0;
static int    g_us_range    = 0;
static int    g_http_ok     = 0;
static int    g_http_fail   = 0;
static int    g_sensor_fail = 0;
static bool   g_sensor_ok   = false;
static bool   g_us_ok       = false;
static bool   g_us_ever_valid = false;
static bool   g_ir_ok       = false;
static bool   g_fallback_active = false;
static bool   g_ir_clipped   = false;
static bool   g_ir_near      = false;
static bool   g_sensors_agree = true;

// IR peak tracking for non-linear close zone
static float  g_ir_peak_mv  = 0.0f;
static float  g_ir_baseline = 0.0f;   // Adapts to "empty" voltage
static bool   g_ir_baseline_set = false;
static int    g_close_latch = 0;      // Latch: hold close-zone for N cycles
static float  g_close_cm    = 4.0f;   // Last close-zone distance
static float  g_ir_empty_mv = 0.0f;   // Runtime baseline captured at boot
static float  g_fill_smooth = -1.0f;

static unsigned long tSensor = 0;
static unsigned long tSend   = 0;
static unsigned long tDebug  = 0;
static unsigned long tBlink  = 0;
static bool blueLedState = false;

// ==================== SORT ====================
static void sortFloat(float a[], int n) {
  for (int i = 0; i < n-1; i++)
    for (int j = i+1; j < n; j++)
      if (a[j] < a[i]) { float t=a[i]; a[i]=a[j]; a[j]=t; }
}

// ==================== HC-SR04 ====================
enum PingError {
  PING_OK = 0,
  PING_ECHO_STUCK_HIGH,
  PING_TIMEOUT,
  PING_OUT_OF_RANGE
};

static const char* pingErrorName(PingError error) {
  switch (error) {
    case PING_ECHO_STUCK_HIGH: return "ECHO_STUCK_HIGH";
    case PING_TIMEOUT:         return "NO_ECHO_TIMEOUT";
    case PING_OUT_OF_RANGE:    return "OUT_OF_RANGE";
    case PING_OK:              return "OK";
    default:                   return "UNKNOWN_ERROR";
  }
}

static float pingOnce(PingError* error = nullptr, unsigned long* pulseUs = nullptr) {
  if (error) *error = PING_OK;
  if (pulseUs) *pulseUs = 0;

  // ECHO harus LOW sebelum trigger. HIGH terus-menerus menandakan
  // wiring/sensor bermasalah, jadi jangan membuat pulsa palsu.
  if (digitalRead(PIN_ECHO) == HIGH) {
    const unsigned long waitStart = micros();
    while (digitalRead(PIN_ECHO) == HIGH &&
           micros() - waitStart < US_ECHO_CLEAR_US) {
      delayMicroseconds(10);
    }
    if (digitalRead(PIN_ECHO) == HIGH) {
      if (error) *error = PING_ECHO_STUCK_HIGH;
      return -1.0f;
    }
  }

  // Standard HC-SR04 trigger sequence
  digitalWrite(PIN_TRIG, LOW);  delayMicroseconds(5);
  digitalWrite(PIN_TRIG, HIGH); delayMicroseconds(15);
  digitalWrite(PIN_TRIG, LOW);

  const unsigned long dur = pulseIn(PIN_ECHO, HIGH, US_TIMEOUT_US);
  if (pulseUs) *pulseUs = dur;
  if (dur == 0) {
    if (error) *error = digitalRead(PIN_ECHO) == HIGH
      ? PING_ECHO_STUCK_HIGH : PING_TIMEOUT;
    return -1.0f;
  }

  float cm = dur * 0.0343f / 2.0f;
  if (cm < US_MIN_CM || cm > US_MAX_CM) {
    if (error) *error = PING_OUT_OF_RANGE;
    return -1.0f;
  }
  return cm;
}

static float readUS() {
  float valid[US_SAMPLES];
  int validCount = 0;
  int cycleTimeouts = 0;
  int cycleStuck = 0;
  int cycleRange = 0;

  for (int i = 0; i < US_SAMPLES; i++) {
    PingError error;
    const float cm = pingOnce(&error);
    if (cm > 0.0f) {
      valid[validCount++] = cm;
    } else if (error == PING_ECHO_STUCK_HIGH) {
      cycleStuck++;
    } else if (error == PING_TIMEOUT) {
      cycleTimeouts++;
    } else if (error == PING_OUT_OF_RANGE) {
      cycleRange++;
    }
    // Termasuk setelah ping terakhir agar siklus berikutnya tidak langsung
    // men-trigger HC-SR04 tanpa masa recovery.
    delay(US_PING_GAP_MS);
    yield();
  }

  g_us_timeouts += cycleTimeouts;
  g_us_stuck += cycleStuck;
  g_us_range += cycleRange;

  if (validCount < US_MIN_VALID_READS) {
    if (g_us_fail < 20) g_us_fail++;
    if (g_us_fail >= SENSOR_FAIL_LIMIT && !g_us_fault) {
      g_us_fault = true;
      Serial.printf(
        "!!! HC-SR04 FAIL: valid=%d/%d timeout=%d stuckHigh=%d outRange=%d echoIdle=%d\n",
        validCount, US_SAMPLES, g_us_timeouts, g_us_stuck, g_us_range,
        digitalRead(PIN_ECHO));
    }
    return -1.0f;
  }

  if (g_us_fault) {
    Serial.println("[HC-SR04] RECOVERED - valid echo received");
  }
  g_us_fail = 0;
  g_us_fault = false;
  g_us_timeouts = 0;
  g_us_stuck = 0;
  g_us_range = 0;
  sortFloat(valid, validCount);
  return valid[validCount / 2];
}

// ==================== SHARP IR GP2Y0A21 ====================
/*
 * DATASHEET NON-LINEAR BEHAVIOR:
 *
 *   Distance | Voltage (approx)
 *   ---------|------------------
 *    80 cm   |  ~400 mV
 *    40 cm   |  ~750 mV
 *    20 cm   | ~1500 mV
 *    15 cm   | ~1800 mV
 *    10 cm   | ~2200 mV   ← PEAK
 *     6 cm   | ~2600 mV   ← absolute max
 *     4 cm   | ~2300 mV   ← voltage DROPS!
 *     2 cm   | ~1200 mV   ← same as 25cm!
 *
 * Problem: Below ~8cm, voltage drops back — same voltage as far objects.
 * Solution: Track peak voltage. If peak was high (>1800mV) and current drops,
 * the object is getting CLOSER not farther.
 */

static float readIR_mV(int* rawOut) {
  const int N = IR_SAMPLES;
  float valid[N];
  int vc = 0;
  int sumRaw = 0;
  for (int i = 0; i < N; i++) {
    int raw = analogRead(PIN_IR_ANALOG);
    sumRaw += raw;
    float mv = (float)analogReadMilliVolts(PIN_IR_ANALOG);
    if (mv > 40.0f) valid[vc++] = mv;
    delayMicroseconds(500);
  }
  *rawOut = (N > 0) ? sumRaw / N : 0;
  if (vc < 3) return 0.0f;
  sortFloat(valid, vc);
  return valid[vc / 2];
}

// Convert IR millivolts to distance
// GP2Y0A21 pada ESP32 ADC: baseline ~640mV (jauh), peak ~1100-1200mV (10-15cm)
static float irFormulaCm(float mv) {
  if (mv < 250.0f || mv > 2800.0f) return -1.0f;
  float volts = mv / 1000.0f;
  float cm = 29.988f * powf(volts, -1.173f);
  if (cm < 10.0f || cm > 80.0f) return -1.0f;
  return cm;
}

/*
 * smartIR — handle Sharp GP2Y0A21 non-linear close zone
 *
 * DATA REAL dari sensor kamu:
 *   Kosong (jauh):    ~640 mV  → baseline
 *   Mendekat:         ~900 mV  → object approaching
 *   Peak (10-15cm):  ~1163 mV  → closest before voltage drops
 *   Ditempel (<5cm):  ~609 mV  → voltage DROPS BACK (dead zone!)
 *
 * Strategi:
 *   1. Track baseline (voltage saat tidak ada objek)
 *   2. Track peak (voltage tertinggi baru-baru ini)
 *   3. Jika peak >> baseline DAN sekarang turun → objek sangat dekat
 */
static float smartIR(float mv) {
  if (mv <= 0) {
    g_ir_peak_mv *= 0.8f;
    return -1.0f;
  }

  // === BASELINE TRACKING ===
  // Baseline = voltage saat bin kosong, adaptif
  if (!g_ir_baseline_set) {
    g_ir_baseline = mv;
    g_ir_baseline_set = true;
  }
  // Jika mv stabil dan rendah (dekat baseline), slowly adapt
  if (mv < g_ir_baseline * 1.10f) {
    // Readings dekat baseline → update perlahan
    g_ir_baseline = g_ir_baseline * 0.97f + mv * 0.03f;
  }
  // Jangan biarkan baseline naik terlalu tinggi
  if (g_ir_baseline > 800.0f) g_ir_baseline = 800.0f;
  if (g_ir_baseline < 200.0f) g_ir_baseline = 200.0f;

  // === PEAK TRACKING ===
  if (mv > g_ir_peak_mv) {
    g_ir_peak_mv = mv;
  } else {
    // Decay: slower when latched (to keep detection), faster otherwise
    float decay = (g_close_latch > 0) ? 0.95f : 0.90f;
    g_ir_peak_mv = g_ir_peak_mv * decay + mv * (1.0f - decay);
  }

  // === RISE RATIO: seberapa tinggi peak dari baseline ===
  float riseRatio = (g_ir_baseline > 100.0f) ? g_ir_peak_mv / g_ir_baseline : 1.0f;

  // === CASE 1: CLOSE ZONE DETECTION ===
  // Peak naik >25% di atas baseline DAN sekarang voltage turun >20% dari peak
  // Ini artinya objek melewati titik peak dan sekarang SANGAT DEKAT
  if (riseRatio > 1.25f && mv < g_ir_peak_mv * 0.80f) {
    // Seberapa dalam drop-nya dari peak
    float dropFromPeak = 1.0f - (mv / g_ir_peak_mv);  // 0.2 - 0.7
    // Makin besar drop → makin dekat
    // drop 0.2 → ~6cm, drop 0.5+ → ~2cm
    float cm = 6.0f - dropFromPeak * 8.0f;
    cm = constrain(cm, 2.0f, 6.0f);
    g_close_latch = 30;  // Hold close-zone for 30 cycles (~15s)
    g_close_cm = cm;     // Remember distance
    Serial.printf("[IR-CLOSE] base=%.0f peak=%.0f now=%.0f rise=%.2f drop=%.0f%% → %.1fcm LATCH=30\n",
      g_ir_baseline, g_ir_peak_mv, mv, riseRatio, dropFromPeak*100, cm);
    return cm;
  }

  // === CASE 1b: LATCHED CLOSE ZONE ===
  // Jika close-zone baru terdeteksi dan voltage masih rendah (dekat baseline),
  // objek kemungkinan masih sangat dekat → pertahankan status FULL
  if (g_close_latch > 0) {
    if (mv < g_ir_baseline * 1.20f) {
      // Voltage masih rendah → objek masih di dead zone
      g_close_latch--;
      Serial.printf("[IR-LATCH] mv=%.0f base=%.0f → %.1fcm (latch=%d)\n",
        mv, g_ir_baseline, g_close_cm, g_close_latch);
      return g_close_cm;
    } else {
      // Voltage naik → objek menjauh dari dead zone, release latch
      g_close_latch = 0;
      Serial.printf("[IR-LATCH-RELEASE] mv=%.0f > base*1.2=%.0f\n", mv, g_ir_baseline * 1.20f);
    }
  }

  // === CASE 2: VOLTAGE RISING (normal approach) ===
  // Standard inverse formula works here
  float cm = irFormulaCm(mv);

  // Jika voltage lebih tinggi dari baseline (objek mendekat), formula normal OK
  if (mv > g_ir_baseline * 1.15f && cm > 0) {
    return cm;
  }

  // === CASE 3: NEAR BASELINE (no object or far) ===
  return cm;
}

// ==================== FILL & STATUS ====================
static int calcFill(float cm) {
  const float span = EMPTY_DISTANCE_CM - FULL_DISTANCE_CM;
  if (span <= 0.0f || cm < 0.0f) return -1;
  const float fill = (EMPTY_DISTANCE_CM - cm) * 100.0f / span;
  return constrain((int)roundf(fill), 0, 100);
}

static void calcStatusStable(int fill, char* out) {
  if (fill < 0) {
    strcpy(out, "EMPTY");
    return;
  }

  if (strcmp(g_status, "FULL") == 0) {
    if (fill >= STATUS_FULL_OFF) strcpy(out, "FULL");
    else if (fill >= STATUS_MEDIUM_OFF) strcpy(out, "MEDIUM");
    else strcpy(out, "EMPTY");
    return;
  }

  if (strcmp(g_status, "MEDIUM") == 0) {
    if (fill >= STATUS_FULL_ON) strcpy(out, "FULL");
    else if (fill >= STATUS_MEDIUM_OFF) strcpy(out, "MEDIUM");
    else strcpy(out, "EMPTY");
    return;
  }

  if (fill >= STATUS_FULL_ON) strcpy(out, "FULL");
  else if (fill >= STATUS_MEDIUM_ON) strcpy(out, "MEDIUM");
  else strcpy(out, "EMPTY");
}

// ==================== WiFi ====================
static void connectWiFi() {
  Serial.printf("[WiFi] → '%s'...\n", WIFI_SSID);
  WiFi.disconnect(false);
  delay(100);
  WiFi.mode(WIFI_STA);
  WiFi.setAutoReconnect(true);
  WiFi.persistent(true);
  WiFi.setSleep(false);

  // === STATIC IP (PERMANENT) ===
#if USE_STATIC_IP
  IPAddress ip(STATIC_IP);
  IPAddress gw(STATIC_GATEWAY);
  IPAddress sn(STATIC_SUBNET);
  IPAddress dns(STATIC_DNS);
  if (!WiFi.config(ip, gw, sn, dns)) {
    Serial.println("[WiFi] Static IP config GAGAL!");
  } else {
    Serial.printf("[WiFi] Static IP: %s  GW: %s\n", ip.toString().c_str(), gw.toString().c_str());
  }
#else
  Serial.println("[WiFi] DHCP mode");
#endif

  WiFi.begin(WIFI_SSID, WIFI_PASS);
  int n = 0;
  while (WiFi.status() != WL_CONNECTED && n < 20) {
    delay(500); Serial.print("."); n++;
    digitalWrite(PIN_BLUE, n % 2);
  }
  if (WiFi.status() == WL_CONNECTED)
    Serial.printf("\n[WiFi] OK IP=%s RSSI=%d\n", WiFi.localIP().toString().c_str(), WiFi.RSSI());
  else
    Serial.println("\n[WiFi] GAGAL");
}

// ==================== HTTP POST ====================
static void sendData() {
  if (WiFi.status() != WL_CONNECTED) { g_http_fail++; return; }
  esp_task_wdt_reset();

  HTTPClient http;
  char url[128];
  snprintf(url, sizeof(url), "http://%s:%d%s", SERVER_HOST, SERVER_PORT, INGEST_PATH);
  http.begin(url);
  http.addHeader("Content-Type", "application/json");
  http.setTimeout(5000);

  JsonDocument doc;
  doc["ts"]           = (unsigned long)(millis() / 1000);
  doc["tenantId"]     = TENANT_ID;
  doc["siteId"]       = SITE_ID;
  doc["deviceId"]     = DEVICE_ID;
  doc["firmware"]     = FIRMWARE_VERSION;
  doc["us_cm"]        = g_us_cm > 0 ? roundf(g_us_cm * 10.0f) / 10.0f : -1;
  doc["ir_cm"]        = g_ir_cm > 0 ? roundf(g_ir_cm * 10.0f) / 10.0f : -1;
  doc["ir_mv"]        = roundf(g_ir_mv);
  doc["distance_cm"]  = g_distance > 0 ? roundf(g_distance * 10.0f) / 10.0f : -1;
  doc["fill_percent"] = g_fill;
  doc["status"]       = g_status;
  doc["rssi"]         = WiFi.RSSI();
  doc["lat"]          = DEVICE_LAT;
  doc["lon"]          = DEVICE_LON;
  doc["sensor_ok"]    = g_sensor_ok;
  doc["us_ok"]        = g_us_ok;
  doc["ir_ok"]        = g_ir_ok;
  doc["fallback_active"] = g_fallback_active;
  const bool usWarmingUp = !g_us_ever_valid && millis() < SENSOR_STARTUP_GRACE_MS;
  doc["sensor_health"] = usWarmingUp ? "WARMING_UP"
                          : (g_sensor_ok ? "NORMAL" : ((g_us_ok || g_ir_ok) ? "DEGRADED" : "FAILED"));
  doc["measurement_valid"] = g_us_ok || g_ir_ok;
  doc["ir_clipped"] = g_ir_clipped;
  doc["ir_near"] = g_ir_near;
  doc["sensors_agree"] = g_sensors_agree;

  String body;
  serializeJson(doc, body);
  Serial.printf("[SEND] %s\n", body.c_str());

  int code = http.POST(body);
  if (code >= 200 && code < 300) {
    g_http_ok++;
  } else {
    g_http_fail++;
    String resp = http.getString();
    Serial.printf("[HTTP] FAIL code=%d resp=%s\n", code, resp.c_str());
  }
  http.end();
}

// ==================== SERIAL CMD ====================
static void testLEDs();
static void testSensors();

static void printHelp() {
  Serial.println("\n=== COMMANDS ===");
  Serial.println("h = help");
  Serial.println("s = run sensor test now");
  Serial.println("t = run LED test now");
  Serial.println("p = toggle LED polarity");
  Serial.println("w = reconnect WiFi");
  Serial.println("r/y/g = force red/yellow/green LED");
  Serial.println("a = LED auto mode");
  Serial.println("0 = LEDs off\n");
}

static void handleSerialCommand() {
  while (Serial.available()) {
    char c = Serial.read();
    switch(c) {
      case 'h': case 'H': printHelp(); break;
      case 's': case 'S': testSensors(); break;
      case 't': case 'T': testLEDs(); break;
      case 'p': case 'P':
        g_ledActiveLow = !g_ledActiveLow;
        Serial.printf(">>> LED polarity: %s\n", g_ledActiveLow ? "COMMON ANODE / LOW=ON" : "COMMON CATHODE / HIGH=ON");
        testLEDs();
        break;
      case 'w': case 'W': connectWiFi(); break;
      case 'r': case 'R': g_led_mode='r'; allLedOff(); ledON(PIN_LED_RED);    Serial.println(">>> MERAH"); break;
      case 'y': case 'Y': g_led_mode='y'; allLedOff(); ledON(PIN_LED_YELLOW); Serial.println(">>> KUNING"); break;
      case 'g': case 'G': g_led_mode='g'; allLedOff(); ledON(PIN_LED_GREEN);  Serial.println(">>> HIJAU"); break;
      case 'a': case 'A': g_led_mode='a'; Serial.println(">>> AUTO"); break;
      case '0':           g_led_mode='0'; allLedOff(); Serial.println(">>> OFF"); break;
    }
  }
}

// ==================== BOOT TESTS ====================
static void testLEDs() {
  Serial.println("\n=== LED TEST ===");
  allLedOff(); delay(300);
  ledON(PIN_LED_RED);
  Serial.printf("[LED] RED R=%d Y=%d G=%d\n", digitalRead(PIN_LED_RED), digitalRead(PIN_LED_YELLOW), digitalRead(PIN_LED_GREEN));
  delay(700);
  allLedOff(); ledON(PIN_LED_YELLOW);
  Serial.printf("[LED] YEL R=%d Y=%d G=%d\n", digitalRead(PIN_LED_RED), digitalRead(PIN_LED_YELLOW), digitalRead(PIN_LED_GREEN));
  delay(700);
  allLedOff(); ledON(PIN_LED_GREEN);
  Serial.printf("[LED] GRN R=%d Y=%d G=%d\n", digitalRead(PIN_LED_RED), digitalRead(PIN_LED_YELLOW), digitalRead(PIN_LED_GREEN));
  delay(700);
  allLedOff();
  Serial.println("=== LED DONE ===\n");
}

static void testSensors() {
  Serial.println("=== SENSOR TEST ===");

  // HC-SR04
  Serial.printf("[US] HC-SR04 3V3 TRIG=D%d ECHO=D%d idle=%d\n", PIN_TRIG, PIN_ECHO, digitalRead(PIN_ECHO));
  for (int i = 0; i < 5; i++) {
    PingError error;
    unsigned long pulseUs = 0;
    float cm = pingOnce(&error, &pulseUs);
    if (cm > 0.0f) {
      g_us_cm = cm;
      g_us_ever_valid = true;
      Serial.printf("[US] #%d: %.1f cm (OK, pulse=%luus)\n", i + 1, cm, pulseUs);
    } else {
      Serial.printf("[US] #%d: %s (pulse=%luus idle=%d)\n",
                    i + 1, pingErrorName(error), pulseUs, digitalRead(PIN_ECHO));
    }
    delay(US_PING_GAP_MS);
  }

  // Sharp IR
  Serial.printf("[IR] GPIO%d — 10 samples:\n", PIN_IR_ANALOG);
  for (int i = 0; i < 10; i++) {
    int raw = analogRead(PIN_IR_ANALOG);
    float mv = (float)analogReadMilliVolts(PIN_IR_ANALOG);
    float basic = irFormulaCm(mv);
    float smart = smartIR(mv);
    Serial.printf("[IR] #%02d ADC=%d mv=%.0f basic=%.1f smart=%.1f peak=%.0f\n",
      i+1, raw, mv, basic, smart, g_ir_peak_mv);
    delay(80);
  }
  Serial.println("=== SENSOR DONE ===\n");
}

// ==================== SETUP ====================
void setup() {
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
  Serial.begin(115200);
  delay(500);

  esp_task_wdt_init(30, true);
  esp_task_wdt_add(NULL);

  Serial.println("\n################################################");
  Serial.printf("#  SmartBin firmware %-10s                 #\n", FIRMWARE_VERSION);
  Serial.println("################################################");
  Serial.printf("Device: %s  FW: %s\n", DEVICE_ID, FIRMWARE_VERSION);
  Serial.printf("Server: %s:%d\n", SERVER_HOST, SERVER_PORT);
  Serial.printf("Pins: HC-SR04 3V3 TRIG=D%d ECHO=D%d IR=D%d\n", PIN_TRIG, PIN_ECHO, PIN_IR_ANALOG);
  Serial.printf("LED: R=%d Y=%d G=%d (%s)\n",
    PIN_LED_RED, PIN_LED_YELLOW, PIN_LED_GREEN,
    g_ledActiveLow ? "Anode LOW=ON" : "Cathode HIGH=ON");
  Serial.printf("Bin: %.0fcm  FULL>=%d%%  MED>=%d%%\n\n",
    BIN_DEPTH_CM, THRESHOLD_FULL, THRESHOLD_MEDIUM);

  pinMode(PIN_TRIG, OUTPUT);
  digitalWrite(PIN_TRIG, LOW);
  // HC-SR04 diberi 3V3, sehingga ECHO tersambung langsung ke GPIO18/D18.
  pinMode(PIN_ECHO, INPUT_PULLDOWN);
  pinMode(PIN_LED_RED, OUTPUT);
  pinMode(PIN_LED_YELLOW, OUTPUT);
  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_BLUE, OUTPUT);

  analogReadResolution(12);
  analogSetAttenuation(ADC_11db);

  allLedOff();
  digitalWrite(PIN_BLUE, LOW);

  testLEDs();
  testSensors();
  connectWiFi();

  strcpy(g_status, "EMPTY");
  setTrafficLight(g_status);
  Serial.println("[INIT] READY! WDT=30s Sensor=500ms Send=2s");
  Serial.printf("[INIT] LED polarity: %s\n", g_ledActiveLow ? "COMMON ANODE / LOW=ON" : "COMMON CATHODE / HIGH=ON");
  printHelp();
}

// ==================== LOOP ====================
void loop() {
  unsigned long now = millis();

  esp_task_wdt_reset();
  handleSerialCommand();

  // Blue heartbeat
  if (now - tBlink >= 1000) {
    tBlink = now;
    blueLedState = !blueLedState;
    digitalWrite(PIN_BLUE, blueLedState);
  }

  // WiFi reconnect
  if (WiFi.status() != WL_CONNECTED) {
    static unsigned long lastRetry = 0;
    if (now - lastRetry > 10000) { lastRetry = now; connectWiFi(); }
  }

  // === SENSOR + STATUS + LED (500ms) ===
  if (now - tSensor >= SENSOR_READ_INTERVAL) {
    // 1. Ultrasonic
    const float usReading = readUS();
    if (usReading >= US_MIN_CM && usReading <= US_MAX_CM) {
      g_us_cm = usReading;
      g_us_ever_valid = true;
    } else if (g_us_fail >= SENSOR_FAIL_LIMIT) {
      // Jangan mengubah HC menjadi NO_ECHO hanya karena satu siklus ping
      // terganggu. Pertahankan nilai valid terakhir selama masa toleransi;
      // status gagal baru diterbitkan setelah beberapa siklus berturut-turut.
      g_us_cm = -1.0f;
    }

    // 2. IR
    int rawAdc = 0;
    g_ir_mv = readIR_mV(&rawAdc);
    g_raw_adc = rawAdc;
    // GP2Y0A21 mencapai tegangan maksimum pada batas dekat spesifikasinya.
    // Saat ADC clipped kita hanya tahu jarak <=10 cm, bukan jarak presisi.
    // Tetap gunakan sebagai fallback FULL, tetapi laporkan ir_clipped=true.
    const bool irAdcSaturated = rawAdc >= 4080;
    g_ir_clipped = irAdcSaturated && g_ir_mv >= 2400.0f;
    g_ir_cm = g_ir_clipped ? FULL_DISTANCE_CM
                           : ((g_ir_mv > 0) ? smartIR(g_ir_mv) : -1.0f);
    // GP2Y0A21 tidak menghasilkan jarak presisi di bawah 10 cm. Nilai ini
    // tetap berguna sebagai indikator objek sangat dekat / konfirmasi FULL.
    g_ir_near = g_ir_clipped || (g_ir_cm >= 2.0f && g_ir_cm < IR_MIN_DISTANCE_CM);

    if (g_ir_empty_mv < IR_MIN_VALID_MV && g_ir_mv >= IR_MIN_VALID_MV) {
      g_ir_empty_mv = g_ir_mv;
      Serial.printf("[IR-CAL] Empty baseline captured: %.0f mV\n", g_ir_empty_mv);
    }

    // 3. Pick fill candidate
    g_distance = -1.0f;

    // Gunakan seluruh rentang valid fisik HC-SR04. Jarak di bawah ambang
    // FULL bukan error: itu berarti permukaan sampah sangat dekat (100%).
    // Sebelumnya batas FULL_DISTANCE_CM - 2 (8cm) membuang bacaan 4-6cm,
    // sehingga sistem selalu jatuh ke IR dan dashboard tertahan sekitar 75%.
    bool usValid = (g_us_cm >= US_MIN_CM && g_us_cm <= US_MAX_CM);
    bool irDistanceValid = (g_ir_cm >= IR_MIN_DISTANCE_CM && g_ir_cm <= IR_MAX_DISTANCE_CM);
    bool irValid = irDistanceValid || g_ir_near;
    g_sensors_agree = !(usValid && irValid)
      || (g_ir_near ? g_us_cm <= (FULL_DISTANCE_CM + SENSOR_AGREEMENT_CM)
                   : fabsf(g_us_cm - g_ir_cm) <= SENSOR_AGREEMENT_CM);
    g_us_ok = usValid;
    g_ir_ok = irValid;
    g_fallback_active = !usValid && irValid;
    // Kesehatan berarti sensor merespons dalam rentang operasional. Kesamaan
    // hasil adalah diagnostik kalibrasi terpisah (`sensors_agree`) dan tidak
    // boleh membuat sensor yang hidup dilaporkan rusak.
    g_sensor_ok = usValid && irValid;
    int irFill = g_ir_near ? 100 : (irDistanceValid ? calcFill(g_ir_cm) : -1);
    int rawFill = -1;

    // HC-SR04 adalah sumber utama. Sharp IR hanya dipakai sebagai fallback
    // ketika ultrasonic tidak menghasilkan pembacaan yang valid.
    if (usValid) {
      g_distance = g_us_cm;
      rawFill = calcFill(g_distance);
    } else if (irValid) {
      // Saat HC gagal, hitung kapasitas dari jarak Sharp IR yang nyata.
      // Deviasi tegangan terhadap baseline dapat jenuh 100% walaupun hasil
      // konversi jarak masih 20-30 cm, sehingga tidak boleh menjadi sumber
      // utama fill_percent.
      g_distance = g_ir_cm;
      rawFill = irFill;
    }

    if (rawFill >= 0) {
      g_sensor_fail = 0;
      if (g_fill_smooth < 0) g_fill_smooth = rawFill;
      else g_fill_smooth = g_fill_smooth * (1.0f - SMOOTH_ALPHA) + rawFill * SMOOTH_ALPHA;
      g_fill = constrain((int)roundf(g_fill_smooth), 0, 100);
    } else {
      g_sensor_fail++;
      Serial.printf("[SENSOR] INVALID us=%.1f ir=%.1f mv=%.0f fail=%d; keep fill=%d%% (STALE)\n",
                    g_us_cm, g_ir_cm, g_ir_mv, g_sensor_fail, g_fill);
    }

    // 5. Status
    char newStatus[10];
    calcStatusStable(g_fill, newStatus);
    if (strcmp(newStatus, g_status) != 0) {
      Serial.printf("\n>>> STATUS: %s → %s (fill=%d%% raw=%d%% dist=%.1f us=%.1f ir=%.1f irFill=%d)\n\n",
        g_status, newStatus, g_fill, rawFill, g_distance, g_us_cm, g_ir_cm, irFill);
      strcpy(g_status, newStatus);
    }

    // 6. LED
    if (g_led_mode == 'a') setTrafficLight(g_status);
    // Jadwalkan interval dari akhir pekerjaan sensor, bukan dari awal. Tanpa
    // ini, waktu blocking readUS() membuat siklus berikutnya langsung mulai.
    tSensor = millis();
  }

  // === DEBUG (2s) ===
  if (now - tDebug >= 2000) {
    tDebug = now;
    Serial.printf("[FW %s] up=%lus US=%.1f(%s) IR:adc=%d mv=%.0f empty=%.0f cm=%.1f → dist=%.1f fill=%d%% %s http=%d/%d\n",
      FIRMWARE_VERSION,
      millis()/1000,
      g_us_cm, g_us_fault ? "FAULT" : (g_us_cm > 0 ? "OK" : "NO_ECHO"),
      g_raw_adc, g_ir_mv, g_ir_empty_mv, g_ir_cm,
      g_distance, g_fill, g_status,
      g_http_ok, g_http_fail);
    Serial.printf("     LED: R(%d)=%d Y(%d)=%d G(%d)=%d wifi=%s\n",
      PIN_LED_RED, digitalRead(PIN_LED_RED),
      PIN_LED_YELLOW, digitalRead(PIN_LED_YELLOW),
      PIN_LED_GREEN, digitalRead(PIN_LED_GREEN),
      WiFi.status() == WL_CONNECTED ? "OK" : "DOWN");
  }

  // === TELEMETRY (2s) ===
  if (now - tSend >= TELEMETRY_INTERVAL_MS) {
    tSend = now;
    sendData();
  }
}