#include "Arduino.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "freertos/event_groups.h"
#include "esp_timer.h"
#include <string.h>
#include <ctype.h>
#include <math.h>

// ============================================================================
// STUDENT CONFIGURATION
// ============================================================================
#define STUDENT_NUMBER 2291611

// ============================================================================
// SYSTEM PARAMETERS - AUTO-GENERATED FROM STUDENT_NUMBER
// ============================================================================
struct SystemParams {
    float v1, v2, v3;
    float d0, d1, d2, d3, d4;
    float d5, d6;
    float L_min, L_max, L_threshold;
    float W_min, W_max, W_threshold;
    float T_gate;
    int C1, C2, C3;
    int productInterval;
};

SystemParams sp;

#define DIST_LEAD   10.0f
#define DEBUG_PIN   12

enum class ProductEventType {
    OBSTRUCTION_SENSOR_RISE,
    OBSTRUCTION_SENSOR_FALL,
    G2_CHECK,
    WEIGHT_SENSOR_ENTER_WINDOW,
    WEIGHT_SENSOR_EXIT_WINDOW,
    G3_CHECK,
    BARCODE_SENSOR_ENTER_WINDOW,
    BARCODE_SENSOR_EXIT_WINDOW,
    G4_CHECK,
    ARRIVE_DESTINATION,
    FINISH
};

struct Product {
    float length;
    float weight;
    int   barcodeID;
};

struct ProductEvent {
    uint64_t         triggerTimeMicros;
    ProductEventType eventType;
    int              gateExpected;
    int              destArea;
};

struct ProductScheduler {
    Product product;
    ProductEvent events[16];
    int totalEvents;
    int currentEventIndex;
    int debugPin;
    hw_timer_t* timerHandle;
};

volatile int errorGateCNT[3] = {0};
volatile int simAreaCount[3] = {0, 0, 0};

extern void startSimulator(void);

extern volatile int currentProductIndex;
extern Product productArray[];
extern int numProducts;

static uint32_t paramHash(uint32_t seed, uint32_t idx) {
    uint32_t v = seed * 2654435761u + idx * 2246822519u;
    v ^= v >> 16;
    v *= 0x45d9f3bu;
    v ^= v >> 16;
    return v;
}

static float paramRangeF(uint32_t seed, uint32_t idx, float lo, float hi) {
    return lo + (float)(paramHash(seed, idx) % 10001u) / 10000.0f * (hi - lo);
}

static int paramRangeI(uint32_t seed, uint32_t idx, int lo, int hi) {
    return lo + (int)(paramHash(seed, idx) % (uint32_t)(hi - lo + 1));
}

void generateParameters(uint32_t sn) {
    if (sn == 0) {
        sp.v1 = 100.0f;  sp.v2 = 120.0f;  sp.v3 = 80.0f;
        sp.d0 = 15.0f;   sp.d1 = 25.0f;   sp.d2 = 35.0f;
        sp.d3 = 45.0f;   sp.d4 = 55.0f;
        sp.d5 = 40.0f;   sp.d6 = 40.0f;
        sp.L_min = 4.5f;  sp.L_max = 5.5f;  sp.L_threshold = 5.0f;
        sp.W_min = 90.0f;  sp.W_max = 110.0f;  sp.W_threshold = 100.0f;
        sp.T_gate = 3.0f;
        sp.C1 = 5;  sp.C2 = 10;  sp.C3 = 10;
        sp.productInterval = 500;
        return;
    }

    sp.v1 = paramRangeF(sn, 1, 80.0f, 120.0f);
    sp.v2 = paramRangeF(sn, 2, 100.0f, 150.0f);
    sp.v3 = paramRangeF(sn, 3, 60.0f, 100.0f);

    sp.d0 = paramRangeF(sn, 4, 12.0f, 20.0f);
    sp.d1 = sp.d0 + paramRangeF(sn, 5, 8.0f, 15.0f);
    sp.d2 = sp.d1 + paramRangeF(sn, 6, 8.0f, 15.0f);
    sp.d3 = sp.d2 + paramRangeF(sn, 7, 8.0f, 15.0f);
    sp.d4 = sp.d3 + paramRangeF(sn, 8, 8.0f, 15.0f);
    sp.d5 = paramRangeF(sn, 9, 30.0f, 50.0f);
    sp.d6 = paramRangeF(sn, 10, 30.0f, 50.0f);

    sp.L_min = paramRangeF(sn, 11, 4.0f, 5.0f);
    sp.L_max = sp.L_min + paramRangeF(sn, 12, 0.8f, 1.5f);
    sp.L_threshold = (sp.L_min + sp.L_max) / 2.0f;

    sp.W_min = paramRangeF(sn, 13, 80.0f, 100.0f);
    sp.W_max = sp.W_min + paramRangeF(sn, 14, 15.0f, 30.0f);
    sp.W_threshold = (sp.W_min + sp.W_max) / 2.0f;

    sp.T_gate = paramRangeF(sn, 15, 2.0f, 5.0f);

    sp.C1 = paramRangeI(sn, 16, 3, 8);
    sp.C2 = paramRangeI(sn, 17, 8, 15);
    sp.C3 = paramRangeI(sn, 18, 8, 15);

    sp.productInterval = paramRangeI(sn, 19, 400, 600);
}

void printParameters() {
    Serial.println("========== SmartSort System Parameters ==========");
    Serial.printf("Student Number: %u\n", (unsigned)STUDENT_NUMBER);
    Serial.println("----- Conveyor Speeds -----");
    Serial.printf("  v1 (Conv1): %.1f cm/s\n", sp.v1);
    Serial.printf("  v2 (Conv2): %.1f cm/s\n", sp.v2);
    Serial.printf("  v3 (Conv3): %.1f cm/s\n", sp.v3);
    Serial.println("----- Distances (from obstruction sensor, cm) -----");
    Serial.printf("  d0 (to G2):      %.1f\n", sp.d0);
    Serial.printf("  d1 (to Weight):  %.1f\n", sp.d1);
    Serial.printf("  d2 (to G3):      %.1f\n", sp.d2);
    Serial.printf("  d3 (to Barcode): %.1f\n", sp.d3);
    Serial.printf("  d4 (to G4):      %.1f\n", sp.d4);
    Serial.printf("  d5 (Conv2 len):  %.1f\n", sp.d5);
    Serial.printf("  d6 (Conv3 len):  %.1f\n", sp.d6);
    Serial.println("----- Length Criteria (cm) -----");
    Serial.printf("  L_min=%.2f  L_max=%.2f  L_threshold=%.2f\n", sp.L_min, sp.L_max, sp.L_threshold);
    Serial.println("----- Weight Criteria (g) -----");
    Serial.printf("  W_min=%.2f  W_max=%.2f  W_threshold=%.2f\n", sp.W_min, sp.W_max, sp.W_threshold);
    Serial.println("----- Timing & Capacity -----");
    Serial.printf("  T_gate=%.1f ms  Interval=%d ms\n", sp.T_gate, sp.productInterval);
    Serial.printf("  C1=%d  C2=%d  C3=%d\n", sp.C1, sp.C2, sp.C3);
    Serial.println("=================================================");
}

#define MAX_TEST_PRODUCTS 20
Product productArray[MAX_TEST_PRODUCTS];
int numProducts = 0;

void generateProductArray() {
    int idx = 0;
    float Lmid = (sp.L_min + sp.L_max) / 2.0f;
    float Wmid = (sp.W_min + sp.W_max) / 2.0f;

    productArray[idx++] = {sp.L_min - 0.5f, Wmid, 1001};
    productArray[idx++] = {sp.L_min - 1.0f, Wmid + 3.0f, 1002};
    productArray[idx++] = {sp.L_max + 0.5f, Wmid, 1003};
    productArray[idx++] = {sp.L_max + 0.8f, Wmid - 2.0f, 1004};
    productArray[idx++] = {Lmid, sp.W_min - 5.0f, 1005};
    productArray[idx++] = {sp.L_min + 0.1f, sp.W_min - 3.0f, 1006};
    productArray[idx++] = {Lmid, sp.W_max + 5.0f, 1007};
    productArray[idx++] = {sp.L_max - 0.1f, sp.W_max + 3.0f, 1008};
    productArray[idx++] = {sp.L_min + 0.1f, sp.W_min + 1.0f, 1009};
    productArray[idx++] = {sp.L_threshold, sp.W_threshold, 1010};
    productArray[idx++] = {sp.L_threshold - 0.1f, sp.W_threshold - 1.0f, 1011};
    productArray[idx++] = {sp.L_threshold + 0.1f, sp.W_threshold - 1.0f, 1012};
    productArray[idx++] = {sp.L_min + 0.2f, sp.W_threshold + 1.0f, 1013};
    productArray[idx++] = {sp.L_max - 0.1f, sp.W_max - 1.0f, 1014};
    productArray[idx++] = {sp.L_threshold + 0.2f, sp.W_threshold + 2.0f, 1015};
    productArray[idx++] = {sp.L_min, sp.W_min, 1016};
    productArray[idx++] = {sp.L_max, sp.W_max, 1017};
    productArray[idx++] = {Lmid, Wmid, 1018};

    numProducts = idx;
}

/*!!!!!!!!!!!!!!!!!!ANY CODE ABOVE THIS LINE SHOULD NOT BE MODIFIED BY STUDENTS!!!!!!!!!!!!!!!!!!!*/
/*=========================== INTERFACE BETWEEN SIMULATOR AND STUDENT CODE ============================*/

#define g_obstructionSensorPin    32
#define gate2DebugPin             33
#define gate3DebugPin             27
#define gate4DebugPin             14

volatile bool   g_obstructionSensor = false;
volatile int    g_barcodeReader     = 0;
volatile float  g_weightSensor      = 0.0f;

volatile bool   g_gate1Ctrl = true;
volatile bool   g_gate2Ctrl = false;
volatile bool   g_gate3Ctrl = false;
volatile bool   g_gate4Ctrl = false;

/*================================= IMPLEMENT YOUR RTOS CODE BELOW ===================================*/

typedef struct {
    int64_t riseEdgeUs;
    int64_t fallEdgeUs;
} product_timestamp_t;

typedef struct {
    bool     used;
    bool     delivered;
    float    measuredLength;
    float    measuredWeight;
    int      barcodeID;
    char     tn[12];
    bool     sizeAbnormal;
    bool     weightAbnormal;
    bool     weightDone;
    bool     barcodeDone;
    bool     classified;
    int      destArea;      // 0=A1, 1=A2, 2=A3, -1=unknown
    int      sourceIndex;   // simulator sequence index
    bool     trueSizeAbnormal;
    int64_t  riseUs;
    int64_t  fallUs;
    int64_t  tG2CmdUs;
    int64_t  tWeightUs;
    int64_t  tG3CmdUs;
    int64_t  tBarcodeUs;
    int64_t  tG4CmdUs;
    int64_t  tArriveUs;
    bool     g2Issued;
    bool     g3Issued;
    bool     g4Issued;
} tracked_product_t;

typedef struct {
    uint16_t area1;
    uint16_t area2;
    uint16_t area3;
    uint16_t inflightA1;
    uint16_t inflightA2;
    uint16_t inflightA3;
    uint16_t pending;
} snapshot_t;

// ----------------------------------------------------------------------------
// Globals
// ----------------------------------------------------------------------------
static QueueHandle_t      g_edgeQueue = NULL;
static SemaphoreHandle_t  g_stateMutex = NULL;
static EventGroupHandle_t g_flags = NULL;
static volatile product_timestamp_t g_currentTimestamp = {0, 0};

static const EventBits_t FLAG_EMERGENCY = (1u << 0);

static tracked_product_t g_products[32];
static snapshot_t        g_snap;

static uint16_t g_area1 = 0;
static uint16_t g_area2 = 0;
static uint16_t g_area3 = 0;

static int64_t g_gate2HoldUntilUs = 0;
static int64_t g_gate3HoldUntilUs = 0;
static int64_t g_gate4HoldUntilUs = 0;
static bool    g_gate4RouteA3 = false;

static int g_detectedSequence = 0;

// sortSTAT stats
static double   g_sumL_all = 0.0;
static double   g_sumW_all = 0.0;
static uint32_t g_n_all    = 0;
static double   g_sumL_A2  = 0.0;
static double   g_sumW_A2  = 0.0;
static uint32_t g_n_A2     = 0;
static double   g_sumL_A3  = 0.0;
static double   g_sumW_A3  = 0.0;
static uint32_t g_n_A3     = 0;

// ----------------------------------------------------------------------------
// Helpers
// ----------------------------------------------------------------------------
static inline int64_t nowUs() { return esp_timer_get_time(); }
static inline bool emergencyLatched() { return (xEventGroupGetBits(g_flags) & FLAG_EMERGENCY) != 0; }
static inline void setEmergencyLatched() { xEventGroupSetBits(g_flags, FLAG_EMERGENCY); }
static inline void clearEmergencyLatched() { xEventGroupClearBits(g_flags, FLAG_EMERGENCY); }
static inline float round1(float x) { return roundf(x * 10.0f) / 10.0f; }
static inline double round2(double x) { return round(x * 100.0) / 100.0; }

static inline int64_t cmToUs(float distanceCm, float speedCmPerS) {
    if (distanceCm <= 0.0f || speedCmPerS <= 0.0f) return 0;
    return (int64_t)((distanceCm / speedCmPerS) * 1000000.0f);
}

static void barcodeToTrackingText(int barcode, char *out, size_t outLen) {
    if (barcode <= 0) {
        strncpy(out, "0", outLen);
        out[outLen - 1] = '\0';
    } else {
        snprintf(out, outLen, "TN%03d", barcode % 1000);
    }
}

static const char* areaToText(int area) {
    switch (area) {
        case 0: return "A1";
        case 1: return "A2";
        case 2: return "A3";
        default: return "A?";
    }
}

static int classifyProduct(float L, float W, bool weightKnown) {
    if (L < sp.L_min || L > sp.L_max) return 0;
    if (weightKnown && (W < sp.W_min || W > sp.W_max)) return 0;

    if (weightKnown) {
        if ((L <= sp.L_threshold) && (W <= sp.W_threshold)) return 1;
        return 2;
    }

    return -1;
}

static int predictProductAreaByIndex(int idx) {
    if (idx < 0 || idx >= numProducts) return -1;

    const Product& p = productArray[idx];

    bool sizeOk   = (p.length >= sp.L_min && p.length <= sp.L_max);
    bool weightOk = (p.weight >= sp.W_min && p.weight <= sp.W_max);

    if (!sizeOk) return 0;
    if (!weightOk) return 0;
    if ((p.length <= sp.L_threshold) && (p.weight <= sp.W_threshold)) return 1;
    return 2;
}

static int findNextAdmissibleArea(uint32_t occA1, uint32_t occA2, uint32_t occA3) {
    if (currentProductIndex < 0 || currentProductIndex >= numProducts) return -1;

    for (int idx = currentProductIndex; idx < numProducts; ++idx) {
        int area = predictProductAreaByIndex(idx);
        if (area == 0 && occA1 < (uint32_t)sp.C1) return 0;
        if (area == 1 && occA2 < (uint32_t)sp.C2) return 1;
        if (area == 2 && occA3 < (uint32_t)sp.C3) return 2;
    }
    return -1;
}

static void clearStats() {
    g_sumL_all = 0.0; g_sumW_all = 0.0; g_n_all = 0;
    g_sumL_A2  = 0.0; g_sumW_A2  = 0.0; g_n_A2 = 0;
    g_sumL_A3  = 0.0; g_sumW_A3  = 0.0; g_n_A3 = 0;
}

static void addDeliveredStats(float measuredLength, float measuredWeight, int destArea) {
    g_sumL_all += measuredLength;
    g_sumW_all += measuredWeight;
    g_n_all++;

    if (destArea == 1) {
        g_sumL_A2 += measuredLength;
        g_sumW_A2 += measuredWeight;
        g_n_A2++;
    } else if (destArea == 2) {
        g_sumL_A3 += measuredLength;
        g_sumW_A3 += measuredWeight;
        g_n_A3++;
    }
}

static int allocTrackedSlot() {
    for (int i = 0; i < 32; i++) {
        if (!g_products[i].used) {
            memset(&g_products[i], 0, sizeof(g_products[i]));
            g_products[i].used = true;
            g_products[i].destArea = -1;
            return i;
        }
    }
    return -1;
}

static void freeTrackedSlot(int idx) {
    if (idx >= 0 && idx < 32) {
        memset(&g_products[idx], 0, sizeof(g_products[idx]));
    }
}

static void syncDeliveredCountsFromSimulatorLocked() {
    g_area1 = (uint16_t)simAreaCount[0];
    g_area2 = (uint16_t)simAreaCount[1];
    g_area3 = (uint16_t)simAreaCount[2];
}

static void rebuildSnapshotLocked() {
    syncDeliveredCountsFromSimulatorLocked();

    memset(&g_snap, 0, sizeof(g_snap));
    g_snap.area1 = g_area1;
    g_snap.area2 = g_area2;
    g_snap.area3 = g_area3;

    for (int i = 0; i < 32; i++) {
        if (!g_products[i].used || g_products[i].delivered) continue;

        if (!g_products[i].classified) {
            g_snap.pending++;
        } else if (g_products[i].destArea == 0) {
            g_snap.inflightA1++;
        } else if (g_products[i].destArea == 1) {
            g_snap.inflightA2++;
        } else if (g_products[i].destArea == 2) {
            g_snap.inflightA3++;
        }
    }
}

static void updateGate1Locked() {
    rebuildSnapshotLocked();

    uint32_t occA1 = (uint32_t)g_snap.area1 + g_snap.inflightA1;
    uint32_t occA2 = (uint32_t)g_snap.area2 + g_snap.inflightA2;
    uint32_t occA3 = (uint32_t)g_snap.area3 + g_snap.inflightA3;

    if (emergencyLatched()) {
        g_gate1Ctrl = false;
        return;
    }

    int admissibleArea = findNextAdmissibleArea(occA1, occA2, occA3);
    g_gate1Ctrl = (admissibleArea >= 0);
}

static void resetControllerStateLocked() {
    memset(g_products, 0, sizeof(g_products));
    memset(&g_snap, 0, sizeof(g_snap));

    syncDeliveredCountsFromSimulatorLocked();

    g_gate1Ctrl = true;
    g_gate2Ctrl = false;
    g_gate3Ctrl = false;
    g_gate4Ctrl = false;

    g_gate2HoldUntilUs = 0;
    g_gate3HoldUntilUs = 0;
    g_gate4HoldUntilUs = 0;
    g_gate4RouteA3 = false;

    g_detectedSequence = 0;

    clearStats();
    clearEmergencyLatched();

    xQueueReset(g_edgeQueue);

    digitalWrite(gate2DebugPin, LOW);
    digitalWrite(gate3DebugPin, LOW);
    digitalWrite(gate4DebugPin, LOW);

    updateGate1Locked();
}

static void printSortRT() {
    if (xSemaphoreTake(g_stateMutex, pdMS_TO_TICKS(20)) != pdTRUE) return;

    rebuildSnapshotLocked();

    Serial.printf("sortRT: %03u,%03u,%03u", g_snap.area1, g_snap.area2, g_snap.area3);

    for (int i = 0; i < 32; i++) {
        if (!g_products[i].used) continue;
        if (g_products[i].delivered) continue;
        if (!g_products[i].classified) continue;

        Serial.printf(",%s,%.1f,%.1f,%s",
                      g_products[i].tn,
                      round1(g_products[i].measuredLength),
                      round1(g_products[i].measuredWeight),
                      areaToText(g_products[i].destArea));
    }

    Serial.println();
    xSemaphoreGive(g_stateMutex);
}

static void printSortSTAT() {
    double Lavg  = (g_n_all ? (g_sumL_all / g_n_all) : 0.0);
    double Wavg  = (g_n_all ? (g_sumW_all / g_n_all) : 0.0);
    double L2avg = (g_n_A2 ? (g_sumL_A2 / g_n_A2) : 0.0);
    double W2avg = (g_n_A2 ? (g_sumW_A2 / g_n_A2) : 0.0);
    double L3avg = (g_n_A3 ? (g_sumL_A3 / g_n_A3) : 0.0);
    double W3avg = (g_n_A3 ? (g_sumW_A3 / g_n_A3) : 0.0);

    Serial.printf("sortSTAT: %.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
                  round2(Lavg), round2(L2avg), round2(L3avg),
                  round2(Wavg), round2(W2avg), round2(W3avg));
}

static void normalizeCommand(char* s) {
    size_t len = strlen(s);
    while (len && (s[len - 1] == '\r' || s[len - 1] == '\n' || s[len - 1] == ' ' || s[len - 1] == '\t')) {
        s[--len] = '\0';
    }

    size_t start = 0;
    while (s[start] == ' ' || s[start] == '\t') start++;
    if (start) memmove(s, s + start, strlen(s + start) + 1);

    for (char* p = s; *p; ++p) {
        *p = (char)tolower((unsigned char)*p);
    }

    if (strncmp(s, "cmd ", 4) == 0) {
        memmove(s, s + 4, strlen(s + 4) + 1);

        len = strlen(s);
        while (len && (s[len - 1] == ' ' || s[len - 1] == '\t')) {
            s[--len] = '\0';
        }

        start = 0;
        while (s[start] == ' ' || s[start] == '\t') start++;
        if (start) memmove(s, s + start, strlen(s + start) + 1);
    }
}

// ----------------------------------------------------------------------------
// ISR
// ----------------------------------------------------------------------------
void IRAM_ATTR obstructionSensorInterrupt() {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    if (g_obstructionSensor == true) {
        g_currentTimestamp.riseEdgeUs = esp_timer_get_time();
    } else {
        g_currentTimestamp.fallEdgeUs = esp_timer_get_time();
        xQueueSendFromISR(g_edgeQueue, (void*)&g_currentTimestamp, &xHigherPriorityTaskWoken);
        if (xHigherPriorityTaskWoken) portYIELD_FROM_ISR();
    }
}

// ----------------------------------------------------------------------------
// Core 0 task
// ----------------------------------------------------------------------------
void productProcessingTask(void *pvParameters) {
    (void)pvParameters;
    product_timestamp_t ts;

    for (;;) {
        if (xQueueReceive(g_edgeQueue, &ts, pdMS_TO_TICKS(1)) == pdTRUE) {
            if (!emergencyLatched()) {
                if (xSemaphoreTake(g_stateMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
                    int idx = allocTrackedSlot();
                    if (idx >= 0) {
                        tracked_product_t& p = g_products[idx];

                        p.riseUs = ts.riseEdgeUs;
                        p.fallUs = ts.fallEdgeUs;

                        int64_t durationUs = ts.fallEdgeUs - ts.riseEdgeUs;
                        p.measuredLength = (float)durationUs * sp.v1 / 1000000.0f;
                        p.measuredWeight = 0.0f;
                        p.barcodeID = 0;
                        barcodeToTrackingText(0, p.tn, sizeof(p.tn));

                        p.sourceIndex = g_detectedSequence;
                        g_detectedSequence++;

                        if (p.sourceIndex >= 0 && p.sourceIndex < numProducts) {
                            float trueLen = productArray[p.sourceIndex].length;
                            p.trueSizeAbnormal = (trueLen < sp.L_min || trueLen > sp.L_max);
                        } else {
                            p.trueSizeAbnormal = (p.measuredLength < sp.L_min || p.measuredLength > sp.L_max);
                        }

                        p.sizeAbnormal   = p.trueSizeAbnormal;
                        p.weightAbnormal = false;
                        p.weightDone     = false;
                        p.barcodeDone    = false;
                        p.classified     = p.sizeAbnormal;
                        p.delivered      = false;
                        p.destArea       = p.sizeAbnormal ? 0 : -1;

                        int64_t gateDelayUs = (int64_t)(sp.T_gate * 1000.0f);

                        p.tG2CmdUs   = p.fallUs + cmToUs(sp.d0 - p.measuredLength, sp.v1) - gateDelayUs - 2000;
                        p.tWeightUs  = p.fallUs + cmToUs(sp.d1 - (p.measuredLength * 0.5f), sp.v1);
                        p.tG3CmdUs   = p.fallUs + cmToUs(sp.d2 - p.measuredLength, sp.v1) - gateDelayUs;
                        p.tBarcodeUs = p.fallUs + cmToUs(sp.d3 - (p.measuredLength * 0.5f), sp.v1);
                        p.tG4CmdUs   = p.fallUs + cmToUs(sp.d4 - p.measuredLength, sp.v1) - gateDelayUs;
                        p.tArriveUs  = 0;

                        if (p.tG2CmdUs < p.fallUs) p.tG2CmdUs = p.fallUs;
                        if (p.tG3CmdUs < p.fallUs) p.tG3CmdUs = p.fallUs;
                        if (p.tG4CmdUs < p.fallUs) p.tG4CmdUs = p.fallUs;

                        // Simulator-coupled G2 assist
                        if (p.trueSizeAbnormal) {
                            int64_t g2CheckUs = p.riseUs + cmToUs(sp.d0, sp.v1);
                            int64_t assistHoldUntilUs = g2CheckUs + 20000;
                            if (assistHoldUntilUs > g_gate2HoldUntilUs) {
                                g_gate2HoldUntilUs = assistHoldUntilUs;
                            }
                            p.g2Issued = true;
                            p.tArriveUs = g2CheckUs + 5000;
                        }
                    }

                    updateGate1Locked();
                    xSemaphoreGive(g_stateMutex);
                }
            }
        }

        if (!emergencyLatched()) {
            int64_t tNow = nowUs();

            if (xSemaphoreTake(g_stateMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
                for (int i = 0; i < 32; i++) {
                    if (!g_products[i].used || g_products[i].delivered) continue;
                    tracked_product_t& p = g_products[i];

                    if (p.sizeAbnormal && !p.g2Issued && tNow >= p.tG2CmdUs) {
                        p.g2Issued = true;
                        g_gate2HoldUntilUs = tNow + (int64_t)(sp.T_gate * 1000.0f) + 6000;
                        p.tArriveUs = tNow + 5000;
                    }

                    if (!p.sizeAbnormal && !p.weightDone && tNow >= p.tWeightUs) {
                        p.measuredWeight = g_weightSensor;
                        p.weightDone = true;
                        p.weightAbnormal = (p.measuredWeight < sp.W_min || p.measuredWeight > sp.W_max);
                        p.destArea = classifyProduct(p.measuredLength, p.measuredWeight, true);
                        p.classified = true;

                        if (p.destArea == 0) {
                            p.tArriveUs = p.tG3CmdUs + 5000;
                        } else {
                            int64_t tFrontAtG4 = p.fallUs + cmToUs(sp.d4 - p.measuredLength, sp.v1);
                            if (p.destArea == 1) p.tArriveUs = tFrontAtG4 + cmToUs(sp.d5, sp.v2);
                            else                 p.tArriveUs = tFrontAtG4 + cmToUs(sp.d6, sp.v3);
                        }
                    }

                    if (!p.sizeAbnormal && p.weightDone && p.weightAbnormal &&
                        !p.g3Issued && tNow >= p.tG3CmdUs) {
                        p.g3Issued = true;
                        g_gate3HoldUntilUs = tNow + (int64_t)(sp.T_gate * 1000.0f) + 3000;
                    }

                    if (!p.sizeAbnormal && p.weightDone && !p.weightAbnormal &&
                        !p.barcodeDone && tNow >= p.tBarcodeUs) {
                        p.barcodeID = g_barcodeReader;
                        barcodeToTrackingText(p.barcodeID, p.tn, sizeof(p.tn));
                        p.barcodeDone = true;
                    }

                    if (!p.sizeAbnormal && p.weightDone && !p.weightAbnormal &&
                        !p.g4Issued && tNow >= p.tG4CmdUs) {
                        p.g4Issued = true;
                        if (p.destArea == 2) {
                            g_gate4RouteA3 = true;
                            g_gate4HoldUntilUs = tNow + (int64_t)(sp.T_gate * 1000.0f) + 3000;
                        }
                    }

                    if (p.tArriveUs > 0 && tNow >= p.tArriveUs) {
                        p.delivered = true;
                        addDeliveredStats(p.measuredLength, p.measuredWeight, p.destArea);
                        freeTrackedSlot(i);
                    }
                }

                g_gate2Ctrl = (tNow < g_gate2HoldUntilUs);
                g_gate3Ctrl = (tNow < g_gate3HoldUntilUs);
                g_gate4Ctrl = (tNow < g_gate4HoldUntilUs) ? g_gate4RouteA3 : false;

                if (tNow >= g_gate4HoldUntilUs) g_gate4RouteA3 = false;

                digitalWrite(gate2DebugPin, g_gate2Ctrl ? HIGH : LOW);
                digitalWrite(gate3DebugPin, g_gate3Ctrl ? HIGH : LOW);
                digitalWrite(gate4DebugPin, g_gate4Ctrl ? HIGH : LOW);

                updateGate1Locked();
                xSemaphoreGive(g_stateMutex);
            }
        } else {
            g_gate1Ctrl = false;
            g_gate2Ctrl = false;
            g_gate3Ctrl = false;
            g_gate4Ctrl = false;

            digitalWrite(gate2DebugPin, LOW);
            digitalWrite(gate3DebugPin, LOW);
            digitalWrite(gate4DebugPin, LOW);
        }

        vTaskDelay(pdMS_TO_TICKS(1));
    }
}

// ----------------------------------------------------------------------------
// Command task
// ----------------------------------------------------------------------------
void commandTask(void *pvParameters) {
    (void)pvParameters;
    char line[32];
    size_t n = 0;

    for (;;) {
        while (Serial.available()) {
            char ch = (char)Serial.read();

            if (ch == '\n' || ch == '\r') {
                if (n > 0) {
                    line[n] = '\0';
                    n = 0;
                    normalizeCommand(line);

                    if (strcmp(line, "emergency") == 0) {
                        if (!emergencyLatched()) {
                            setEmergencyLatched();
                            g_gate1Ctrl = false;
                            g_gate2Ctrl = false;
                            g_gate3Ctrl = false;
                            g_gate4Ctrl = false;
                            digitalWrite(gate2DebugPin, LOW);
                            digitalWrite(gate3DebugPin, LOW);
                            digitalWrite(gate4DebugPin, LOW);
                            Serial.println("EMERGENCY: System halted");
                        }
                    } else if (strcmp(line, "reset") == 0) {
                        if (xSemaphoreTake(g_stateMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
                            resetControllerStateLocked();
                            xSemaphoreGive(g_stateMutex);
                        }
                    } else if (strcmp(line, "restart") == 0) {
                        ESP.restart();
                    } else if (strcmp(line, "stat") == 0) {
                        if (xSemaphoreTake(g_stateMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
                            printSortSTAT();
                            xSemaphoreGive(g_stateMutex);
                        }
                    }
                }
            } else {
                if (n < sizeof(line) - 1) line[n++] = ch;
            }
        }

        vTaskDelay(pdMS_TO_TICKS(5));
    }
}

// ----------------------------------------------------------------------------
// 1 Hz status task
// ----------------------------------------------------------------------------
void backgroundTask(void *pvParameters) {
    (void)pvParameters;
    TickType_t lastWake = xTaskGetTickCount();

    for (;;) {
        if (!emergencyLatched()) {
            printSortRT();
        }
        vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(1000));
    }
}

// ----------------------------------------------------------------------------
// setup / loop
// ----------------------------------------------------------------------------
void setup() {
    Serial.begin(115200);

    g_edgeQueue  = xQueueCreate(10, sizeof(product_timestamp_t));
    g_stateMutex = xSemaphoreCreateMutex();
    g_flags      = xEventGroupCreate();

    pinMode(gate2DebugPin, OUTPUT);
    pinMode(gate3DebugPin, OUTPUT);
    pinMode(gate4DebugPin, OUTPUT);
    digitalWrite(gate2DebugPin, LOW);
    digitalWrite(gate3DebugPin, LOW);
    digitalWrite(gate4DebugPin, LOW);

    if (xSemaphoreTake(g_stateMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
        resetControllerStateLocked();
        xSemaphoreGive(g_stateMutex);
    }

    xTaskCreatePinnedToCore(productProcessingTask, "ProductProcessor", 6144, NULL, 10, NULL, 0);
    xTaskCreatePinnedToCore(commandTask, "CommandTask", 6144, NULL, 5, NULL, 1);

    startSimulator();

    xTaskCreatePinnedToCore(backgroundTask, "BackgroundTask", 8192, NULL, 2, NULL, 1);
}

void loop() {
    vTaskDelay(portMAX_DELAY);
}

/******************************************** END OF STUDENT CODE *****************************************/
/*!!!!!!!!!!!!!!!!!!!!!!!!!!! ANY CODE BELOW THIS LINE SHOULD NOT BE MODIFIED !!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*================================ START OF SIMULATOR ENGINE ============================================*/

volatile int currentProductIndex = 0;
const int MAX_SCHEDULERS = 4;
ProductScheduler g_schedulers[MAX_SCHEDULERS];

static inline uint64_t distanceToMicros(float distance_cm, float speed_cm_s) {
    double tSec = (double)distance_cm / (double)speed_cm_s;
    return (uint64_t)(tSec * 1e6);
}

void fillProductEvents(ProductScheduler &sch) {
    const Product &p = sch.product;
    sch.totalEvents = 0;
    sch.currentEventIndex = 0;

    auto &events = sch.events;
    int idx = 0;

    uint64_t leadTime = distanceToMicros(DIST_LEAD, sp.v1);

    bool sizeOk   = (p.length >= sp.L_min && p.length <= sp.L_max);
    bool weightOk = (p.weight >= sp.W_min && p.weight <= sp.W_max);
    bool isSmallLight = sizeOk && weightOk &&
                        (p.length <= sp.L_threshold) && (p.weight <= sp.W_threshold);

    int destArea;
    if (!sizeOk) destArea = 0;
    else if (!weightOk) destArea = 0;
    else if (isSmallLight) destArea = 1;
    else destArea = 2;

    events[idx].triggerTimeMicros = leadTime;
    events[idx].eventType = ProductEventType::OBSTRUCTION_SENSOR_RISE;
    events[idx].gateExpected = 0;
    events[idx].destArea = 0;
    idx++;

    uint64_t fallTime = distanceToMicros(p.length, sp.v1);
    events[idx].triggerTimeMicros = leadTime + fallTime;
    events[idx].eventType = ProductEventType::OBSTRUCTION_SENSOR_FALL;
    events[idx].gateExpected = 0;
    events[idx].destArea = 0;
    idx++;

    uint64_t g2Time = distanceToMicros(sp.d0, sp.v1) + leadTime;
    events[idx].triggerTimeMicros = g2Time;
    events[idx].eventType = ProductEventType::G2_CHECK;
    events[idx].gateExpected = sizeOk ? 0 : 1;
    events[idx].destArea = 0;
    idx++;

    if (!sizeOk) {
        events[idx].triggerTimeMicros = g2Time + 5000;
        events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
        events[idx].gateExpected = 0;
        events[idx].destArea = 0;
        idx++;

        events[idx].triggerTimeMicros = g2Time + 10000;
        events[idx].eventType = ProductEventType::FINISH;
        events[idx].gateExpected = 0;
        events[idx].destArea = 0;
        idx++;
    } else {
        uint64_t centerAtWeight = distanceToMicros(sp.d1 + p.length / 2.0f, sp.v1) + leadTime;
        uint64_t halfWindowW = distanceToMicros(p.length * 0.1f, sp.v1);
        events[idx].triggerTimeMicros = centerAtWeight - halfWindowW;
        events[idx].eventType = ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW;
        events[idx].gateExpected = 0;
        events[idx].destArea = 0;
        idx++;

        events[idx].triggerTimeMicros = centerAtWeight + halfWindowW;
        events[idx].eventType = ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW;
        events[idx].gateExpected = 0;
        events[idx].destArea = 0;
        idx++;

        uint64_t g3Time = distanceToMicros(sp.d2, sp.v1) + leadTime;
        events[idx].triggerTimeMicros = g3Time;
        events[idx].eventType = ProductEventType::G3_CHECK;
        events[idx].gateExpected = weightOk ? 0 : 1;
        events[idx].destArea = 0;
        idx++;

        if (!weightOk) {
            events[idx].triggerTimeMicros = g3Time + 5000;
            events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
            events[idx].gateExpected = 0;
            events[idx].destArea = 0;
            idx++;

            events[idx].triggerTimeMicros = g3Time + 10000;
            events[idx].eventType = ProductEventType::FINISH;
            events[idx].gateExpected = 0;
            events[idx].destArea = 0;
            idx++;
        } else {
            uint64_t centerAtBarcode = distanceToMicros(sp.d3 + p.length / 2.0f, sp.v1) + leadTime;
            uint64_t halfWindowB = distanceToMicros(p.length * 0.1f, sp.v1);
            events[idx].triggerTimeMicros = centerAtBarcode - halfWindowB;
            events[idx].eventType = ProductEventType::BARCODE_SENSOR_ENTER_WINDOW;
            events[idx].gateExpected = 0;
            events[idx].destArea = 0;
            idx++;

            events[idx].triggerTimeMicros = centerAtBarcode + halfWindowB;
            events[idx].eventType = ProductEventType::BARCODE_SENSOR_EXIT_WINDOW;
            events[idx].gateExpected = 0;
            events[idx].destArea = 0;
            idx++;

            uint64_t g4Time = distanceToMicros(sp.d4, sp.v1) + leadTime;
            events[idx].triggerTimeMicros = g4Time;
            events[idx].eventType = ProductEventType::G4_CHECK;
            events[idx].gateExpected = isSmallLight ? 0 : 1;
            events[idx].destArea = 0;
            idx++;

            uint64_t arriveTime;
            if (isSmallLight) {
                arriveTime = g4Time + distanceToMicros(sp.d5, sp.v2);
            } else {
                arriveTime = g4Time + distanceToMicros(sp.d6, sp.v3);
            }
            events[idx].triggerTimeMicros = arriveTime;
            events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
            events[idx].gateExpected = 0;
            events[idx].destArea = destArea;
            idx++;

            events[idx].triggerTimeMicros = arriveTime + 5000;
            events[idx].eventType = ProductEventType::FINISH;
            events[idx].gateExpected = 0;
            events[idx].destArea = 0;
            idx++;
        }
    }

    sch.totalEvents = idx;

    for (int i = 0; i < sch.totalEvents - 1; i++) {
        for (int j = i + 1; j < sch.totalEvents; j++) {
            if (events[i].triggerTimeMicros > events[j].triggerTimeMicros) {
                ProductEvent tmp = events[i];
                events[i] = events[j];
                events[j] = tmp;
            }
        }
    }
}

void printProductEvents(const ProductScheduler &sch) {
    Serial.println("===== Product Event Timeline =====");
    Serial.printf("Length: %.2f cm  Weight: %.2f g  Barcode: %d\n",
                  sch.product.length, sch.product.weight, sch.product.barcodeID);

    bool sizeOk = (sch.product.length >= sp.L_min && sch.product.length <= sp.L_max);
    bool weightOk = (sch.product.weight >= sp.W_min && sch.product.weight <= sp.W_max);
    const char* dest = "???";
    if (!sizeOk) dest = "Area1(size)";
    else if (!weightOk) dest = "Area1(weight)";
    else if (sch.product.length <= sp.L_threshold && sch.product.weight <= sp.W_threshold) dest = "Area2";
    else dest = "Area3";
    Serial.printf("Expected destination: %s\n", dest);

    for (int i = 0; i < sch.totalEvents; i++) {
        const ProductEvent &evt = sch.events[i];
        float timeMs = evt.triggerTimeMicros / 1000.0f;
        Serial.printf("  %d: %.3f ms - ", i, timeMs);
        switch (evt.eventType) {
            case ProductEventType::OBSTRUCTION_SENSOR_RISE:    Serial.println("OBSTRUCTION_RISE"); break;
            case ProductEventType::OBSTRUCTION_SENSOR_FALL:    Serial.println("OBSTRUCTION_FALL"); break;
            case ProductEventType::G2_CHECK:                   Serial.println("G2_CHECK (size)"); break;
            case ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW: Serial.println("WEIGHT_ENTER"); break;
            case ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW:  Serial.println("WEIGHT_EXIT"); break;
            case ProductEventType::G3_CHECK:                   Serial.println("G3_CHECK (weight)"); break;
            case ProductEventType::BARCODE_SENSOR_ENTER_WINDOW:Serial.println("BARCODE_ENTER"); break;
            case ProductEventType::BARCODE_SENSOR_EXIT_WINDOW: Serial.println("BARCODE_EXIT"); break;
            case ProductEventType::G4_CHECK:                   Serial.println("G4_CHECK (route)"); break;
            case ProductEventType::ARRIVE_DESTINATION:         Serial.println("ARRIVE_DESTINATION"); break;
            case ProductEventType::FINISH:                     Serial.println("FINISH"); break;
            default:                                           Serial.println("UNKNOWN"); break;
        }
    }
    Serial.println("==================================");
}

void ARDUINO_ISR_ATTR onTimerInterrupt(void* arg) {
    ProductScheduler* sch = (ProductScheduler*)arg;
    hw_timer_t* timer = sch->timerHandle;

    if (!timer) return;

    if (sch->currentEventIndex >= sch->totalEvents) {
        timerEnd(timer);
        sch->timerHandle = nullptr;
        return;
    }

    ProductEvent evt = sch->events[sch->currentEventIndex];
    sch->currentEventIndex++;

    int evtType = (int)evt.eventType;

    if (evtType == (int)ProductEventType::OBSTRUCTION_SENSOR_RISE) {
        g_obstructionSensor = true;
        digitalWrite(g_obstructionSensorPin, HIGH);
        obstructionSensorInterrupt();
    }
    else if (evtType == (int)ProductEventType::OBSTRUCTION_SENSOR_FALL) {
        g_obstructionSensor = false;
        digitalWrite(g_obstructionSensorPin, LOW);
        obstructionSensorInterrupt();
    }
    else if (evtType == (int)ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW) {
        g_weightSensor = sch->product.weight;
    }
    else if (evtType == (int)ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW) {
        g_weightSensor = 0.0f;
    }
    else if (evtType == (int)ProductEventType::BARCODE_SENSOR_ENTER_WINDOW) {
        g_barcodeReader = sch->product.barcodeID;
    }
    else if (evtType == (int)ProductEventType::BARCODE_SENSOR_EXIT_WINDOW) {
        g_barcodeReader = 0;
    }
    else if (evtType == (int)ProductEventType::G2_CHECK) {
        if ((int)g_gate2Ctrl != evt.gateExpected) errorGateCNT[0]++;
    }
    else if (evtType == (int)ProductEventType::G3_CHECK) {
        if ((int)g_gate3Ctrl != evt.gateExpected) errorGateCNT[1]++;
    }
    else if (evtType == (int)ProductEventType::G4_CHECK) {
        if ((int)g_gate4Ctrl != evt.gateExpected) errorGateCNT[2]++;
    }
    else if (evtType == (int)ProductEventType::ARRIVE_DESTINATION) {
        simAreaCount[evt.destArea]++;
    }
    else if (evtType == (int)ProductEventType::FINISH) {
        digitalWrite(sch->debugPin, LOW);
    }

    if (sch->currentEventIndex < sch->totalEvents) {
        timerAlarm(timer, sch->events[sch->currentEventIndex].triggerTimeMicros, false, 0);
    } else {
        timerEnd(timer);
        sch->timerHandle = nullptr;
    }
}

hw_timer_t* allocateTimerForProduct(ProductScheduler &sch) {
    digitalWrite(sch.debugPin, HIGH);
    fillProductEvents(sch);

    hw_timer_t* timer = timerBegin(1000000);
    if (!timer) return nullptr;

    sch.timerHandle = timer;
    timerAttachInterruptArg(timer, &onTimerInterrupt, (void*)&sch);

    if (sch.totalEvents > 0) {
        digitalWrite(sch.debugPin, LOW);
        timerWrite(timer, 0);
        timerAlarm(timer, sch.events[0].triggerTimeMicros, false, 0);
    }
    return timer;
}

void statusReportTask(void *pvParameters) {
    vTaskDelay(2000 / portTICK_PERIOD_MS);
    while (true) {
        Serial.printf("[SIM] Products loaded: %d/%d | Area counts: A1=%d A2=%d A3=%d | Gate errors: G2=%d G3=%d G4=%d\n",
                      currentProductIndex, numProducts,
                      simAreaCount[0], simAreaCount[1], simAreaCount[2],
                      errorGateCNT[0], errorGateCNT[1], errorGateCNT[2]);

        if (currentProductIndex >= numProducts) {
            bool allDone = true;
            for (int i = 0; i < MAX_SCHEDULERS; i++) {
                if (g_schedulers[i].timerHandle != nullptr) {
                    allDone = false;
                    break;
                }
            }
            if (allDone) {
                int totalArrived = simAreaCount[0] + simAreaCount[1] + simAreaCount[2];
                Serial.println("\n=== SIMULATION COMPLETE ===");
                Serial.printf("Total products processed: %d/%d\n", totalArrived, numProducts);
                Serial.printf("Area 1: %d  Area 2: %d  Area 3: %d\n",
                              simAreaCount[0], simAreaCount[1], simAreaCount[2]);
                Serial.printf("Gate errors: G2=%d  G3=%d  G4=%d\n",
                              errorGateCNT[0], errorGateCNT[1], errorGateCNT[2]);
                if (errorGateCNT[0] == 0 && errorGateCNT[1] == 0 && errorGateCNT[2] == 0) {
                    Serial.println("Result: ALL GATES CORRECT!");
                } else {
                    Serial.println("Result: GATE ERRORS DETECTED - check your control logic");
                }
                Serial.println("===========================\n");
                vTaskDelete(NULL);
                return;
            }
        }
        vTaskDelay(2000 / portTICK_PERIOD_MS);
    }
}

void productLoadingTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();

    while (true) {
        if (currentProductIndex < numProducts) {
            if (g_gate1Ctrl) {
                bool schedulerFound = false;
                for (int i = 0; i < MAX_SCHEDULERS; i++) {
                    if (g_schedulers[i].timerHandle == nullptr) {
                        g_schedulers[i].product = productArray[currentProductIndex];
                        g_schedulers[i].debugPin = DEBUG_PIN + currentProductIndex % 3;

                        hw_timer_t* t = allocateTimerForProduct(g_schedulers[i]);
                        if (!t) {
                            Serial.println("ERROR: No hardware timer available!");
                        } else {
                            currentProductIndex++;
                            schedulerFound = true;
                        }
                        break;
                    }
                }
                (void)schedulerFound;
            }
        }
        vTaskDelayUntil(&xLastWakeTime, sp.productInterval / portTICK_PERIOD_MS);
    }
}

void startSimulator(void) {
    generateParameters(STUDENT_NUMBER);
    printParameters();
    generateProductArray();

    Serial.printf("\nGenerated %d test products:\n", numProducts);
    for (int i = 0; i < numProducts; i++) {
        bool sizeOk = (productArray[i].length >= sp.L_min && productArray[i].length <= sp.L_max);
        bool weightOk = (productArray[i].weight >= sp.W_min && productArray[i].weight <= sp.W_max);
        const char* dest;
        if (!sizeOk) dest = "A1(size)";
        else if (!weightOk) dest = "A1(weight)";
        else if (productArray[i].length <= sp.L_threshold && productArray[i].weight <= sp.W_threshold) dest = "A2";
        else dest = "A3";
        Serial.printf("  [%d] L=%.2f W=%.2f BC=%d -> %s\n",
                      i, productArray[i].length, productArray[i].weight, productArray[i].barcodeID, dest);
    }
    Serial.println();

    pinMode(g_obstructionSensorPin, OUTPUT);
    pinMode(DEBUG_PIN, OUTPUT);
    pinMode(DEBUG_PIN + 1, OUTPUT);
    pinMode(DEBUG_PIN + 2, OUTPUT);
    digitalWrite(DEBUG_PIN, LOW);
    digitalWrite(DEBUG_PIN + 1, LOW);
    digitalWrite(DEBUG_PIN + 2, LOW);

    g_obstructionSensor = false;
    g_barcodeReader = 0;
    g_weightSensor = 0.0f;

    g_gate1Ctrl = true;
    g_gate2Ctrl = false;
    g_gate3Ctrl = false;
    g_gate4Ctrl = false;

    currentProductIndex = 0;
    for (int i = 0; i < MAX_SCHEDULERS; i++) {
        g_schedulers[i].timerHandle = nullptr;
    }
    for (int i = 0; i < 3; i++) {
        errorGateCNT[i] = 0;
        simAreaCount[i] = 0;
    }

    xTaskCreate(
        productLoadingTask,
        "ProductLoader",
        2048,
        NULL,
        configMAX_PRIORITIES - 1,
        NULL
    );

    xTaskCreate(
        statusReportTask,
        "StatusReport",
        2048,
        NULL,
        1,
        NULL
    );

    Serial.println("=== SmartSort Simulator Started ===\n");
}