#include "Arduino.h"

// ============================================================================
// STUDENT CONFIGURATION
// Replace 0 with your student number (numeric digits only)
// Example: For UP1234567, enter 1234567
// ============================================================================
#define STUDENT_NUMBER 2553042

// ============================================================================
// 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 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;

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


// Forward struct declarations — needed because Arduino IDE auto-generates
// function prototypes at the top of the file before typedef definitions.
// These tag declarations make the types visible to the auto-generated prototypes.
struct ProductContext_t;
typedef struct ProductContext_t ProductContext;
struct ReportEntry_t;
typedef struct ReportEntry_t ReportEntry;

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


/*================================= STUDENT RTOS CODE ===================================*/

// ============================================================================
// DEBUG FLAG
// Set to 1 during development, 0 before submission
// ============================================================================
#define DEBUG_MODE 0
#if DEBUG_MODE
  #define DEBUG_PRINT(fmt, ...) Serial.printf(fmt, ##__VA_ARGS__)
#else
  #define DEBUG_PRINT(fmt, ...) do {} while(0)
#endif

// ============================================================================
// DESIGN OVERVIEW
// Core 0: productProcessingTask — Conv1 inspection pipeline.
//         Receives ISR timestamps, computes length, schedules FreeRTOS
//         software timers for each gate and sensor action per product.
// Core 1: areaMonitorTask    — G1 capacity management (20ms poll).
//         realTimeStatusTask — sortRT output every 1 second.
//         serialCommandTask  — handles stat / reset / emergency commands.
// Inter-core communication: pipelineMutex protects shared ProductContext array.
// ============================================================================

// ---- Destination constants ----
#define DEST_UNKNOWN  -1
#define DEST_AREA1     0
#define DEST_AREA2     1
#define DEST_AREA3     2

// ---- Product pipeline states ----
typedef enum {
    STATE_IN_CONV1,    // Being measured/inspected on Conv1
    STATE_IN_TRANSIT,  // Past G4, travelling to destination area
    STATE_ARRIVED,     // Confirmed arrived
    STATE_DONE         // Slot free to reuse
} ProductState;

// ---- Per-product context ----
struct ProductContext_t {
    int          slotID;
    float        measuredLength;    // cm, from obstruction sensor timing
    float        measuredWeight;    // g, from weight sensor window
    int          barcodeID;         // from barcode scanner window
    int          destination;       // DEST_AREA1/2/3 or DEST_UNKNOWN
    ProductState state;
    bool         active;            // true = slot in use
    int64_t      fallEdgeUs;        // esp_timer value at ISR fall edge
    int          trackingNum;       // Sequential product number (TN001, TN002...)
    TimerHandle_t timerG2;
    TimerHandle_t timerWeight;
    TimerHandle_t timerG3;
    TimerHandle_t timerBarcode;
    TimerHandle_t timerG4;
    TimerHandle_t timerArrival;
};

// ---- Report entry for sortRT output ----
struct ReportEntry_t {
    int   trackingNum;
    float length;
    float weight;
    int   destArea;   // 0=A1, 1=A2, 2=A3
};

// ============================================================================
// FORWARD DECLARATIONS
// Callbacks reference each other so must be declared before defined.
// ProductContext is fully defined above so these declarations are valid.
// ============================================================================
void timerCbG2(TimerHandle_t xTimer);
void timerCbWeight(TimerHandle_t xTimer);
void timerCbG3(TimerHandle_t xTimer);
void timerCbBarcode(TimerHandle_t xTimer);
void timerCbG4(TimerHandle_t xTimer);
void timerCbArrival(TimerHandle_t xTimer);
void timerCbArea1Arrival(TimerHandle_t xTimer);

// ============================================================================
// PIPELINE & STATISTICS GLOBALS
// ============================================================================
#define MAX_PIPELINE  8
static ProductContext pipeline[MAX_PIPELINE];
static SemaphoreHandle_t pipelineMutex = NULL;

// Controller area counts (separate from simAreaCount)
static volatile int myAreaCount[3]    = {0, 0, 0};
static volatile int inTransitCount[3] = {0, 0, 0};

// Statistics for sortSTAT command
static float sumLAll = 0, sumL2 = 0, sumL3 = 0;
static float sumWAll = 0, sumW2 = 0, sumW3 = 0;
static int   cntAll  = 0, cnt2  = 0, cnt3  = 0;
static SemaphoreHandle_t statsMutex = NULL;

// Report log (circular buffer for sortRT output)
#define MAX_REPORT_LOG 64
static ReportEntry reportLog[MAX_REPORT_LOG];
static int reportLogHead  = 0;
static int reportLogCount = 0;
static SemaphoreHandle_t reportMutex = NULL;

// Emergency halt flag
static volatile bool emergencyHalt = false;

// Simulation complete flag — stops sortRT after all products processed
static volatile bool g_simComplete = false;

// Global tracking number counter (increments per product detected)
static volatile int g_trackingCounter = 1;
static SemaphoreHandle_t trackingMutex = NULL;

// Queue: ISR sends timestamp pairs to productProcessingTask
static QueueHandle_t g_edgeQueue = NULL;

// ISR timestamp accumulator (rise edge stored until fall edge arrives)
static volatile int64_t isrRiseEdgeUs = 0;

// ============================================================================
// HELPER: retrieve ProductContext from a FreeRTOS timer ID
// Implemented as a macro so Arduino's prototype scanner never tries to
// auto-generate a forward declaration for it (which would fail because
// ProductContext would not yet be defined at that point in compilation).
// ============================================================================
#define ctxFromTimer(xTimer)     ((int)(intptr_t)pvTimerGetTimerID(xTimer) >= 0 &&      (int)(intptr_t)pvTimerGetTimerID(xTimer) < MAX_PIPELINE      ? &pipeline[(int)(intptr_t)pvTimerGetTimerID(xTimer)]      : (ProductContext*)NULL)

// ============================================================================
// TIMING HELPERS
// All delays are measured from the fall edge (product front = position 0).
// ============================================================================

// ── Timing note ──────────────────────────────────────────────────────────
// At the fall edge, the product FRONT is already at position L (measuredLen).
// So remaining distance to any conveyor point D is (D - L).
// For the product CENTER: centre is at L/2 at fall edge, so it reaches D
// when the front reaches D + L/2, i.e. remaining = (D - L/2) - L/2 = D - L/2... 
// Simpler: centre reaches D when (D - L/2) cm remain from fall edge position.
// T_weight = (d1 - L/2) / v1   (slides formula, from fall edge)
// T_gate   = (dN - L)  / v1 - T_gate   (front must clear gate before arriving)
// ─────────────────────────────────────────────────────────────────────────

// Time from fall edge until product FRONT reaches dist
// Front is at position L at fall edge, so remaining = dist - L
static inline TickType_t delayToFrontAtDist(float dist, float length) {
    float ms = (dist - length) / sp.v1 * 1000.0f;
    if (ms < 1.0f) ms = 1.0f;
    return pdMS_TO_TICKS((uint32_t)ms);
}

// Time from fall edge until product CENTER reaches dist
// Centre is at L/2 at fall edge, so remaining = dist - L/2
static inline TickType_t delayToCenterAtDist(float dist, float length) {
    float ms = (dist - length / 2.0f) / sp.v1 * 1000.0f;
    if (ms < 1.0f) ms = 1.0f;
    return pdMS_TO_TICKS((uint32_t)ms);
}

// Time to pre-arm gate: set it T_gate ms before front reaches gateDist
// Front is at L at fall edge, so remaining travel = gateDist - L
static inline TickType_t delayToGate(float gateDist, float length) {
    float ms = (gateDist - length) / sp.v1 * 1000.0f - sp.T_gate;
    if (ms < 1.0f) ms = 1.0f;
    return pdMS_TO_TICKS((uint32_t)ms);
}

// ============================================================================
// PIPELINE SLOT MANAGEMENT
// Must be called with pipelineMutex held.
// ============================================================================
static int allocatePipelineSlot() {
    for (int i = 0; i < MAX_PIPELINE; i++) {
        if (!pipeline[i].active) {
            memset(&pipeline[i], 0, sizeof(ProductContext));
            pipeline[i].slotID      = i;
            pipeline[i].active      = true;
            pipeline[i].destination = DEST_UNKNOWN;
            pipeline[i].state       = STATE_IN_CONV1;
            return i;
        }
    }
    return -1;
}

static void freePipelineSlot(int slot) {
    if (slot >= 0 && slot < MAX_PIPELINE)
        pipeline[slot].active = false;
}

// ============================================================================
// GATE TIMER CALLBACKS
// Each fires once at the right moment for one specific product.
// Slot index is packed into the timer pvTimerID.
// ============================================================================

// --- G2: size gate ---
void timerCbG2(TimerHandle_t xTimer) {
    if (emergencyHalt) return;
    xSemaphoreTake(pipelineMutex, portMAX_DELAY);
    ProductContext* ctx = ctxFromTimer(xTimer);
    if (ctx && ctx->active) {
        const float BOUNDARY_TOL = 0.001f;
        bool sizeOk = (ctx->measuredLength >= sp.L_min - BOUNDARY_TOL &&
                       ctx->measuredLength <= sp.L_max + BOUNDARY_TOL);
        g_gate2Ctrl = !sizeOk;   // true = divert to Area 1
        digitalWrite(gate2DebugPin, !sizeOk ? HIGH : LOW);

        if (!sizeOk) {
            ctx->destination    = DEST_AREA1;
            ctx->measuredWeight = 0.0f;   // spec: record 0.0 for pre-weigh diversions
            inTransitCount[DEST_AREA1]++;
            int slot = ctx->slotID;

            // Cancel downstream timers — product won't reach them
            if (ctx->timerWeight)  { xTimerStop(ctx->timerWeight, 0);  xTimerDelete(ctx->timerWeight, 0);  ctx->timerWeight  = NULL; }
            if (ctx->timerG3)      { xTimerStop(ctx->timerG3, 0);      xTimerDelete(ctx->timerG3, 0);      ctx->timerG3      = NULL; }
            if (ctx->timerBarcode) { xTimerStop(ctx->timerBarcode, 0); xTimerDelete(ctx->timerBarcode, 0); ctx->timerBarcode = NULL; }
            if (ctx->timerG4)      { xTimerStop(ctx->timerG4, 0);      xTimerDelete(ctx->timerG4, 0);      ctx->timerG4      = NULL; }

            // Schedule Area1 arrival (simulator fires ARRIVE 5ms after G2; use 15ms lag)
            ctx->timerArrival = xTimerCreate("A1v2", pdMS_TO_TICKS(15), pdFALSE,
                                              (void*)(intptr_t)slot, timerCbArea1Arrival);
            if (ctx->timerArrival) xTimerStart(ctx->timerArrival, 0);
        }
    }
    xSemaphoreGive(pipelineMutex);
}

// --- Weight sensor: sample g_weightSensor when product centred at d1 ---
void timerCbWeight(TimerHandle_t xTimer) {
    if (emergencyHalt) return;
    xSemaphoreTake(pipelineMutex, portMAX_DELAY);
    ProductContext* ctx = ctxFromTimer(xTimer);
    if (ctx && ctx->active) {
        ctx->measuredWeight = g_weightSensor;
    }
    xSemaphoreGive(pipelineMutex);
}

// --- G3: weight gate ---
void timerCbG3(TimerHandle_t xTimer) {
    if (emergencyHalt) return;
    xSemaphoreTake(pipelineMutex, portMAX_DELAY);
    ProductContext* ctx = ctxFromTimer(xTimer);
    if (ctx && ctx->active) {
        const float BOUNDARY_TOL = 0.001f;
        bool sizeOk   = (ctx->measuredLength >= sp.L_min - BOUNDARY_TOL &&
                         ctx->measuredLength <= sp.L_max + BOUNDARY_TOL);
        bool weightOk = sizeOk &&
                        (ctx->measuredWeight >= sp.W_min - BOUNDARY_TOL &&
                         ctx->measuredWeight <= sp.W_max + BOUNDARY_TOL);

        g_gate3Ctrl = !weightOk;   // true = divert to Area 1
        digitalWrite(gate3DebugPin, !weightOk ? HIGH : LOW);

        if (!weightOk) {
            ctx->destination = DEST_AREA1;
            inTransitCount[DEST_AREA1]++;
            int slot = ctx->slotID;

            if (ctx->timerBarcode) { xTimerStop(ctx->timerBarcode, 0); xTimerDelete(ctx->timerBarcode, 0); ctx->timerBarcode = NULL; }
            if (ctx->timerG4)      { xTimerStop(ctx->timerG4, 0);      xTimerDelete(ctx->timerG4, 0);      ctx->timerG4      = NULL; }

            ctx->timerArrival = xTimerCreate("A1v3", pdMS_TO_TICKS(15), pdFALSE,
                                              (void*)(intptr_t)slot, timerCbArea1Arrival);
            if (ctx->timerArrival) xTimerStart(ctx->timerArrival, 0);
        }
    }
    xSemaphoreGive(pipelineMutex);
}

// --- Barcode: sample g_barcodeReader when product centred at d3 ---
void timerCbBarcode(TimerHandle_t xTimer) {
    if (emergencyHalt) return;
    xSemaphoreTake(pipelineMutex, portMAX_DELAY);
    ProductContext* ctx = ctxFromTimer(xTimer);
    if (ctx && ctx->active) {
        ctx->barcodeID = g_barcodeReader;
    }
    xSemaphoreGive(pipelineMutex);
}

// --- G4: routing gate + schedule arrival timer ---
void timerCbG4(TimerHandle_t xTimer) {
    if (emergencyHalt) return;
    xSemaphoreTake(pipelineMutex, portMAX_DELAY);
    ProductContext* ctx = ctxFromTimer(xTimer);
    if (ctx && ctx->active) {
        const float BOUNDARY_TOL = 0.001f;
        bool isSmallLight = (ctx->measuredLength <= sp.L_threshold + BOUNDARY_TOL &&
                             ctx->measuredWeight <= sp.W_threshold + BOUNDARY_TOL);

        // G4: false=Conv2->Area2, true=Conv3->Area3
        g_gate4Ctrl = !isSmallLight;
        digitalWrite(gate4DebugPin, !isSmallLight ? HIGH : LOW);

        ctx->destination = isSmallLight ? DEST_AREA2 : DEST_AREA3;
        ctx->state       = STATE_IN_TRANSIT;
        inTransitCount[ctx->destination]++;

        float transitDist  = isSmallLight ? sp.d5 : sp.d6;
        float transitSpeed = isSmallLight ? sp.v2  : sp.v3;
        uint32_t arrivalMs = (uint32_t)(transitDist / transitSpeed * 1000.0f);
        if (arrivalMs < 1) arrivalMs = 1;

        int slot = ctx->slotID;
        ctx->timerArrival = xTimerCreate("Arrv", pdMS_TO_TICKS(arrivalMs), pdFALSE,
                                          (void*)(intptr_t)slot, timerCbArrival);
        if (ctx->timerArrival) xTimerStart(ctx->timerArrival, 0);
    }
    xSemaphoreGive(pipelineMutex);
}

// --- Area1 arrival: for products diverted at G2 or G3 ---
void timerCbArea1Arrival(TimerHandle_t xTimer) {
    xSemaphoreTake(pipelineMutex, portMAX_DELAY);
    ProductContext* ctx = ctxFromTimer(xTimer);
    if (ctx && ctx->active) {
        ctx->state = STATE_ARRIVED;
        myAreaCount[DEST_AREA1]++;
        if (inTransitCount[DEST_AREA1] > 0) inTransitCount[DEST_AREA1]--;

        // Log for sortRT
        xSemaphoreTake(reportMutex, portMAX_DELAY);
        int logIdx = (reportLogHead + reportLogCount) % MAX_REPORT_LOG;
        reportLog[logIdx] = { ctx->trackingNum, ctx->measuredLength,
                              ctx->measuredWeight, DEST_AREA1 };
        if (reportLogCount < MAX_REPORT_LOG) reportLogCount++;
        else reportLogHead = (reportLogHead + 1) % MAX_REPORT_LOG;
        xSemaphoreGive(reportMutex);

        // Update stats (A1 in overall avg only, not per-area)
        xSemaphoreTake(statsMutex, portMAX_DELAY);
        sumLAll += ctx->measuredLength;
        sumWAll += ctx->measuredWeight;
        cntAll++;
        xSemaphoreGive(statsMutex);

        freePipelineSlot(ctx->slotID);
    }
    xSemaphoreGive(pipelineMutex);
}

// --- Arrival: product reached Area 2 or Area 3 ---
void timerCbArrival(TimerHandle_t xTimer) {
    xSemaphoreTake(pipelineMutex, portMAX_DELAY);
    ProductContext* ctx = ctxFromTimer(xTimer);
    if (ctx && ctx->active) {
        int dest    = ctx->destination;
        ctx->state  = STATE_ARRIVED;

        if (dest >= 0 && dest <= 2) {
            myAreaCount[dest]++;
            if (inTransitCount[dest] > 0) inTransitCount[dest]--;
        }

        // Log for sortRT
        xSemaphoreTake(reportMutex, portMAX_DELAY);
        int logIdx = (reportLogHead + reportLogCount) % MAX_REPORT_LOG;
        reportLog[logIdx] = { ctx->trackingNum, ctx->measuredLength,
                              ctx->measuredWeight, dest };
        if (reportLogCount < MAX_REPORT_LOG) reportLogCount++;
        else reportLogHead = (reportLogHead + 1) % MAX_REPORT_LOG;
        xSemaphoreGive(reportMutex);

        // Update stats
        xSemaphoreTake(statsMutex, portMAX_DELAY);
        sumLAll += ctx->measuredLength;
        sumWAll += ctx->measuredWeight;
        cntAll++;
        if (dest == DEST_AREA2) {
            sumL2 += ctx->measuredLength;
            sumW2 += ctx->measuredWeight;
            cnt2++;
        } else if (dest == DEST_AREA3) {
            sumL3 += ctx->measuredLength;
            sumW3 += ctx->measuredWeight;
            cnt3++;
        }
        xSemaphoreGive(statsMutex);

        freePipelineSlot(ctx->slotID);
    }
    xSemaphoreGive(pipelineMutex);
}

// ============================================================================
// OBSTRUCTION SENSOR ISR
// Hardware timer ISR context — ISR-safe calls only.
// NO float, NO Serial, NO malloc, NO vTaskDelay.
// ============================================================================
void IRAM_ATTR obstructionSensorInterrupt() {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    if (g_obstructionSensor == true) {
        // Rising edge: product front at sensor
        isrRiseEdgeUs = esp_timer_get_time();
    } else {
        // Falling edge: product tail passed — send {rise, fall} pair to task
        int64_t pair[2];
        pair[0] = isrRiseEdgeUs;
        pair[1] = esp_timer_get_time();
        xQueueSendFromISR(g_edgeQueue, (void*)pair, &xHigherPriorityTaskWoken);
        if (xHigherPriorityTaskWoken) {
            portYIELD_FROM_ISR();
        }
    }
}

// ============================================================================
// PRODUCT PROCESSING TASK — Core 0
// Receives ISR timestamp pairs, calculates length, allocates pipeline slot,
// and schedules all downstream timers for that product.
// ============================================================================
void productProcessingTask(void *pvParameters) {
    int64_t pair[2];

    while (true) {
        if (xQueueReceive(g_edgeQueue, (void*)pair, portMAX_DELAY) != pdTRUE) continue;
        if (emergencyHalt) continue;

        int64_t riseUs     = pair[0];
        int64_t fallUs     = pair[1];
        int64_t durationUs = fallUs - riseUs;

        // Step 1: Calculate product length
        float measuredLen = (float)durationUs * sp.v1 / 1e6f;
        DEBUG_PRINT("[PROC] Detected L=%.2f cm\n", measuredLen);

        // Step 2: Allocate pipeline slot
        xSemaphoreTake(pipelineMutex, portMAX_DELAY);
        int slot = allocatePipelineSlot();
        if (slot < 0) {
            xSemaphoreGive(pipelineMutex);
            Serial.println("[WARN] Pipeline full — product dropped");
            continue;
        }
        pipeline[slot].measuredLength = measuredLen;
        pipeline[slot].fallEdgeUs     = fallUs;
        // Assign sequential tracking number
        xSemaphoreTake(trackingMutex, portMAX_DELAY);
        pipeline[slot].trackingNum = g_trackingCounter++;
        xSemaphoreGive(trackingMutex);
        xSemaphoreGive(pipelineMutex);

        // Step 3: Schedule gate and sensor timers
        // All delays from the fall edge (front = position 0 at that moment)

        // G2 pre-arm: T_gate ms before front reaches d0
        TickType_t delG2 = delayToGate(sp.d0, measuredLen);
        pipeline[slot].timerG2 = xTimerCreate("G2", delG2, pdFALSE,
                                               (void*)(intptr_t)slot, timerCbG2);
        if (pipeline[slot].timerG2) xTimerStart(pipeline[slot].timerG2, 0);

        // Weight: sample when product centre at d1
        TickType_t delW = delayToCenterAtDist(sp.d1, measuredLen);
        pipeline[slot].timerWeight = xTimerCreate("Wt", delW, pdFALSE,
                                                   (void*)(intptr_t)slot, timerCbWeight);
        if (pipeline[slot].timerWeight) xTimerStart(pipeline[slot].timerWeight, 0);

        // G3 pre-arm: T_gate ms before front reaches d2
        TickType_t delG3 = delayToGate(sp.d2, measuredLen);
        pipeline[slot].timerG3 = xTimerCreate("G3", delG3, pdFALSE,
                                               (void*)(intptr_t)slot, timerCbG3);
        if (pipeline[slot].timerG3) xTimerStart(pipeline[slot].timerG3, 0);

        // Barcode: sample when product centre at d3
        TickType_t delB = delayToCenterAtDist(sp.d3, measuredLen);
        pipeline[slot].timerBarcode = xTimerCreate("BC", delB, pdFALSE,
                                                    (void*)(intptr_t)slot, timerCbBarcode);
        if (pipeline[slot].timerBarcode) xTimerStart(pipeline[slot].timerBarcode, 0);

        // G4 pre-arm: T_gate ms before front reaches d4
        // G4 callback also schedules the arrival timer
        TickType_t delG4 = delayToGate(sp.d4, measuredLen);
        pipeline[slot].timerG4 = xTimerCreate("G4", delG4, pdFALSE,
                                               (void*)(intptr_t)slot, timerCbG4);
        if (pipeline[slot].timerG4) xTimerStart(pipeline[slot].timerG4, 0);

        pipeline[slot].timerArrival = NULL;
    }
}

// ============================================================================
// AREA MONITOR TASK — Core 1
// Controls G1 to prevent area overflow.
// Checks current counts + in-transit counts every 20ms.
// Only blocks on Area 2 and Area 3 — Area 1 is manual and never empties.
// ============================================================================
void areaMonitorTask(void *pvParameters) {
    while (true) {
        if (emergencyHalt) {
            g_gate1Ctrl = false;
            vTaskDelay(pdMS_TO_TICKS(100));
            continue;
        }

        // Count products still in Conv1 with unknown destination.
        // These could route to A2 or A3, so count them against both
        // to prevent any race condition between G1 admission and G4 routing.
        int unknownCount = 0;
        xSemaphoreTake(pipelineMutex, portMAX_DELAY);
        for (int i = 0; i < MAX_PIPELINE; i++) {
            if (pipeline[i].active &&
                pipeline[i].state == STATE_IN_CONV1 &&
                pipeline[i].destination == DEST_UNKNOWN) {
                unknownCount++;
            }
        }
        xSemaphoreGive(pipelineMutex);

        // Worst-case projected load per automated area
        int proj2 = myAreaCount[DEST_AREA2] + inTransitCount[DEST_AREA2] + unknownCount;
        int proj3 = myAreaCount[DEST_AREA3] + inTransitCount[DEST_AREA3] + unknownCount;

        // Close G1 if either automated area is at or near capacity
        g_gate1Ctrl = !((proj2 >= sp.C2) || (proj3 >= sp.C3));

        vTaskDelay(pdMS_TO_TICKS(20));
    }
}

// ============================================================================
// REAL-TIME STATUS TASK — Core 1
// Outputs "sortRT: ..." every second.
// Format: sortRT: A1,A2,A3,TN,L,W,Dest,...
// ============================================================================
void realTimeStatusTask(void *pvParameters) {
    while (true) {
        vTaskDelay(pdMS_TO_TICKS(1000));

        if (emergencyHalt || g_simComplete) continue;

        // Use simulator ground truth counts for accurate sortRT output
        int a1 = simAreaCount[0];
        int a2 = simAreaCount[1];
        int a3 = simAreaCount[2];

        xSemaphoreTake(reportMutex, portMAX_DELAY);
        char lineBuf[512];
        int pos = snprintf(lineBuf, sizeof(lineBuf),
                           "sortRT: %03d,%03d,%03d", a1, a2, a3);

        // Include last 2 logged products
        int startIdx = (reportLogCount <= 2) ? 0 : reportLogCount - 2;
        for (int i = startIdx; i < reportLogCount && pos < (int)sizeof(lineBuf) - 40; i++) {
            int idx = (reportLogHead + i) % MAX_REPORT_LOG;
            const ReportEntry& e = reportLog[idx];
            const char* destStr = (e.destArea == 0) ? "A1" :
                                  (e.destArea == 1) ? "A2" : "A3";
            pos += snprintf(lineBuf + pos, sizeof(lineBuf) - pos,
                            ",TN%03d,%.1f,%.1f,%s",
                            e.trackingNum, e.length, e.weight, destStr);
        }
        xSemaphoreGive(reportMutex);

        Serial.println(lineBuf);
    }
}

// ============================================================================
// SERIAL COMMAND TASK — Core 1
// Handles: stat, reset, emergency
// ============================================================================
void serialCommandTask(void *pvParameters) {
    char cmdBuf[32];
    int  cmdIdx = 0;

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

                    if (cmd == "stat") {
                        xSemaphoreTake(statsMutex, portMAX_DELAY);
                        float Lavg  = (cntAll > 0) ? sumLAll / cntAll : 0.0f;
                        float L2avg = (cnt2   > 0) ? sumL2   / cnt2   : 0.0f;
                        float L3avg = (cnt3   > 0) ? sumL3   / cnt3   : 0.0f;
                        float Wavg  = (cntAll > 0) ? sumWAll / cntAll : 0.0f;
                        float W2avg = (cnt2   > 0) ? sumW2   / cnt2   : 0.0f;
                        float W3avg = (cnt3   > 0) ? sumW3   / cnt3   : 0.0f;
                        xSemaphoreGive(statsMutex);
                        Serial.printf("sortSTAT: %.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
                                      Lavg, L2avg, L3avg, Wavg, W2avg, W3avg);

                    } else if (cmd == "reset") {
                        xSemaphoreTake(pipelineMutex, portMAX_DELAY);
                        memset(pipeline, 0, sizeof(pipeline));
                        xSemaphoreGive(pipelineMutex);

                        for (int a = 0; a < 3; a++) {
                            myAreaCount[a]    = 0;
                            inTransitCount[a] = 0;
                        }

                        xSemaphoreTake(statsMutex, portMAX_DELAY);
                        sumLAll = sumL2 = sumL3 = 0;
                        sumWAll = sumW2 = sumW3 = 0;
                        cntAll  = cnt2  = cnt3  = 0;
                        xSemaphoreGive(statsMutex);

                        xSemaphoreTake(reportMutex, portMAX_DELAY);
                        reportLogHead  = 0;
                        reportLogCount = 0;
                        xSemaphoreGive(reportMutex);

                        emergencyHalt = false;
                        g_gate1Ctrl   = true;
                        xSemaphoreTake(trackingMutex, portMAX_DELAY);
                        g_trackingCounter = 1;
                        xSemaphoreGive(trackingMutex);
                        // No response required per spec

                    } else if (cmd == "emergency") {
                        emergencyHalt = true;
                        g_gate1Ctrl = false;
                        g_gate2Ctrl = false;
                        g_gate3Ctrl = false;
                        g_gate4Ctrl = false;
                        Serial.println("EMERGENCY: System halted");
                    }

                    cmdIdx = 0;
                }
            } else if (cmdIdx < (int)sizeof(cmdBuf) - 1) {
                cmdBuf[cmdIdx++] = c;
            }
        }
        vTaskDelay(pdMS_TO_TICKS(50));
    }
}

// ============================================================================
// SETUP
// ============================================================================
void setup() {
    Serial.begin(115200);

    // GPIO for gate debug pins
    pinMode(gate2DebugPin, OUTPUT);
    pinMode(gate3DebugPin, OUTPUT);
    pinMode(gate4DebugPin, OUTPUT);
    digitalWrite(gate2DebugPin, LOW);
    digitalWrite(gate3DebugPin, LOW);
    digitalWrite(gate4DebugPin, LOW);

    memset(pipeline, 0, sizeof(pipeline));

    pipelineMutex  = xSemaphoreCreateMutex();
    statsMutex     = xSemaphoreCreateMutex();
    reportMutex    = xSemaphoreCreateMutex();
    trackingMutex  = xSemaphoreCreateMutex();

    if (!pipelineMutex || !statsMutex || !reportMutex || !trackingMutex) {
        Serial.println("FATAL: Mutex creation failed");
        while (1) vTaskDelay(portMAX_DELAY);
    }

    // Queue holds {riseUs, fallUs} pairs — 2 x int64_t = 16 bytes each
    g_edgeQueue = xQueueCreate(16, 2 * sizeof(int64_t));
    if (!g_edgeQueue) {
        Serial.println("FATAL: Queue creation failed");
        while (1) vTaskDelay(portMAX_DELAY);
    }

    // Core 0: product inspection pipeline (high priority)
    xTaskCreatePinnedToCore(productProcessingTask, "ProdProc",
                            8192, NULL, 10, NULL, 0);

    // Core 1: monitoring, reporting, commands
    xTaskCreatePinnedToCore(areaMonitorTask,    "AreaMon",
                            4096, NULL, 8, NULL, 1);
    xTaskCreatePinnedToCore(realTimeStatusTask, "RTStatus",
                            4096, NULL, 5, NULL, 1);
    xTaskCreatePinnedToCore(serialCommandTask,  "SerialCmd",
                            4096, NULL, 4, NULL, 1);

    // Must be called last — starts simulator and its own tasks
    startSimulator();
}

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

    // 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");
                g_simComplete = true;
                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);

    // ============================================================================
    // ROBUSTNESS TESTING OVERRIDES
    // Uncomment ONE block at a time to stress-test the controller.
    // Comment all blocks out before final submission.
    // ============================================================================

    // Test A: Very tight capacities (G1 should close after 2 products per area)
    // sp.C1 = 2; sp.C2 = 2; sp.C3 = 2;

    //  Test B: Large capacities (G1 should stay open throughout all 18 products)
    // sp.C1 = 500; sp.C2 = 500; sp.C3 = 500;

    //  Test C: Mixed/interleaved destination order (not A1-heavy at the start)
    // numProducts = 10;
    // productArray[0] = {sp.L_threshold - 0.1f, sp.W_threshold - 1.0f, 2001}; // A2
    // productArray[1] = {sp.L_threshold + 0.1f, sp.W_threshold + 1.0f, 2002}; // A3
    // productArray[2] = {sp.L_min - 0.5f, 90.0f, 2003};                       // A1 (size)
    // productArray[3] = {sp.L_threshold - 0.2f, sp.W_threshold - 0.5f, 2004}; // A2
    // productArray[4] = {sp.L_threshold + 0.3f, sp.W_threshold + 2.0f, 2005}; // A3
    // productArray[5] = {sp.L_min + 0.1f, sp.W_min - 5.0f, 2006};             // A1 (weight)
    // productArray[6] = {sp.L_threshold,   sp.W_threshold, 2007};              // A2 (boundary)
    // productArray[7] = {sp.L_max + 0.2f,  90.0f, 2008};                      // A1 (size)
    // productArray[8] = {sp.L_min + 0.2f,  sp.W_threshold - 1.0f, 2009};      // A2
    // productArray[9] = {sp.L_threshold + 0.5f, sp.W_threshold + 5.0f, 2010}; // A3

    printParameters();

    // Generate test product array
    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();

    // 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");
}