// =====================================================================
// ESP32_LD2453.ino
//
// ESP32-S3 + Hi-Link HLK-LD2453 radar firmware.
// Ported from https://github.com/thanhtantran/ESP32-LD2450
//
// *** LD2453Parser's byte-level decoding is currently a STUB ***
// See the comment block at the top of LD2453Parser.h for why, and
// the README for how to help confirm the real protocol. Everything
// else in this sketch (Wi-Fi, REST API, WebSocket, zones, diagnostic
// printing) is fully functional today.
//
// Copy config.example.h to config.h and edit it before building.
// =====================================================================
#include <Arduino.h>
#include "config.h"
#include "LD2453Parser.h"
#include "ZoneManager.h"

#if !DIAGNOSTIC_MODE
#include "WebServerManager.h"
#endif

// ---------------------------------------------------------------
// Pick the HardwareSerial instance for the radar based on
// RADAR_UART_PORT in config.h. Serial0/USB is left free for the
// Serial Monitor.
// ---------------------------------------------------------------
#if RADAR_UART_PORT == 1
HardwareSerial &RadarSerial = Serial1;
#else
HardwareSerial &RadarSerial = Serial2;
#endif

LD2453Parser radar;
ZoneManager zoneManager;

#if OLED_ENABLED
OLEDDisplay oledDisplay(OLED_SDA_PIN, OLED_SCL_PIN, OLED_I2C_ADDR);
#endif

#if !DIAGNOSTIC_MODE
WebServerManager webServer(zoneManager);
#endif

void printDiagnostics(const RadarFrame &frame, const LD2453Stats &stats) {
    Serial.println();
    Serial.println(F("LD2453 connected"));
    Serial.print(F("UART: "));
    Serial.println(RADAR_BAUD_RATE);
    Serial.print(F("Frame length (last): "));
    Serial.println(stats.lastFrameLength);
    Serial.print(F("Parse errors: "));
    Serial.println(stats.parseErrors);
    Serial.print(F("Packets/sec: "));
    Serial.println(stats.packetsPerSecond, 1);
    Serial.print(F("Target count: "));
    Serial.println(frame.targetCount);

    for (uint8_t i = 0; i < 3; i++) {
        Serial.println();
        Serial.print(F("Target "));
        Serial.print(i + 1);
        Serial.println(':');

        const RadarTarget &t = frame.targets[i];
        if (!t.valid) {
            Serial.println(F("INVALID"));
            continue;
        }
        Serial.print(F("X: "));
        Serial.print(t.x_mm);
        Serial.println(F(" mm"));
        Serial.print(F("Y: "));
        Serial.print(t.y_mm);
        Serial.println(F(" mm"));
        Serial.print(F("Speed: "));
        Serial.println(t.speed);
    }
}

void setup() {
    Serial.begin(115200);
    delay(1500); // lets the Serial Monitor catch up without blocking the app loop later

    Serial.println(F("=== ESP32_LD2453 booting ==="));
    Serial.print(F("Node ID: "));
    Serial.println(NODE_ID);

#if OLED_ENABLED
    // Khởi tạo màn hình OLED
    Serial.println(F("[SETUP] Initializing OLED..."));
    bool oledOk = false;
    try {
        oledOk = oledDisplay.begin();
        if (oledOk) {
            oledDisplay.showBootScreen(NODE_ID);
            delay(2000); // Hiển thị màn hình boot 2 giây
        } else {
            Serial.println(F("[SETUP] OLED init failed, continuing without display"));
        }
    } catch (...) {
        Serial.println(F("[SETUP] OLED init exception, continuing without display"));
        oledOk = false;
    }
#endif

    Serial.println(F("[SETUP] Initializing radar..."));
    radar.begin(RadarSerial, RADAR_RX_PIN, RADAR_TX_PIN, RADAR_BAUD_RATE,
                NODE_ID, DEBUG_RADAR_RAW);

#if !DIAGNOSTIC_MODE
    #if OLED_ENABLED
        if (oledOk) oledDisplay.showWiFiConnecting(WIFI_SSID);
    #endif
    Serial.println(F("[SETUP] Starting WiFi..."));
    webServer.begin(WIFI_SSID, WIFI_PASSWORD);
    #if OLED_ENABLED
        if (oledOk) oledDisplay.showWiFiConnected(WiFi.localIP());
    #endif
#else
    Serial.println(F("DIAGNOSTIC_MODE=1: Wi-Fi/web server disabled. Serial-only output."));
    #if OLED_ENABLED
        if (oledOk) oledDisplay.showDiagnosticMode();
    #endif
#endif
    
    Serial.println(F("[SETUP] Setup complete!"));
}

void loop() {
    // Radar acquisition/parsing is non-blocking and runs every
    // iteration regardless of mode, independent of Wi-Fi state.
    bool gotFrame = radar.update();

    if (gotFrame) {
        const RadarFrame &frame = radar.getLastFrame();
        zoneManager.update(frame);

#if DIAGNOSTIC_MODE
        printDiagnostics(frame, radar.getStats());
        
        #if OLED_ENABLED
            oledDisplay.updateRadarInfo(frame, radar.getStats());
        #endif
#else
        webServer.broadcastFrame(frame);
        
        #if OLED_ENABLED
            // Hiển thị tóm tắt detection và zones
            oledDisplay.updateDetectionSummary(
                frame.targetCount,
                zoneManager.isZoneOccupied(0),
                zoneManager.isZoneOccupied(1),
                zoneManager.isZoneOccupied(2)
            );
        #endif
#endif
    }

#if DIAGNOSTIC_MODE
    // Print stats periodically even if no frame is parsed yet
    static uint32_t lastStatsReport = 0;
    uint32_t now = millis();
    if (now - lastStatsReport >= 5000) { // every 5 seconds
        const LD2453Stats &stats = radar.getStats();
        Serial.println(F("\n--- Periodic Status ---"));
        Serial.print(F("Bytes received: "));
        Serial.println(stats.bytesReceived);
        Serial.print(F("Frames parsed: "));
        Serial.println(stats.framesParsed);
        Serial.print(F("Parse errors: "));
        Serial.println(stats.parseErrors);
        Serial.print(F("Packets/sec: "));
        Serial.println(stats.packetsPerSecond, 1);
        lastStatsReport = now;
        
        #if OLED_ENABLED
            // Cập nhật OLED ngay cả khi không có frame
            oledDisplay.updateStats(stats);
        #endif
    }
#else
    webServer.loop();
#endif
}