#include "Arduino.h"


#define STUDENT_NUMBER 2231065

// ============================================================================
// SYSTEM PARAMETERS - AUTO-GENERATED FROM STUDENT_NUMBER
// DO NOT MODIFY MANUALLY - Computed by generateParameters()
// ============================================================================
struct SystemParams {
    float v1, v2, v3;                  // Conveyor speeds (cm/s)
    float d0, d1, d2, d3, d4;          // Conv1 distances from obstruction sensor (cm)
    float d5, d6;                      // Conv2/Conv3 total lengths (cm)
    float L_min, L_max, L_threshold;   // Length criteria (cm)
    float W_min, W_max, W_threshold;   // Weight criteria (g)
    float T_gate;                      // Gate mechanical response time (ms)
    int C1, C2, C3;                    // Area capacities (products)
    int productInterval;               // Product loading interval (ms)
};

SystemParams sp;

#define DIST_LEAD   10.0f   // Fixed: entry point to obstruction sensor (cm)
#define DEBUG_PIN   12

// ============================================================================
// TYPE DEFINITIONS
// ============================================================================
enum class ProductEventType {
    OBSTRUCTION_SENSOR_RISE,
    OBSTRUCTION_SENSOR_FALL,
    G2_CHECK,                       // Size gate check
    WEIGHT_SENSOR_ENTER_WINDOW,
    WEIGHT_SENSOR_EXIT_WINDOW,
    G3_CHECK,                       // Weight gate check
    BARCODE_SENSOR_ENTER_WINDOW,
    BARCODE_SENSOR_EXIT_WINDOW,
    G4_CHECK,                       // Routing gate check
    ARRIVE_DESTINATION,             // Product reaches final area
    FINISH
};

struct Product {
    float length;       // Product length (cm)
    float weight;       // Product weight (g)
    int   barcodeID;    // Barcode tracking number
};

struct ProductEvent {
    uint64_t         triggerTimeMicros;
    ProductEventType eventType;
    int              gateExpected;       // For gate checks: expected gate state (0 or 1)
    int              destArea;           // For ARRIVE: destination area (0=A1, 1=A2, 2=A3)
};

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

// Gate error counters: [0]=G2, [1]=G3, [2]=G4
// Check these to verify your gate control is correct (should all be 0)
volatile int errorGateCNT[3] = {0};

// Ground truth area counts (maintained by simulator)
volatile int simAreaCount[3] = {0, 0, 0};  // Area1, Area2, Area3

extern void startSimulator(void);

// ============================================================================
// PARAMETER GENERATION (deterministic from student number)
// ============================================================================
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) {
        // Default parameters (no student number set)
        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("=================================================");
}

// ============================================================================
// PRODUCT ARRAY - Generated based on parameters to cover all sorting paths
// Students may modify this array for additional testing
// ============================================================================
#define MAX_TEST_PRODUCTS 24   // increased from 20 to accommodate additional test products
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;

    // ── ADDITIONAL TEST PRODUCTS ─────────────────────────────────────────────
    // Added to demonstrate full G3/G4 routing and populate sortRT product list.
    // Placed first so they load before G1 closes due to Area 1 capacity.
    // Per scaffold comment (line 168): "Students may modify this array for
    // additional testing."
    //
    // Small & light → Area 2 (exercises G4 false path, Conv2)
    productArray[idx++] = {sp.L_threshold - 0.2f, sp.W_threshold - 5.0f, 2001};
    productArray[idx++] = {sp.L_min + 0.2f,       sp.W_min + 2.0f,       2002};
    // Large or heavy → Area 3 (exercises G4 true path, Conv3)
    productArray[idx++] = {sp.L_threshold + 0.2f, sp.W_threshold + 5.0f, 3001};
    productArray[idx++] = {sp.L_max - 0.1f,       sp.W_max - 2.0f,       3002};
    // ─────────────────────────────────────────────────────────────────────────

    // Category 1: Size abnormal (too short) -> Area 1 via G2
    productArray[idx++] = {sp.L_min - 0.5f, Wmid, 1001};
    productArray[idx++] = {sp.L_min - 1.0f, Wmid + 3.0f, 1002};

    // Category 2: Size abnormal (too long) -> Area 1 via G2
    productArray[idx++] = {sp.L_max + 0.5f, Wmid, 1003};
    productArray[idx++] = {sp.L_max + 0.8f, Wmid - 2.0f, 1004};

    // Category 3: Normal size, weight too light -> Area 1 via G3
    productArray[idx++] = {Lmid, sp.W_min - 5.0f, 1005};
    productArray[idx++] = {sp.L_min + 0.1f, sp.W_min - 3.0f, 1006};

    // Category 4: Normal size, weight too heavy -> Area 1 via G3
    productArray[idx++] = {Lmid, sp.W_max + 5.0f, 1007};
    productArray[idx++] = {sp.L_max - 0.1f, sp.W_max + 3.0f, 1008};

    // Category 5: Normal, small & light -> Area 2 (L<=Lthresh AND W<=Wthresh)
    productArray[idx++] = {sp.L_min + 0.1f, sp.W_min + 1.0f, 1009};
    productArray[idx++] = {sp.L_threshold, sp.W_threshold, 1010};       // Edge case: exactly at threshold
    productArray[idx++] = {sp.L_threshold - 0.1f, sp.W_threshold - 1.0f, 1011};

    // Category 6: Normal, large or heavy -> Area 3
    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};

    // Edge cases
    productArray[idx++] = {sp.L_min, sp.W_min, 1016};    // Exactly at min boundaries -> Area 2
    productArray[idx++] = {sp.L_max, sp.W_max, 1017};    // Exactly at max -> Area 3
    productArray[idx++] = {Lmid, Wmid, 1018};             // Center values -> Area 2

    numProducts = idx;
}

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

/*
 * g_obstructionSensorPin outputs the physical signal of the obstruction sensor (for oscilloscope).
 */
#define g_obstructionSensorPin    32

/*
 * Debug output pins for gate signals (for oscilloscope verification).
 * The simulator does NOT read these - they are for your debugging only.
 */
#define gate2DebugPin             33
#define gate3DebugPin             27
#define gate4DebugPin             14

/*
 * SHARED VARIABLES - DO NOT RENAME
 * The simulator uses these to provide sensor data and check your gate controls.
 */
// ========== Simulator writes, students read ==========
volatile bool   g_obstructionSensor = false;  // Obstruction sensor state
volatile int    g_barcodeReader     = 0;       // Barcode reading (0=invalid/not in window)
volatile float  g_weightSensor      = 0.0f;    // Weight reading (0=invalid/not in window)

// ========== Students write, simulator reads ==========
// G1 (Intake): true=allow products in, false=block entry
volatile bool   g_gate1Ctrl = true;
// G2 (Size divert): false=let pass, true=activate pusher (divert to Area 1)
volatile bool   g_gate2Ctrl = false;
// G3 (Weight divert): false=let pass, true=activate pusher (divert to Area 1)
volatile bool   g_gate3Ctrl = false;
// G4 (Routing): false=route to Conv2/Area2, true=route to Conv3/Area3
volatile bool   g_gate4Ctrl = false;

/*================================= IMPLEMENT YOUR RTOS CODE BELOW ===================================*/
// Student ID: UP2231065
// Module: M21410 Real-Time Embedded Systems
// University of Portsmouth, 2025/26
// AI tools acknowledged per the module policy.

// ============================================================================
// SECTION A — CONSTANTS
// ============================================================================
#define DEST_UNKNOWN  (-1)
#define DEST_A1         0     // Area 1 (rejection / off-spec)
#define DEST_A2         1     // Area 2 (Conv2, small-light)
#define DEST_A3         2     // Area 3 (Conv3, large-heavy)
#define GATE_TIMING_MARGIN_MS 25.0f  // early actuation margin for close product spacing
#define MEAS_TOL 0.005f              // tolerance for boundary length/weight readings

// ============================================================================
// SECTION B — DATA STRUCTURES
// ============================================================================

// Timestamp pair captured by the obstruction sensor ISR.
typedef struct {
    int64_t riseEdgeUs;   // esp_timer_get_time() at rising edge
    int64_t fallEdgeUs;   // esp_timer_get_time() at falling edge
    int     seqNum;
} product_timestamp_t;

// Dynamically allocated job passed to a per-product task.
typedef struct {
    int64_t riseEdgeUs;
    int64_t fallEdgeUs;
    int     seqNum;
} product_process_t;

// Message sent from Core 0 → Core 1 once a product is fully classified.
typedef struct {
    float   length;        // measured length (cm)
    float   weight;        // measured weight (g); 0.0 if diverted before weighing
    int     barcodeID;     // barcode value; 0 if scan failed or never reached
    int     dest;          // DEST_A1 / DEST_A2 / DEST_A3
    int     seqNum;        // monotonic product sequence number
    int64_t fallEdgeUs;    // fall-edge µs timestamp (timing reference)
    int     divertGate;    // 2 = diverted at G2, 3 = at G3, 4 = routed via G4
} route_msg_t;

// One slot in the live product report buffer shown in the sortRT output.
#define MAX_REPORT_SLOTS 128
typedef struct {
    int     seqNum;
    float   length;
    float   weight;
    int     barcodeID;
    int     dest;
    bool    active;
} report_slot_t;

// ============================================================================
// SECTION C — GLOBAL VARIABLES
// ============================================================================

// ── Queues ──────────────────────────────────────────────────────────────────
static QueueHandle_t     g_edgeQueue   = NULL;   // ISR  → Core 0 processing task
static QueueHandle_t     g_routeQueue  = NULL;   // Core 0 → Core 1 arrival tracker

// ── Mutexes ─────────────────────────────────────────────────────────────────
static SemaphoreHandle_t g_statsMutex   = NULL;  // protect statistics accumulators
static SemaphoreHandle_t g_reportMutex  = NULL;  // protect report slot array
static SemaphoreHandle_t g_transitMutex = NULL;  // protect in-transit counters

// ── In-transit counts ───────────────────────────────────────────────────────
// Products that have been classified (destination known) but not yet arrived.
// Index corresponds to DEST_A1/A2/A3.  Protected by g_transitMutex.
static int g_inTransit[3] = {0, 0, 0};
static int g_areaCount[3] = {0, 0, 0};

// ── Statistics ──────────────────────────────────────────────────────────────
// Length: all products (G2+G3+A2+A3). Weight: products that reached the scale.
// Protected by g_statsMutex.
static float g_sumLenAll = 0.0f;  static int g_cntLenAll = 0;
static float g_sumWgtAll = 0.0f;  static int g_cntWgtAll = 0;
static float g_sumLenA2  = 0.0f;  static int g_cntLenA2  = 0;
static float g_sumWgtA2  = 0.0f;  static int g_cntWgtA2  = 0;
static float g_sumLenA3  = 0.0f;  static int g_cntLenA3  = 0;
static float g_sumWgtA3  = 0.0f;  static int g_cntWgtA3  = 0;

// Barcode scan performance (Section 3.5: "critical performance metric").
// Counts products that reached the barcode scanner (past G2 and G3).
// Protected by g_statsMutex.
static int g_cntBarcodeAttempts = 0;   // products that reached the scanner
static int g_cntBarcodeSuccess  = 0;   // successful scans (non-zero reading)

// ── Live product report buffer ───────────────────────────────────────────────
// Protected by g_reportMutex.
static report_slot_t g_reportSlots[MAX_REPORT_SLOTS];

// ── System-wide flags ────────────────────────────────────────────────────────
static volatile bool g_emergency  = false;
static volatile int  g_productSeq = 0;   // incremented per detected product

// ── ISR timestamp buffer ─────────────────────────────────────────────────────
// Written only inside the ISR (single hardware timer context) — safe without mutex.
static volatile product_timestamp_t g_currentTimestamp = {0, 0};

// ============================================================================
// SECTION D — HELPER FUNCTIONS
// ============================================================================

/*
 * delayUntilMs()
 * Block the calling task until (refUs + targetMs * 1000) microseconds.
 * Accounts for time already elapsed since refUs; does nothing if already past.
 */
static void delayUntilMs(int64_t refUs, float targetMs) {
    int64_t targetUs = refUs + (int64_t)(targetMs * 1000.0f);
    int64_t remainUs = targetUs - esp_timer_get_time();
    if (remainUs > 1000) {
        vTaskDelay(pdMS_TO_TICKS((uint32_t)(remainUs / 1000)));
    }
    // Sub-ms spin for final precision
    while (esp_timer_get_time() < targetUs) { }
}

/*
 * updateG1()
 * Recompute g_gate1Ctrl from in-transit counts + simulator ground-truth area counts.
 * Closes the intake if ANY area is predicted to overflow.
 * Must NOT be called while g_transitMutex is already held by the caller.
 */
static void updateG1(void) {
    if (g_emergency) {
        g_gate1Ctrl = false;
        return;
    }

    bool block = false;

    if (xSemaphoreTake(g_transitMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        block = ((g_areaCount[DEST_A1] + g_inTransit[DEST_A1]) >= sp.C1) ||
                ((g_areaCount[DEST_A2] + g_inTransit[DEST_A2]) >= sp.C2) ||
                ((g_areaCount[DEST_A3] + g_inTransit[DEST_A3]) >= sp.C3);

        xSemaphoreGive(g_transitMutex);
    }

    g_gate1Ctrl = !block;
}

// Increment in-transit count for dest and refresh G1.
static void transitIncrement(int dest) {
    if (xSemaphoreTake(g_transitMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        g_inTransit[dest]++;
        xSemaphoreGive(g_transitMutex);
    }
    updateG1();
}

// Decrement in-transit count for dest (floor 0) and refresh G1.
static void transitDecrement(int dest) {
    if (xSemaphoreTake(g_transitMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        if (g_inTransit[dest] > 0) {
            g_inTransit[dest]--;
        }

        g_areaCount[dest]++;

        xSemaphoreGive(g_transitMutex);
    }
    updateG1();
}

// Find an inactive report slot, initialise it, and return its index.
// Caller must hold g_reportMutex.
static int allocReportSlot(int seqNum) {
    for (int i = 0; i < MAX_REPORT_SLOTS; i++) {
        if (!g_reportSlots[i].active) {
            g_reportSlots[i].seqNum    = seqNum;
            g_reportSlots[i].length    = 0.0f;
            g_reportSlots[i].weight    = 0.0f;
            g_reportSlots[i].barcodeID = 0;
            g_reportSlots[i].dest      = DEST_UNKNOWN;
            g_reportSlots[i].active    = true;
            return i;
        }
    }
    return -1;  // all slots occupied (should not occur in normal operation)
}

// Mark a report slot inactive by sequence number.
// Caller must hold g_reportMutex.
static void freeReportSlot(int seqNum) {
    for (int i = 0; i < MAX_REPORT_SLOTS; i++) {
        if (g_reportSlots[i].active && g_reportSlots[i].seqNum == seqNum) {
            g_reportSlots[i].active = false;
            return;
        }
    }
}

// ============================================================================
// SECTION E — OBSTRUCTION SENSOR ISR
//
// The simulator calls this function directly from its hardware-timer ISR
// (no attachInterrupt() is required).  Only ISR-safe primitives may be used.
// ============================================================================
void IRAM_ATTR obstructionSensorInterrupt() {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    if (g_obstructionSensor) {
        // Rising edge — product front has arrived at the obstruction sensor.
        g_currentTimestamp.riseEdgeUs = esp_timer_get_time();
    } else {
        // Falling edge — product back has cleared the sensor.
        // Product length can now be computed from the edge-to-edge duration.
        g_currentTimestamp.fallEdgeUs = esp_timer_get_time();
        xQueueSendFromISR(g_edgeQueue, (const void *)&g_currentTimestamp,
                          &xHigherPriorityTaskWoken);
        if (xHigherPriorityTaskWoken) portYIELD_FROM_ISR();
    }
}
void singleProductTask(void *pvParameters);
// ============================================================================
// SECTION F — PRODUCT PROCESSING TASK  (Core 0, priority 10)
//
// Sequential pipeline for every detected product:
//   1. Receive edge timestamps → compute length
//   2. Wait → set G2  (size gate, must be ready T_gate ms before front hits d0)
//   3. Poll weight sensor during its window  (centre ±10%·L around d1)
//   4. Wait → set G3  (weight gate)
//   5. Poll barcode scanner during its window  (centre ±10%·L around d3)
//   6. Wait → set G4  (routing gate)
//   7. Forward product data to Core-1 arrival tracker via g_routeQueue
//
// Timing reference for all delays: ts.fallEdgeUs.
// Simulator gate checks occur when product FRONT reaches d0 / d2 / d4.
// From the fall edge the front is at position = length, so time-to-gate =
//   (dN - length) / v1 * 1000  ms   for gate at position dN.
// Gate must be SET at least T_gate ms before that check fires.
// ============================================================================
void productProcessingTask(void *pvParameters) {
    product_timestamp_t ts;
    (void)pvParameters;

    while (true) {
        if (xQueueReceive(g_edgeQueue, &ts, portMAX_DELAY) != pdTRUE) continue;
        if (g_emergency) continue;

        product_process_t *job = (product_process_t *)pvPortMalloc(sizeof(product_process_t));
        if (job == NULL) {
            continue;
        }

        job->riseEdgeUs = ts.riseEdgeUs;
        job->fallEdgeUs = ts.fallEdgeUs;
        job->seqNum     = ++g_productSeq;

        xTaskCreatePinnedToCore(singleProductTask,
                                "OneProduct",
                                6144,
                                job,
                                9,
                                NULL,
                                0);
    }
}
void singleProductTask(void *pvParameters) {
    product_process_t *job = (product_process_t *)pvParameters;

    product_timestamp_t ts;
    ts.riseEdgeUs = job->riseEdgeUs;
    ts.fallEdgeUs = job->fallEdgeUs;
    int seqNum = job->seqNum;

    vPortFree(job);

    if (g_emergency) {
        vTaskDelete(NULL);
    }

    // ── 1. Measure length ────────────────────────────────────────────
    float length = (float)(ts.fallEdgeUs - ts.riseEdgeUs) * sp.v1 / 1.0e6f;

    int slot = -1;
    if (xSemaphoreTake(g_reportMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        slot = allocReportSlot(seqNum);
        if (slot >= 0) {
            g_reportSlots[slot].length = length;
        }
        xSemaphoreGive(g_reportMutex);
    }

    if (xSemaphoreTake(g_statsMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        g_sumLenAll += length;
        g_cntLenAll++;
        xSemaphoreGive(g_statsMutex);
    }

    // ── 2. Set G2 — size gate ────────────────────────────────────────
    bool sizeOk = (length >= (sp.L_min - MEAS_TOL) && length <= (sp.L_max + MEAS_TOL));
    float g2Ms  = (sp.d0 - length) / sp.v1 * 1000.0f - sp.T_gate - GATE_TIMING_MARGIN_MS;
    delayUntilMs(ts.fallEdgeUs, g2Ms);
    g_gate2Ctrl = !sizeOk;
    digitalWrite(gate2DebugPin, g_gate2Ctrl ? HIGH : LOW);

    if (!sizeOk) {
        if (slot >= 0 && xSemaphoreTake(g_reportMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
            g_reportSlots[slot].dest = DEST_A1;
            xSemaphoreGive(g_reportMutex);
        }

        transitIncrement(DEST_A1);

        route_msg_t msg = { length, 0.0f, 0, DEST_A1, seqNum, ts.fallEdgeUs, 2 };
        xQueueSend(g_routeQueue, &msg, pdMS_TO_TICKS(20));

        vTaskDelete(NULL);
    }

    // ── 3. Read weight sensor ────────────────────────────────────────
    float winOpenMs  = (sp.d1 - 0.6f * length) / sp.v1 * 1000.0f;
    float winCloseMs = (sp.d1 - 0.4f * length) / sp.v1 * 1000.0f;

    delayUntilMs(ts.fallEdgeUs, winOpenMs);

    float weight = 0.0f;
    int64_t winCloseUs = ts.fallEdgeUs + (int64_t)(winCloseMs * 1000.0f);

    while (esp_timer_get_time() < winCloseUs) {
        weight = g_weightSensor;
        if (weight != 0.0f) break;
        vTaskDelay(1);
    }

    // ── 4. Set G3 — weight gate ──────────────────────────────────────
    bool weightOk = (weight >= (sp.W_min - MEAS_TOL) && weight <= (sp.W_max + MEAS_TOL));
    float g3Ms    = (sp.d2 - length) / sp.v1 * 1000.0f - sp.T_gate - GATE_TIMING_MARGIN_MS;

    delayUntilMs(ts.fallEdgeUs, g3Ms);

    g_gate3Ctrl = !weightOk;
    digitalWrite(gate3DebugPin, g_gate3Ctrl ? HIGH : LOW);

    if (!weightOk) {
        if (xSemaphoreTake(g_statsMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
            g_sumWgtAll += weight;
            g_cntWgtAll++;
            xSemaphoreGive(g_statsMutex);
        }

        if (slot >= 0 && xSemaphoreTake(g_reportMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
            g_reportSlots[slot].weight = weight;
            g_reportSlots[slot].dest   = DEST_A1;
            xSemaphoreGive(g_reportMutex);
        }

        transitIncrement(DEST_A1);

        route_msg_t msg = { length, weight, 0, DEST_A1, seqNum, ts.fallEdgeUs, 3 };
        xQueueSend(g_routeQueue, &msg, pdMS_TO_TICKS(20));

        vTaskDelete(NULL);
    }

    // ── 5. Read barcode scanner ──────────────────────────────────────
    float bcOpenMs  = (sp.d3 - 0.6f * length) / sp.v1 * 1000.0f;
    float bcCloseMs = (sp.d3 - 0.4f * length) / sp.v1 * 1000.0f;

    delayUntilMs(ts.fallEdgeUs, bcOpenMs);

    int barcodeID = 0;
    int64_t bcCloseUs = ts.fallEdgeUs + (int64_t)(bcCloseMs * 1000.0f);

    while (esp_timer_get_time() < bcCloseUs) {
        barcodeID = g_barcodeReader;
        if (barcodeID != 0) break;
        vTaskDelay(1);
    }

    // ── 6. Set G4 — final routing gate ───────────────────────────────
    bool toArea2 = (length <= (sp.L_threshold + MEAS_TOL) && weight <= (sp.W_threshold + MEAS_TOL));
    int  dest    = toArea2 ? DEST_A2 : DEST_A3;
    float g4Ms   = (sp.d4 - length) / sp.v1 * 1000.0f - sp.T_gate - GATE_TIMING_MARGIN_MS;

    delayUntilMs(ts.fallEdgeUs, g4Ms);

    g_gate4Ctrl = !toArea2;
    digitalWrite(gate4DebugPin, g_gate4Ctrl ? HIGH : LOW);

    if (xSemaphoreTake(g_statsMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        g_sumWgtAll += weight;
        g_cntWgtAll++;

        g_cntBarcodeAttempts++;
        if (barcodeID != 0) {
            g_cntBarcodeSuccess++;
        }

        if (dest == DEST_A2) {
            g_sumLenA2 += length;
            g_cntLenA2++;
            g_sumWgtA2 += weight;
            g_cntWgtA2++;
        } else {
            g_sumLenA3 += length;
            g_cntLenA3++;
            g_sumWgtA3 += weight;
            g_cntWgtA3++;
        }

        xSemaphoreGive(g_statsMutex);
    }

    if (slot >= 0 && xSemaphoreTake(g_reportMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        g_reportSlots[slot].weight    = weight;
        g_reportSlots[slot].barcodeID = barcodeID;
        g_reportSlots[slot].dest      = dest;
        xSemaphoreGive(g_reportMutex);
    }

    transitIncrement(dest);

    route_msg_t msg = { length, weight, barcodeID, dest, seqNum, ts.fallEdgeUs, 4 };
    xQueueSend(g_routeQueue, &msg, pdMS_TO_TICKS(20));

    vTaskDelete(NULL);
}
// ============================================================================
// SECTION G — DESTINATION TRACKING TASK  (Core 1, priority 5)
//
// Receives route messages from Core 0, waits for each product to physically
// arrive at its destination area (using timing derived from the simulator's own
// event schedule), then decrements the in-transit count and updates G1.
//
// Arrival times from fall edge (match simulator's ARRIVE_DESTINATION events):
//   A1 via G2 : (d0 - length)/v1*1000 + 5  ms
//   A1 via G3 : (d2 - length)/v1*1000 + 5  ms
//   A2        : (d4 - length)/v1*1000 + d5/v2*1000  ms
//   A3        : (d4 - length)/v1*1000 + d6/v3*1000  ms
// ============================================================================
void destinationTrackingTask(void *pvParameters) {
    route_msg_t msg;
    (void)pvParameters;

    while (true) {
        if (xQueueReceive(g_routeQueue, &msg, portMAX_DELAY) != pdTRUE) continue;

        float arrivalMs;
        if (msg.dest == DEST_A1) {
            float gatePos = (msg.divertGate == 2) ? sp.d0 : sp.d2;
            // Simulator fires ARRIVE_DESTINATION at gateTime + 5 ms.
            arrivalMs = (gatePos - msg.length) / sp.v1 * 1000.0f + 5.0f;
        } else if (msg.dest == DEST_A2) {
            arrivalMs = (sp.d4 - msg.length) / sp.v1 * 1000.0f
                      + sp.d5 / sp.v2 * 1000.0f;
        } else {   // DEST_A3
            arrivalMs = (sp.d4 - msg.length) / sp.v1 * 1000.0f
                      + sp.d6 / sp.v3 * 1000.0f;
        }

        delayUntilMs(msg.fallEdgeUs, arrivalMs);

        transitDecrement(msg.dest);

        // Keep completed product records available for sortRT reporting.
// The slot is not freed here because the coursework output format requires
// product data to be reported with the area counts.
        }
    }

// ============================================================================
// SECTION H — STATUS REPORT TASK  (Core 1, priority 2)
//
// Outputs a "sortRT:" line every second.
// Format: sortRT: A1count,A2count,A3count[,barcodeID,length,weight,dest ...]
//
// Area counts come from simAreaCount[] (simulator ground truth — cumulative).
// Product list shows all in-transit products with a known destination.
// ============================================================================
void statusTask(void *pvParameters) {
    TickType_t xLastWake = xTaskGetTickCount();
    (void)pvParameters;

    while (true) {
        if (!g_emergency) {
            char buf[2048];
            int  pos = 0;

            int a1 = 0, a2 = 0, a3 = 0;

            if (xSemaphoreTake(g_transitMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
                a1 = g_areaCount[DEST_A1];
                a2 = g_areaCount[DEST_A2];
                a3 = g_areaCount[DEST_A3];
                xSemaphoreGive(g_transitMutex);
            }

            pos += snprintf(buf + pos, sizeof(buf) - pos,
                            "sortRT: %03d,%03d,%03d", a1, a2, a3);

            if (xSemaphoreTake(g_reportMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
                for (int i = 0; i < MAX_REPORT_SLOTS; i++) {
                    if (!g_reportSlots[i].active)              continue;
                    if (g_reportSlots[i].dest == DEST_UNKNOWN) continue;
                    if (pos >= (int)sizeof(buf) - 80)          break;

                    const char *destStr =
                        (g_reportSlots[i].dest == DEST_A1) ? "A1" :
                        (g_reportSlots[i].dest == DEST_A2) ? "A2" : "A3";

                    if (g_reportSlots[i].barcodeID == 0) {
                        pos += snprintf(buf + pos, sizeof(buf) - pos,
                                        ",0,%.1f,%.1f,%s",
                                        g_reportSlots[i].length,
                                        g_reportSlots[i].weight,
                                        destStr);
                    } else {
                        pos += snprintf(buf + pos, sizeof(buf) - pos,
                                        ",TN%d,%.1f,%.1f,%s",
                                        g_reportSlots[i].barcodeID,
                                        g_reportSlots[i].length,
                                        g_reportSlots[i].weight,
                                        destStr);
                    }
                }
                xSemaphoreGive(g_reportMutex);
            }

            Serial.println(buf);
        }

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

// ============================================================================
// SECTION I — SERIAL COMMAND TASK  (Core 1, priority 1)
//
// Commands (terminated by newline):
//   stat      → print sortSTAT: Lavg,L2avg,L3avg,Wavg,W2avg,W3avg
//   reset     → clear statistics and in-transit counters; re-open G1
//   emergency → halt all gates immediately
// ============================================================================
void serialCmdTask(void *pvParameters) {
    char cmdBuf[32];
    int  cmdLen = 0;
    (void)pvParameters;

    while (true) {
        while (Serial.available()) {
            char c = (char)Serial.read();
            if (c == '\n' || c == '\r') {
                if (cmdLen > 0) {
                    cmdBuf[cmdLen] = '\0';
                    cmdLen = 0;

                    // ── stat ─────────────────────────────────────────
                    if (strcmp(cmdBuf, "stat") == 0) {
                        float Lavg = 0, L2avg = 0, L3avg = 0;
                        float Wavg = 0, W2avg = 0, W3avg = 0;
                        
                        if (xSemaphoreTake(g_statsMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
                            if (g_cntLenAll > 0) Lavg  = g_sumLenAll / (float)g_cntLenAll;
                            if (g_cntWgtAll > 0) Wavg  = g_sumWgtAll / (float)g_cntWgtAll;
                            if (g_cntLenA2  > 0) L2avg = g_sumLenA2  / (float)g_cntLenA2;
                            if (g_cntWgtA2  > 0) W2avg = g_sumWgtA2  / (float)g_cntWgtA2;
                            if (g_cntLenA3  > 0) L3avg = g_sumLenA3  / (float)g_cntLenA3;
                            if (g_cntWgtA3  > 0) W3avg = g_sumWgtA3  / (float)g_cntWgtA3;
                           
                            xSemaphoreGive(g_statsMutex);
                        }
                            Serial.printf("sortSTAT: %.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
                          Lavg, L2avg, L3avg, Wavg, W2avg, W3avg);
                    }

                    // ── reset ─────────────────────────────────────────
                    else if (strcmp(cmdBuf, "reset") == 0) {
                        g_emergency = false;
                        if (xSemaphoreTake(g_statsMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
                            g_sumLenAll = g_sumWgtAll = 0.0f;
                            g_sumLenA2  = g_sumWgtA2  = 0.0f;
                            g_sumLenA3  = g_sumWgtA3  = 0.0f;
                            g_cntLenAll = g_cntWgtAll = 0;
                            g_cntLenA2  = g_cntWgtA2  = 0;
                            g_cntLenA3  = g_cntWgtA3  = 0;
                            g_cntBarcodeAttempts = 0;
                            g_cntBarcodeSuccess  = 0;
                            xSemaphoreGive(g_statsMutex);
                        }
                        if (xSemaphoreTake(g_reportMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
                            memset(g_reportSlots, 0, sizeof(g_reportSlots));
                            xSemaphoreGive(g_reportMutex);
                        }
                        if (xSemaphoreTake(g_transitMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
                            g_inTransit[0] = g_inTransit[1] = g_inTransit[2] = 0;
                            g_areaCount[0] = g_areaCount[1] = g_areaCount[2] = 0;
                            xSemaphoreGive(g_transitMutex);
                        }
                        g_productSeq = 0;
                        xQueueReset(g_edgeQueue);
                        xQueueReset(g_routeQueue);
                        g_gate2Ctrl  = false;
                        g_gate3Ctrl  = false;
                        g_gate4Ctrl  = false;
                        updateG1();  // Re-evaluate G1 based on actual area state
                    }

                    // ── emergency ─────────────────────────────────────
                    else if (strcmp(cmdBuf, "emergency") == 0) {
                        g_emergency  = true;
                        g_gate1Ctrl  = false;
                        g_gate2Ctrl  = false;
                        g_gate3Ctrl  = false;
                        g_gate4Ctrl  = false;
                        Serial.println("EMERGENCY: System halted");
                    }
                }
            } else if (cmdLen < 31) {
                cmdBuf[cmdLen++] = c;
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

// ============================================================================
// SECTION J — SETUP & LOOP
// ============================================================================
void setup() {
    Serial.begin(115200);

    // Initialise report buffer.
    memset(g_reportSlots, 0, sizeof(g_reportSlots));

    // Initialise gate debug pins (used for oscilloscope waveform verification).
    pinMode(gate2DebugPin, OUTPUT); digitalWrite(gate2DebugPin, LOW);
    pinMode(gate3DebugPin, OUTPUT); digitalWrite(gate3DebugPin, LOW);
    pinMode(gate4DebugPin, OUTPUT); digitalWrite(gate4DebugPin, LOW);

    // Create queues.
    g_edgeQueue  = xQueueCreate(30, sizeof(product_timestamp_t));
    g_routeQueue = xQueueCreate(60, sizeof(route_msg_t));
    if (!g_edgeQueue || !g_routeQueue) {
        Serial.println("FATAL: Queue creation failed");
        while (1) vTaskDelay(pdMS_TO_TICKS(1000));
    }

    // Create mutexes.
    g_statsMutex   = xSemaphoreCreateMutex();
    g_reportMutex  = xSemaphoreCreateMutex();
    g_transitMutex = xSemaphoreCreateMutex();
    if (!g_statsMutex || !g_reportMutex || !g_transitMutex) {
        Serial.println("FATAL: Mutex creation failed");
        while (1) vTaskDelay(pdMS_TO_TICKS(1000));
    }

    // Core 0 — product dispatcher; creates one short task per detected product.
    xTaskCreatePinnedToCore(productProcessingTask, "ProdProc",   8192, NULL, 10, NULL, 0);

    // Core 1 — destination arrival tracker (unblocks G1 as soon as product lands).
    xTaskCreatePinnedToCore(destinationTrackingTask, "DestTrack", 4096, NULL,  5, NULL, 1);

    // Core 1 — 1-second sortRT status report.
    xTaskCreatePinnedToCore(statusTask,    "StatusTask",  4096, NULL,  2, NULL, 1);

    // Core 1 — serial command handler.
    xTaskCreatePinnedToCore(serialCmdTask, "SerialCmd",   4096, NULL,  1, NULL, 1);

    // MUST be last — starts the simulator and begins loading products.
    startSimulator();
}

void loop() {
    vTaskDelay(portMAX_DELAY);   // All work is done in RTOS tasks.
}


/******************************************** 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);

    // Determine product's ground-truth path
    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);

    // Pre-compute destination area index (0=A1, 1=A2, 2=A3)
    int destArea;
    if (!sizeOk) destArea = 0;
    else if (!weightOk) destArea = 0;
    else if (isSmallLight) destArea = 1;
    else destArea = 2;

    // 1. Obstruction sensor: front edge arrives
    events[idx].triggerTimeMicros = leadTime;
    events[idx].eventType = ProductEventType::OBSTRUCTION_SENSOR_RISE;
    events[idx].gateExpected = 0;
    events[idx].destArea = 0;
    idx++;

    // 2. Obstruction sensor: back edge leaves
    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++;

    // 3. G2 check (size gate) - front edge at d0
    // G2: true=divert (size abnormal), expected true if !sizeOk
    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) {
        // Product diverted at G2 -> Area 1
        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 {
        // 4. Weight sensor window
        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++;

        // 5. G3 check (weight gate) - front edge at d2
        // G3: true=divert (weight abnormal), expected true if !weightOk
        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) {
            // Product diverted at G3 -> Area 1
            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 {
            // 6. Barcode sensor window
            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++;

            // 7. G4 check (routing gate) - front edge at d4
            // G4: false=Conv2/Area2, true=Conv3/Area3
            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++;

            // 8. Arrive at destination area
            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;

    // Sort events by time
    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("==================================");
}

// Hardware timer ISR - executes product events
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;
    }

    // Get current event
    ProductEvent evt = sch->events[sch->currentEventIndex];
    sch->currentEventIndex++;

    // Execute event logic (no floating-point comparisons - all pre-computed)
    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) {
        // G2: true=divert. Pre-computed expected value in gateExpected
        if ((int)g_gate2Ctrl != evt.gateExpected) {
            errorGateCNT[0]++;
        }
    }
    else if (evtType == (int)ProductEventType::G3_CHECK) {
        // G3: true=divert. Pre-computed expected value in gateExpected
        if ((int)g_gate3Ctrl != evt.gateExpected) {
            errorGateCNT[1]++;
        }
    }
    else if (evtType == (int)ProductEventType::G4_CHECK) {
        // G4: false=Conv2, true=Conv3. Pre-computed expected value
        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);
    }

    // Set up next event or end timer
    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);  // 1MHz = 1us resolution
    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;
}

// Status reporting task - prints simulator state periodically
void statusReportTask(void *pvParameters) {
    vTaskDelay(2000 / portTICK_PERIOD_MS);  // Initial delay
    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]);

        // Check if all products finished
        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);
    }
}

// Product loading task - loads products at fixed intervals, checks G1 gate
void productLoadingTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();

    while (true) {
        if (currentProductIndex < numProducts) {
            // Check G1 intake gate
            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;
                    }
                }
                if (!schedulerFound) {
                    // No scheduler available this cycle, will retry next interval
                }
            } else {
                // G1 is closed - product blocked at intake
                // The product remains in the queue, will be loaded when G1 opens
            }
        }
        vTaskDelayUntil(&xLastWakeTime, sp.productInterval / portTICK_PERIOD_MS);
    }
}

void startSimulator(void) {
    // Generate parameters from student number
    generateParameters(STUDENT_NUMBER);
    printParameters();

    // Generate test product array
    generateProductArray();
    // TEST OVERRIDES DISABLED FOR DEFAULT RUN
// sp.C1 = 2;
// sp.C2 = 2;
// sp.C3 = 2;
// sp.productInterval = 400;
    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();

    // Setup GPIO pins
    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);

    // Initialize sensor outputs
    g_obstructionSensor = false;
    g_barcodeReader = 0;
    g_weightSensor = 0.0f;

    // Initialize gate controls to default
    g_gate1Ctrl = true;
    g_gate2Ctrl = false;
    g_gate3Ctrl = false;
    g_gate4Ctrl = false;

    // Reset counters
    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;
    }

    // Create product loading task (highest priority)
    xTaskCreate(
        productLoadingTask,
        "ProductLoader",
        2048,
        NULL,
        configMAX_PRIORITIES - 1,
        NULL
    );

    // Create status reporting task
    xTaskCreate(
        statusReportTask,
        "StatusReport",
        2048,
        NULL,
        1,
        NULL
    );

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