/**
 * ================================================================
 *  Traffic Light Controller — SafeCheck Plugin Demo
 *  Platform : ESP32 (Wokwi Simulator)
 *  Framework: Arduino + FreeRTOS
 *
 *  SafeCheck Use Cases:
 *    UC-1 : Periodicity Check
 *    UC-2 : Execution Time Monitoring
 *    UC-3 : ISR-to-Handler Latency (type 'E' in Serial Monitor)
 *    UC-4 : Secured Queue with CRC
 *    UC-5 : Stack Usage Monitoring
 *
 *  Road A — Red: GPIO25  Yellow: GPIO26  Green: GPIO27
 *  Road B — Red: GPIO32  Yellow: GPIO33  Green: GPIO4
 * ================================================================
 */

#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

/* ================================================================
 *  ALL STRUCTS/TYPEDEFS — must be at the top for Arduino .ino
 * ================================================================ */

// UC-1: Periodicity check handle
typedef struct {
    const char*  taskName;
    uint32_t     expectedPeriodMs;
    uint32_t     toleranceMs;
    TickType_t   lastCheckInTick;
    bool         initialized;
    bool         firstCheckIn;
} SC_PeriodicityHandle_t;

// UC-4: Queue message with CRC
typedef struct {
    uint8_t  road;    // 0 = Road A, 1 = Road B
    uint8_t  state;   // 0 = Red, 1 = Yellow, 2 = Green
    uint32_t crc;
} TL_QueueMsg_t;

/* ================================================================
 *  PIN DEFINITIONS
 * ================================================================ */
#define PIN_A_RED     25
#define PIN_A_YELLOW  26
#define PIN_A_GREEN   27
#define PIN_B_RED     32
#define PIN_B_YELLOW  33
#define PIN_B_GREEN   4

/* ================================================================
 *  TIMING & THRESHOLDS
 * ================================================================ */
#define GREEN_MS         3000
#define YELLOW_MS        1000
#define CYCLE_MS         ((GREEN_MS + YELLOW_MS) * 2)
#define MONITOR_MS       2000
#define PERIOD_TOL_MS    500
#define MAX_EXEC_MS      (GREEN_MS + YELLOW_MS + 500)
#define MAX_LATENCY_MS   20
#define STACK_MIN_W      200

/* ================================================================
 *  GLOBAL HANDLES
 * ================================================================ */
SemaphoreHandle_t     xRoadA_Go;
SemaphoreHandle_t     xRoadB_Go;
QueueHandle_t         xStatusQueue;
TaskHandle_t          xHandle_RoadA;
TaskHandle_t          xHandle_RoadB;
TaskHandle_t          xHandle_Monitor;
TaskHandle_t          xHandle_Emergency;

SC_PeriodicityHandle_t xPeriod_RoadA;
SC_PeriodicityHandle_t xPeriod_Monitor;

static const char* stateLabel[] = { "RED", "YELLOW", "GREEN" };

/* ================================================================
 *  UC-1: PERIODICITY CHECK — Stub
 *  TODO: Replace with real SafeCheck vCheckPeriodicity() calls
 * ================================================================ */
void SC_vPeriodicityInit(SC_PeriodicityHandle_t* h, const char* name,
                         uint32_t periodMs, uint32_t tolMs)
{
    h->taskName         = name;
    h->expectedPeriodMs = periodMs;
    h->toleranceMs      = tolMs;
    h->lastCheckInTick  = 0;
    h->initialized      = true;
    h->firstCheckIn     = true;
    Serial.printf("[UC-1 INIT] '%s' | Period=%ums +/-%ums\n", name, periodMs, tolMs);
}

void SC_vPeriodicityCheckIn(SC_PeriodicityHandle_t* h)
{
    if (!h->initialized) return;
    TickType_t now = xTaskGetTickCount();

    if (!h->firstCheckIn) {
        uint32_t actualMs  = (uint32_t)((now - h->lastCheckInTick) * portTICK_PERIOD_MS);
        uint32_t minP      = h->expectedPeriodMs - h->toleranceMs;
        uint32_t maxP      = h->expectedPeriodMs + h->toleranceMs;
        if (actualMs < minP || actualMs > maxP) {
            Serial.printf("[UC-1 FAULT] '%s': Expected %u+/-%ums | Got %ums\n",
                          h->taskName, h->expectedPeriodMs, h->toleranceMs, actualMs);
        } else {
            Serial.printf("[UC-1 OK  ] '%s': Period=%ums (OK)\n", h->taskName, actualMs);
        }
    }
    h->lastCheckInTick = now;
    h->firstCheckIn    = false;
}

/* ================================================================
 *  UC-2: EXECUTION TIME MONITORING — Stub
 *  TODO: Replace with real SafeCheck vStartExecMon() / vStopExecMon()
 * ================================================================ */
static TickType_t _execStartTick = 0;

void SC_vExecTimeStart() {
    _execStartTick = xTaskGetTickCount();
}

void SC_vExecTimeStop(const char* label, uint32_t maxAllowedMs) {
    uint32_t elapsedMs = (uint32_t)((xTaskGetTickCount() - _execStartTick) * portTICK_PERIOD_MS);
    if (elapsedMs > maxAllowedMs) {
        Serial.printf("[UC-2 FAULT] '%s': %ums > max %ums\n", label, elapsedMs, maxAllowedMs);
    } else {
        Serial.printf("[UC-2 OK  ] '%s': Exec=%ums (max=%ums)\n", label, elapsedMs, maxAllowedMs);
    }
}

/* ================================================================
 *  UC-3: ISR-TO-HANDLER LATENCY — Stub
 *  Simulated: type 'E' in Serial Monitor to trigger emergency
 *  TODO: Replace with real GPIO ISR + SafeCheck latency check
 * ================================================================ */
volatile TickType_t _isrTimestamp  = 0;
volatile bool       _emergencyFlag = false;

void IRAM_ATTR SC_vISR_RecordTimestamp() {
    _isrTimestamp  = xTaskGetTickCountFromISR();
    _emergencyFlag = true;
}

void SC_vISR_CheckLatency(const char* label, uint32_t maxLatencyMs) {
    if (!_emergencyFlag) return;
    uint32_t latencyMs = (uint32_t)((xTaskGetTickCount() - _isrTimestamp) * portTICK_PERIOD_MS);
    _emergencyFlag = false;
    if (latencyMs > maxLatencyMs) {
        Serial.printf("[UC-3 FAULT] '%s': Latency %ums > max %ums\n", label, latencyMs, maxLatencyMs);
    } else {
        Serial.printf("[UC-3 OK  ] '%s': ISR latency=%ums (max=%ums)\n", label, latencyMs, maxLatencyMs);
    }
}

/* ================================================================
 *  UC-4: SECURED QUEUE WITH CRC — Stub
 *  TODO: Replace XOR stub with real CRC-32
 * ================================================================ */
uint32_t SC_u32CRC(const void* data, size_t len) {
    // Stub: XOR-based — replace with CRC-32 for real implementation
    const uint8_t* p   = (const uint8_t*)data;
    uint32_t       crc = 0xFFFFFFFF;
    for (size_t i = 0; i < len; i++) crc ^= p[i];
    return crc;
}

bool SC_bQueueSend(QueueHandle_t xQ, TL_QueueMsg_t* msg) {
    msg->crc = SC_u32CRC(msg, sizeof(TL_QueueMsg_t) - sizeof(uint32_t));
    return xQueueSend(xQ, msg, pdMS_TO_TICKS(10)) == pdTRUE;
}

bool SC_bQueueReceive(QueueHandle_t xQ, TL_QueueMsg_t* out) {
    if (xQueueReceive(xQ, out, pdMS_TO_TICKS(50)) != pdTRUE) return false;
    uint32_t expected = SC_u32CRC(out, sizeof(TL_QueueMsg_t) - sizeof(uint32_t));
    if (out->crc != expected) {
        Serial.printf("[UC-4 FAULT] CRC mismatch! Expected=0x%08X Got=0x%08X\n", expected, out->crc);
        return false;
    }
    return true;
}

/* ================================================================
 *  UC-5: STACK USAGE MONITORING
 *  TODO: Wire fault to SafeCheck error hook
 * ================================================================ */
void SC_vCheckStack(TaskHandle_t xTask, const char* name, UBaseType_t minWords) {
    UBaseType_t wm = uxTaskGetStackHighWaterMark(xTask);
    if (wm < minWords) {
        Serial.printf("[UC-5 WARN] '%s': Watermark=%u words < threshold=%u\n", name, wm, minWords);
    } else {
        Serial.printf("[UC-5 OK  ] '%s': Watermark=%u words (OK)\n", name, wm);
    }
}

/* ================================================================
 *  LED HELPERS (also send UC-4 queue message)
 * ================================================================ */
void setRoadA(uint8_t state) {
    digitalWrite(PIN_A_RED,    state == 0 ? HIGH : LOW);
    digitalWrite(PIN_A_YELLOW, state == 1 ? HIGH : LOW);
    digitalWrite(PIN_A_GREEN,  state == 2 ? HIGH : LOW);
    Serial.printf("  [Road A] --> %s\n", stateLabel[state]);
    TL_QueueMsg_t msg = { 0, state, 0 };
    SC_bQueueSend(xStatusQueue, &msg);
}

void setRoadB(uint8_t state) {
    digitalWrite(PIN_B_RED,    state == 0 ? HIGH : LOW);
    digitalWrite(PIN_B_YELLOW, state == 1 ? HIGH : LOW);
    digitalWrite(PIN_B_GREEN,  state == 2 ? HIGH : LOW);
    Serial.printf("  [Road B] --> %s\n", stateLabel[state]);
    TL_QueueMsg_t msg = { 1, state, 0 };
    SC_bQueueSend(xStatusQueue, &msg);
}

/* ================================================================
 *  TASK: ROAD A (Priority 2)
 * ================================================================ */
void vRoadA_Task(void* pvParameters) {
    SC_vPeriodicityInit(&xPeriod_RoadA, "RoadA", CYCLE_MS, PERIOD_TOL_MS);

    while (1) {
        xSemaphoreTake(xRoadA_Go, portMAX_DELAY);

        Serial.println("\n=== Road A: CYCLE START ===");
        SC_vPeriodicityCheckIn(&xPeriod_RoadA);
        SC_vExecTimeStart();

        setRoadA(2); vTaskDelay(pdMS_TO_TICKS(GREEN_MS));
        setRoadA(1); vTaskDelay(pdMS_TO_TICKS(YELLOW_MS));
        setRoadA(0);

        SC_vExecTimeStop("RoadA", MAX_EXEC_MS);
        Serial.println("=== Road A: handing to Road B ===");
        xSemaphoreGive(xRoadB_Go);
    }
}

/* ================================================================
 *  TASK: ROAD B (Priority 2)
 * ================================================================ */
void vRoadB_Task(void* pvParameters) {
    while (1) {
        xSemaphoreTake(xRoadB_Go, portMAX_DELAY);

        Serial.println("\n=== Road B: CYCLE START ===");
        setRoadB(2); vTaskDelay(pdMS_TO_TICKS(GREEN_MS));
        setRoadB(1); vTaskDelay(pdMS_TO_TICKS(YELLOW_MS));
        setRoadB(0);

        Serial.println("=== Road B: handing to Road A ===");
        xSemaphoreGive(xRoadA_Go);
    }
}

/* ================================================================
 *  TASK: SAFETY MONITOR (Priority 1, every 2s)
 * ================================================================ */
void vMonitor_Task(void* pvParameters) {
    SC_vPeriodicityInit(&xPeriod_Monitor, "Monitor", MONITOR_MS, 100);
    TickType_t xLastWake = xTaskGetTickCount();

    while (1) {
        SC_vPeriodicityCheckIn(&xPeriod_Monitor);
        Serial.println("\n--- SafeCheck Monitor ---");

        // UC-4: Drain and verify queue
        TL_QueueMsg_t rx;
        int n = 0;
        while (SC_bQueueReceive(xStatusQueue, &rx)) {
            Serial.printf("  [UC-4] Road %c -> %s | CRC OK\n",
                          rx.road == 0 ? 'A' : 'B', stateLabel[rx.state]);
            n++;
        }
        if (n == 0) Serial.println("  [UC-4] Queue empty");

        // UC-5: Stack watermarks
        SC_vCheckStack(xHandle_RoadA,    "RoadA",     STACK_MIN_W);
        SC_vCheckStack(xHandle_RoadB,    "RoadB",     STACK_MIN_W);
        SC_vCheckStack(xHandle_Monitor,  "Monitor",   STACK_MIN_W);
        SC_vCheckStack(xHandle_Emergency,"Emergency", STACK_MIN_W);

        Serial.printf("  [SYS] Tick=%lu | Heap=%u bytes\n",
                      (unsigned long)xTaskGetTickCount(),
                      (unsigned)esp_get_free_heap_size());

        vTaskDelayUntil(&xLastWake, pdMS_TO_TICKS(MONITOR_MS));
    }
}

/* ================================================================
 *  TASK: EMERGENCY HANDLER (Priority 3 — highest)
 *  Type 'E' + Enter in Serial Monitor to simulate emergency
 * ================================================================ */
void vEmergency_Task(void* pvParameters) {
    Serial.println("[Emergency] Ready. Type 'E' + Enter to simulate emergency vehicle.");
    while (1) {
        if (Serial.available()) {
            char c = (char)Serial.read();
            if (c == 'E' || c == 'e') {
                SC_vISR_RecordTimestamp();          // UC-3: record ISR time
                vTaskDelay(pdMS_TO_TICKS(5));
                SC_vISR_CheckLatency("Emergency", MAX_LATENCY_MS);  // UC-3: check latency

                Serial.println("\n*** EMERGENCY VEHICLE! All roads RED for 3s ***");
                setRoadA(0);
                setRoadB(0);
                vTaskDelay(pdMS_TO_TICKS(3000));
                Serial.println("*** Emergency cleared. Resuming. ***");
                xSemaphoreGive(xRoadA_Go);
            }
        }
        vTaskDelay(pdMS_TO_TICKS(50));
    }
}

/* ================================================================
 *  SETUP
 * ================================================================ */
void setup() {
    Serial.begin(115200);
    delay(500);
    Serial.println("\n================================================");
    Serial.println("  Traffic Light + SafeCheck | ESP32 FreeRTOS");
    Serial.println("  UC-1 Periodicity  UC-2 ExecTime  UC-3 ISR");
    Serial.println("  UC-4 SecuredQueue UC-5 StackMon");
    Serial.println("  Type 'E' to trigger emergency vehicle");
    Serial.println("================================================\n");

    // Create queue and semaphores FIRST (before any setRoadA/setRoadB calls)
    xRoadA_Go    = xSemaphoreCreateBinary();
    xRoadB_Go    = xSemaphoreCreateBinary();
    xStatusQueue = xQueueCreate(20, sizeof(TL_QueueMsg_t));

    // GPIO22 = permanent LOW = virtual GND for LED cathodes
    pinMode(22, OUTPUT); digitalWrite(22, LOW);

    // Configure GPIO pins explicitly
    pinMode(PIN_A_RED,    OUTPUT); digitalWrite(PIN_A_RED,    LOW);
    pinMode(PIN_A_YELLOW, OUTPUT); digitalWrite(PIN_A_YELLOW, LOW);
    pinMode(PIN_A_GREEN,  OUTPUT); digitalWrite(PIN_A_GREEN,  LOW);
    pinMode(PIN_B_RED,    OUTPUT); digitalWrite(PIN_B_RED,    LOW);
    pinMode(PIN_B_YELLOW, OUTPUT); digitalWrite(PIN_B_YELLOW, LOW);
    pinMode(PIN_B_GREEN,  OUTPUT); digitalWrite(PIN_B_GREEN,  LOW);

    // Now safe to call setRoad (queue exists)
    setRoadA(0); setRoadB(0);

    xSemaphoreGive(xRoadA_Go);  // Road A goes first

    xTaskCreatePinnedToCore(vRoadA_Task,     "RoadA",     4096, NULL, 2, &xHandle_RoadA,     1);
    xTaskCreatePinnedToCore(vRoadB_Task,     "RoadB",     4096, NULL, 2, &xHandle_RoadB,     1);
    xTaskCreatePinnedToCore(vMonitor_Task,   "Monitor",   4096, NULL, 1, &xHandle_Monitor,   1);
    xTaskCreatePinnedToCore(vEmergency_Task, "Emergency", 2048, NULL, 3, &xHandle_Emergency, 1);

    Serial.println("[Setup] Tasks started!\n");
}

void loop() {
    vTaskDelay(portMAX_DELAY);
}