#include "Arduino.h"

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

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

  // ============================================================
  // CREATIVE ROBUSTNESS TEST:
  // Adversarial mixed-route gate-switching test
  //
  // Purpose:
  // - Forces rapid switching between A2, A3, G2 reject and G3 reject.
  // - Uses a reduced interval to increase overlap.
  // - Reaches Area 1 capacity exactly for ID 2245536 (C1 = 3),
  //   but should not overflow.
  //
  // Expected final result:
  // A1 = 3 products
  // A2 = 5 products
  // A3 = 5 products
  // Gate errors: G2=0, G3=0, G4=0
  // ============================================================

  sp.productInterval = 300;

  Serial.println("ROBUSTNESS TEST 3: Adversarial mixed-route gate-switching test");
  Serial.printf("productInterval temporarily set to %d ms\n", sp.productInterval);
  Serial.println("Expected final result: A1=3, A2=5, A3=5, all gate errors zero");

  float A2_L = sp.L_min + 0.10f;
  float A2_W = sp.W_min + 1.00f;

  float A3_L = sp.L_threshold + 0.30f;
  float A3_W = sp.W_threshold + 1.00f;

  float Lmid = (sp.L_min + sp.L_max) / 2.0f;
  float Wmid = (sp.W_min + sp.W_max) / 2.0f;

  // 1) A2: normal small/light
  productArray[idx++] = {A2_L, A2_W, 6001};

  // 2) A3: normal but large/heavy
  productArray[idx++] = {A3_L, A3_W, 6002};

  // 3) A2 again: forces G4 back to A2 route
  productArray[idx++] = {A2_L, A2_W, 6003};

  // 4) A1 by size: short product rejected at G2
  productArray[idx++] = {sp.L_min - 0.30f, Wmid, 6004};

  // 5) A3: valid product after G2 rejection
  productArray[idx++] = {A3_L, A3_W, 6005};

  // 6) A2: valid small/light after A3
  productArray[idx++] = {A2_L, A2_W, 6006};

  // 7) A1 by weight: valid size but too heavy, rejected at G3
  productArray[idx++] = {Lmid, sp.W_max + 3.00f, 6007};

  // 8) A3: valid large/heavy
  productArray[idx++] = {A3_L, A3_W, 6008};

  // 9) A2: valid small/light
  productArray[idx++] = {A2_L, A2_W, 6009};

  // 10) A3: valid large/heavy
  productArray[idx++] = {A3_L, A3_W, 6010};

  // 11) A2: final A2 product
  productArray[idx++] = {A2_L, A2_W, 6011};

  // 12) A3: final A3 product
  productArray[idx++] = {A3_L, A3_W, 6012};

  // 13) A1 by size: long product rejected at G2.
  // This brings A1 to exactly C1 = 3 for ID 2245536.
  productArray[idx++] = {sp.L_max + 0.30f, Wmid, 6013};

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

#include <string.h>

// ============================================================================
// SMARTSORT RTOS CONTROLLER - STUDENT ID 2245536
// ============================================================================
// Student implementation boundary:
// - The module scaffold, parameter generator, product generator and simulator
//   engine are kept unchanged.
// - Only the RTOS controller section between the two scaffold boundary comments
//   is implemented here.
//
// Design summary:
// - Core 0 is used for the timing-critical path: obstruction-edge processing,
//   length measurement, sensor-window timing, classification and G2/G3/G4
//   scheduling.
// - Core 1 is used for arrival tracking, operator commands and one-second
//   serial reporting.
// - The obstruction ISR only captures timestamps and sends a pair to a queue.
//   No heavy calculation is performed inside the ISR.
// - Queues separate the real-time control path from arrival tracking and gate
//   scheduling. A mutex protects shared counts, statistics and recent history.
// - G1 uses live occupancy plus predicted in-transit products and products still
//   under inspection, so it can close before an area overflows.
// - The clear and diag commands are practical validation helpers. clear models
//   an operator emptying a full area, while diag gives extra internal evidence
//   during testing. The required coursework commands stat, reset and emergency
//   are still implemented with the requested output format.
// ============================================================================

#define LENGTH_TOL_CM              0.005f
#define WEIGHT_TOL_G               0.005f
#define SENSOR_POLL_US             250
#define SENSOR_EDGE_GUARD_US       300
#define GATE_EARLY_SAFETY_US       900
#define GATE_RELEASE_MARGIN_US     3000
#define RECENT_HISTORY_SIZE        6
#define EDGE_QUEUE_LENGTH          16
#define GATE_EVENT_QUEUE_LENGTH    96
#define GATE_EVENT_POOL_SIZE       128
#define ARRIVAL_QUEUE_LENGTH       64
#define ARRIVAL_POOL_SIZE          64
#define COMMAND_BUFFER_LENGTH      32

// ============================================================================
// DATA TYPES
// ============================================================================

typedef struct {
    int64_t riseUs;
    int64_t fallUs;
} edge_pair_t;

typedef struct {
    uint32_t sequenceNo;
    int barcode;
    float lengthCm;
    float weightG;
    int destination;        // 1=A1, 2=A2, 3=A3
} recent_record_t;

typedef struct {
    int liveCount[3];       // live controller occupancy after clear operations
    int inTransit[3];       // predicted arrivals not yet completed
    int underInspection;    // products inside Conv1 before final destination known

    float totalLength;
    float totalWeight;
    int totalProducts;

    float area2Length;
    float area2Weight;
    int area2Products;

    float area3Length;
    float area3Weight;
    int area3Products;
} controller_state_t;

typedef struct {
    edge_pair_t edges;
    uint32_t sequenceNo;
    uint32_t epoch;
} product_task_arg_t;

typedef enum {
    GATE_ID_G2 = 2,
    GATE_ID_G3 = 3,
    GATE_ID_G4 = 4
} gate_id_t;

typedef struct {
    int64_t timeUs;
    gate_id_t gateId;
    bool state;
    uint32_t epoch;
} gate_event_t;

typedef struct {
    int64_t timeUs;
    uint32_t sequenceNo;
    int barcode;
    float lengthCm;
    float weightG;
    int destination;
    uint32_t epoch;
} arrival_event_t;

// ============================================================================
// GLOBAL RTOS OBJECTS AND CONTROLLER STATE
// ============================================================================

static QueueHandle_t edgeQueue = NULL;
static QueueHandle_t gateEventQueue = NULL;
static QueueHandle_t arrivalQueue = NULL;
static SemaphoreHandle_t stateMutex = NULL;

static volatile edge_pair_t activeEdgePair = {0, 0};
static volatile uint32_t controllerEpoch = 1;
static volatile bool emergencyActive = false;
static volatile bool capacityBlocked = false;
static volatile bool capacityMessageShown = false;
static volatile int blockedArea = 0;

static controller_state_t ctrlState = {0};
static recent_record_t recentHistory[RECENT_HISTORY_SIZE];
static int recentWriteIndex = 0;
static int recentCount = 0;
static uint32_t nextSequenceNo = 1;

// ============================================================================
// FORWARD DECLARATIONS
// ============================================================================

static int64_t travelTimeUs(float distanceCm, float speedCmPerSec);
static void waitUntilUsWithEpoch(int64_t targetUs, uint32_t epoch);
static bool epochExpired(uint32_t epoch);

static bool lengthValid(float lengthCm);
static bool weightValid(float weightG);
static bool routeToArea2(float lengthCm, float weightG);

static volatile bool* gateSignalPointer(gate_id_t gateId);
static void queueGateEvent(gate_id_t gateId, bool state, int64_t timeUs, uint32_t epoch);
static void scheduleGatePulse(gate_id_t gateId, bool requiredState,
                              int64_t checkTimeUs, float lengthCm, uint32_t epoch);

static void queueArrival(uint32_t sequenceNo, int barcode, float lengthCm, float weightG,
                         int destination, int64_t arrivalTimeUs, uint32_t epoch);

static void updateG1CapacityControl(void);
static void beginInspection(void);
static void cancelInspection(void);
static void reserveKnownDestination(int destination);
static void completeArrival(const arrival_event_t *event);
static void addRecentRecord(uint32_t sequenceNo, int barcode, float lengthCm,
                            float weightG, int destination);

static float readWeightAtCentre(int64_t centreTimeUs, float lengthCm, uint32_t epoch);
static int readBarcodeAtCentre(int64_t centreTimeUs, float lengthCm, uint32_t epoch);

static void handleSerialCommand(const char *command);
static void resetControllerState(void);
static void clearAreasByOperator(void);

void productControlTask(void *pvParameters);
void productProcessingTask(void *pvParameters);
void gateSchedulerTask(void *pvParameters);
void arrivalSchedulerTask(void *pvParameters);
void commandTask(void *pvParameters);
void statusTask(void *pvParameters);

// ============================================================================
// TIME AND CLASSIFICATION HELPERS
// ============================================================================

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

static bool epochExpired(uint32_t epoch) {
    return epoch != controllerEpoch;
}

static void waitUntilUsWithEpoch(int64_t targetUs, uint32_t epoch) {
    while (!emergencyActive && !epochExpired(epoch)) {
        int64_t nowUs = esp_timer_get_time();
        int64_t remainingUs = targetUs - nowUs;

        if (remainingUs <= 0) {
            return;
        }

        if (remainingUs > 4500) {
            int delayMs = (int)((remainingUs - 2000) / 1000);
            if (delayMs < 1) delayMs = 1;
            vTaskDelay(pdMS_TO_TICKS(delayMs));
        } else {
            delayMicroseconds((uint32_t)remainingUs);
            return;
        }
    }
}

static bool lengthValid(float lengthCm) {
    return (lengthCm >= (sp.L_min - LENGTH_TOL_CM) &&
            lengthCm <= (sp.L_max + LENGTH_TOL_CM));
}

static bool weightValid(float weightG) {
    return (weightG >= (sp.W_min - WEIGHT_TOL_G) &&
            weightG <= (sp.W_max + WEIGHT_TOL_G));
}

static bool routeToArea2(float lengthCm, float weightG) {
    return (lengthCm <= (sp.L_threshold + LENGTH_TOL_CM) &&
            weightG <= (sp.W_threshold + WEIGHT_TOL_G));
}

// ============================================================================
// CAPACITY, STATE AND REPORTING HELPERS
// ============================================================================

static void updateG1CapacityControl(void) {
    if (stateMutex == NULL) {
        return;
    }

    if (sp.C1 <= 0 || sp.C2 <= 0 || sp.C3 <= 0) {
        g_gate1Ctrl = true;
        capacityBlocked = false;
        capacityMessageShown = false;
        blockedArea = 0;
        return;
    }

    xSemaphoreTake(stateMutex, portMAX_DELAY);

    int limit[3] = {sp.C1, sp.C2, sp.C3};
    int fullArea = 0;

    for (int i = 0; i < 3; i++) {
        int predicted = ctrlState.liveCount[i] + ctrlState.inTransit[i];

        // Conservative protection: products still under inspection could still
        // be routed to any destination, especially Area 1 after G2/G3 checks.
        if (ctrlState.underInspection > 0) {
            predicted += ctrlState.underInspection;
        }

        if (predicted >= limit[i]) {
            fullArea = i + 1;
            break;
        }
    }

    if (emergencyActive) {
        g_gate1Ctrl = false;
    } else if (fullArea != 0) {
        g_gate1Ctrl = false;
        capacityBlocked = true;
        blockedArea = fullArea;
    } else {
        g_gate1Ctrl = true;
        capacityBlocked = false;
        capacityMessageShown = false;
        blockedArea = 0;
    }

    xSemaphoreGive(stateMutex);
}

static void beginInspection(void) {
    if (stateMutex == NULL) return;
    xSemaphoreTake(stateMutex, portMAX_DELAY);
    ctrlState.underInspection++;
    xSemaphoreGive(stateMutex);
    updateG1CapacityControl();
}

static void cancelInspection(void) {
    if (stateMutex == NULL) return;
    xSemaphoreTake(stateMutex, portMAX_DELAY);
    if (ctrlState.underInspection > 0) {
        ctrlState.underInspection--;
    }
    xSemaphoreGive(stateMutex);
    updateG1CapacityControl();
}

static void reserveKnownDestination(int destination) {
    int index = destination - 1;
    if (index < 0 || index > 2 || stateMutex == NULL) return;

    xSemaphoreTake(stateMutex, portMAX_DELAY);
    if (ctrlState.underInspection > 0) {
        ctrlState.underInspection--;
    }
    ctrlState.inTransit[index]++;
    xSemaphoreGive(stateMutex);

    updateG1CapacityControl();
}

static void addRecentRecord(uint32_t sequenceNo, int barcode, float lengthCm,
                            float weightG, int destination) {
    if (stateMutex == NULL) return;

    xSemaphoreTake(stateMutex, portMAX_DELAY);

    recentHistory[recentWriteIndex].sequenceNo = sequenceNo;
    recentHistory[recentWriteIndex].barcode = barcode;
    recentHistory[recentWriteIndex].lengthCm = lengthCm;
    recentHistory[recentWriteIndex].weightG = weightG;
    recentHistory[recentWriteIndex].destination = destination;

    recentWriteIndex = (recentWriteIndex + 1) % RECENT_HISTORY_SIZE;
    if (recentCount < RECENT_HISTORY_SIZE) {
        recentCount++;
    }

    xSemaphoreGive(stateMutex);
}

static void completeArrival(const arrival_event_t *event) {
    if (event == NULL || stateMutex == NULL) return;

    int index = event->destination - 1;
    if (index < 0 || index > 2) return;

    xSemaphoreTake(stateMutex, portMAX_DELAY);

    if (ctrlState.inTransit[index] > 0) {
        ctrlState.inTransit[index]--;
    }

    ctrlState.liveCount[index]++;
    ctrlState.totalLength += event->lengthCm;
    ctrlState.totalWeight += event->weightG;
    ctrlState.totalProducts++;

    if (event->destination == 2) {
        ctrlState.area2Length += event->lengthCm;
        ctrlState.area2Weight += event->weightG;
        ctrlState.area2Products++;
    } else if (event->destination == 3) {
        ctrlState.area3Length += event->lengthCm;
        ctrlState.area3Weight += event->weightG;
        ctrlState.area3Products++;
    }

    xSemaphoreGive(stateMutex);

    addRecentRecord(event->sequenceNo, event->barcode, event->lengthCm,
                    event->weightG, event->destination);
    updateG1CapacityControl();
}

// ============================================================================
// SENSOR WINDOW READERS
// ============================================================================

static float readWeightAtCentre(int64_t centreTimeUs, float lengthCm, uint32_t epoch) {
    float value = 0.0f;
    int64_t halfWindowUs = travelTimeUs(lengthCm * 0.10f, sp.v1);
    int64_t startUs = centreTimeUs - halfWindowUs + SENSOR_EDGE_GUARD_US;
    int64_t endUs = centreTimeUs + halfWindowUs - SENSOR_EDGE_GUARD_US;

    waitUntilUsWithEpoch(startUs, epoch);

    while (!emergencyActive && !epochExpired(epoch) && esp_timer_get_time() <= endUs) {
        if (g_weightSensor > 0.0f) {
            value = g_weightSensor;
            break;
        }
        delayMicroseconds(SENSOR_POLL_US);
    }

    return value;
}

static int readBarcodeAtCentre(int64_t centreTimeUs, float lengthCm, uint32_t epoch) {
    int value = 0;
    int64_t halfWindowUs = travelTimeUs(lengthCm * 0.10f, sp.v1);
    int64_t startUs = centreTimeUs - halfWindowUs + SENSOR_EDGE_GUARD_US;
    int64_t endUs = centreTimeUs + halfWindowUs - SENSOR_EDGE_GUARD_US;

    waitUntilUsWithEpoch(startUs, epoch);

    while (!emergencyActive && !epochExpired(epoch) && esp_timer_get_time() <= endUs) {
        if (g_barcodeReader != 0) {
            value = g_barcodeReader;
            break;
        }
        delayMicroseconds(SENSOR_POLL_US);
    }

    return value;
}

// ============================================================================
// FIXED-EVENT GATE SCHEDULER
// ============================================================================

static volatile bool* gateSignalPointer(gate_id_t gateId) {
    if (gateId == GATE_ID_G2) return &g_gate2Ctrl;
    if (gateId == GATE_ID_G3) return &g_gate3Ctrl;
    if (gateId == GATE_ID_G4) return &g_gate4Ctrl;
    return NULL;
}

static void queueGateEvent(gate_id_t gateId, bool state, int64_t timeUs, uint32_t epoch) {
    if (gateEventQueue == NULL || epochExpired(epoch)) return;

    gate_event_t event;
    event.timeUs = timeUs;
    event.gateId = gateId;
    event.state = state;
    event.epoch = epoch;

    xQueueSend(gateEventQueue, &event, pdMS_TO_TICKS(20));
}

static void scheduleGatePulse(gate_id_t gateId, bool requiredState,
                              int64_t checkTimeUs, float lengthCm, uint32_t epoch) {
    int64_t leadUs = (int64_t)(sp.T_gate * 1000.0f) + GATE_EARLY_SAFETY_US;
    int64_t setTimeUs = checkTimeUs - leadUs;
    int64_t releaseTimeUs = checkTimeUs + travelTimeUs(lengthCm, sp.v1) + GATE_RELEASE_MARGIN_US;

    queueGateEvent(gateId, requiredState, setTimeUs, epoch);

    // All pushers are returned to the safe/default false state after the product clears.
    // For G4 this means defaulting back to the Area 2 path between products.
    queueGateEvent(gateId, false, releaseTimeUs, epoch);
}

void gateSchedulerTask(void *pvParameters) {
    gate_event_t pending[GATE_EVENT_POOL_SIZE];
    int pendingCount = 0;

    while (true) {
        int64_t nowUs = esp_timer_get_time();

        // Execute all due events first.
        for (int i = 0; i < pendingCount; ) {
            if (pending[i].timeUs <= nowUs) {
                if (!emergencyActive && !epochExpired(pending[i].epoch)) {
                    volatile bool *sig = gateSignalPointer(pending[i].gateId);
                    if (sig != NULL) {
                        *sig = pending[i].state;
                    }
                }
                pending[i] = pending[pendingCount - 1];
                pendingCount--;
            } else {
                i++;
            }
        }

        // Find the next due event to set a bounded queue wait.
        int64_t nextDueUs = -1;
        for (int i = 0; i < pendingCount; i++) {
            if (nextDueUs < 0 || pending[i].timeUs < nextDueUs) {
                nextDueUs = pending[i].timeUs;
            }
        }

        TickType_t waitTicks = pdMS_TO_TICKS(20);
        if (nextDueUs >= 0) {
            int64_t remainingUs = nextDueUs - esp_timer_get_time();
            if (remainingUs <= 0) {
                waitTicks = 0;
            } else if (remainingUs < 20000) {
                waitTicks = pdMS_TO_TICKS(1);
            } else {
                waitTicks = pdMS_TO_TICKS((remainingUs / 1000) > 20 ? 20 : (remainingUs / 1000));
                if (waitTicks < 1) waitTicks = 1;
            }
        }

        gate_event_t incoming;
        if (xQueueReceive(gateEventQueue, &incoming, waitTicks) == pdTRUE) {
            if (!epochExpired(incoming.epoch) && pendingCount < GATE_EVENT_POOL_SIZE) {
                pending[pendingCount++] = incoming;
            }

            // Drain any immediately available events so the pool remains time ordered by due checks,
            // not by queue arrival.
            while (xQueueReceive(gateEventQueue, &incoming, 0) == pdTRUE) {
                if (!epochExpired(incoming.epoch) && pendingCount < GATE_EVENT_POOL_SIZE) {
                    pending[pendingCount++] = incoming;
                }
            }
        }
    }
}

// ============================================================================
// ARRIVAL SCHEDULER ON CORE 1
// ============================================================================

static void queueArrival(uint32_t sequenceNo, int barcode, float lengthCm, float weightG,
                         int destination, int64_t arrivalTimeUs, uint32_t epoch) {
    if (arrivalQueue == NULL || epochExpired(epoch)) return;

    arrival_event_t event;
    event.timeUs = arrivalTimeUs;
    event.sequenceNo = sequenceNo;
    event.barcode = barcode;
    event.lengthCm = lengthCm;
    event.weightG = weightG;
    event.destination = destination;
    event.epoch = epoch;

    xQueueSend(arrivalQueue, &event, pdMS_TO_TICKS(20));
}

void arrivalSchedulerTask(void *pvParameters) {
    arrival_event_t pending[ARRIVAL_POOL_SIZE];
    int pendingCount = 0;

    while (true) {
        int64_t nowUs = esp_timer_get_time();

        for (int i = 0; i < pendingCount; ) {
            if (pending[i].timeUs <= nowUs) {
                if (!emergencyActive && !epochExpired(pending[i].epoch)) {
                    completeArrival(&pending[i]);
                }
                pending[i] = pending[pendingCount - 1];
                pendingCount--;
            } else {
                i++;
            }
        }

        int64_t nextDueUs = -1;
        for (int i = 0; i < pendingCount; i++) {
            if (nextDueUs < 0 || pending[i].timeUs < nextDueUs) {
                nextDueUs = pending[i].timeUs;
            }
        }

        TickType_t waitTicks = pdMS_TO_TICKS(25);
        if (nextDueUs >= 0) {
            int64_t remainingUs = nextDueUs - esp_timer_get_time();
            if (remainingUs <= 0) {
                waitTicks = 0;
            } else if (remainingUs < 25000) {
                waitTicks = pdMS_TO_TICKS(1);
            } else {
                waitTicks = pdMS_TO_TICKS((remainingUs / 1000) > 25 ? 25 : (remainingUs / 1000));
                if (waitTicks < 1) waitTicks = 1;
            }
        }

        arrival_event_t incoming;
        if (xQueueReceive(arrivalQueue, &incoming, waitTicks) == pdTRUE) {
            if (!epochExpired(incoming.epoch) && pendingCount < ARRIVAL_POOL_SIZE) {
                pending[pendingCount++] = incoming;
            }

            while (xQueueReceive(arrivalQueue, &incoming, 0) == pdTRUE) {
                if (!epochExpired(incoming.epoch) && pendingCount < ARRIVAL_POOL_SIZE) {
                    pending[pendingCount++] = incoming;
                }
            }
        }
    }
}

// ============================================================================
// OBSTRUCTION SENSOR ISR
// ============================================================================

void IRAM_ATTR obstructionSensorInterrupt() {
    BaseType_t higherPriorityTaskWoken = pdFALSE;

    if (g_obstructionSensor == true) {
        activeEdgePair.riseUs = esp_timer_get_time();
    } else {
        activeEdgePair.fallUs = esp_timer_get_time();
        if (edgeQueue != NULL) {
            xQueueSendFromISR(edgeQueue, (void*)&activeEdgePair, &higherPriorityTaskWoken);
        }
        if (higherPriorityTaskWoken) {
            portYIELD_FROM_ISR();
        }
    }
}

// ============================================================================
// PRODUCT DISPATCHER AND PER-PRODUCT CONTROL
// ============================================================================

void productProcessingTask(void *pvParameters) {
    edge_pair_t edges;

    while (true) {
        if (xQueueReceive(edgeQueue, &edges, portMAX_DELAY) == pdTRUE) {
            if (emergencyActive) {
                continue;
            }

            product_task_arg_t *arg = (product_task_arg_t*)pvPortMalloc(sizeof(product_task_arg_t));
            if (arg == NULL) {
                continue;
            }

            arg->edges = edges;
            arg->sequenceNo = nextSequenceNo++;
            arg->epoch = controllerEpoch;

            BaseType_t ok = xTaskCreatePinnedToCore(
                productControlTask,
                "ProductControl",
                4096,
                arg,
                8,
                NULL,
                0
            );

            if (ok != pdPASS) {
                vPortFree(arg);
            }
        }
    }
}

void productControlTask(void *pvParameters) {
    product_task_arg_t *arg = (product_task_arg_t*)pvParameters;
    if (arg == NULL) {
        vTaskDelete(NULL);
        return;
    }

    edge_pair_t edges = arg->edges;
    uint32_t seq = arg->sequenceNo;
    uint32_t myEpoch = arg->epoch;
    vPortFree(arg);

    if (emergencyActive || epochExpired(myEpoch)) {
        vTaskDelete(NULL);
        return;
    }

    int64_t blockUs = edges.fallUs - edges.riseUs;
    if (blockUs <= 0) {
        vTaskDelete(NULL);
        return;
    }

    beginInspection();

    float lengthCm = (float)blockUs * sp.v1 / 1000000.0f;
    float weightG = 0.0f;
    int barcode = 0;
    int destination = 0;

    int64_t g2CheckUs = edges.riseUs + travelTimeUs(sp.d0, sp.v1);
    int64_t g3CheckUs = edges.riseUs + travelTimeUs(sp.d2, sp.v1);
    int64_t g4CheckUs = edges.riseUs + travelTimeUs(sp.d4, sp.v1);

    // 1) G2: abnormal size is diverted to Area 1 before weight/barcode.
    if (!lengthValid(lengthCm)) {
        destination = 1;
        reserveKnownDestination(destination);
        scheduleGatePulse(GATE_ID_G2, true, g2CheckUs, lengthCm, myEpoch);
        queueArrival(seq, barcode, lengthCm, weightG, destination,
                     g2CheckUs + 5000, myEpoch);
        vTaskDelete(NULL);
        return;
    }

    scheduleGatePulse(GATE_ID_G2, false, g2CheckUs, lengthCm, myEpoch);

    if (emergencyActive || epochExpired(myEpoch)) {
        cancelInspection();
        vTaskDelete(NULL);
        return;
    }

    // 2) Weight window: product centre at d1.
    float weightCentreDistance = sp.d1 - (lengthCm / 2.0f);
    if (weightCentreDistance < 0.0f) weightCentreDistance = 0.0f;
    int64_t weightCentreUs = edges.fallUs + travelTimeUs(weightCentreDistance, sp.v1);
    weightG = readWeightAtCentre(weightCentreUs, lengthCm, myEpoch);

    if (emergencyActive || epochExpired(myEpoch)) {
        cancelInspection();
        vTaskDelete(NULL);
        return;
    }

    // 3) G3: abnormal weight is diverted to Area 1 before barcode.
    if (!weightValid(weightG)) {
        destination = 1;
        reserveKnownDestination(destination);
        scheduleGatePulse(GATE_ID_G3, true, g3CheckUs, lengthCm, myEpoch);
        queueArrival(seq, barcode, lengthCm, weightG, destination,
                     g3CheckUs + 5000, myEpoch);
        vTaskDelete(NULL);
        return;
    }

    scheduleGatePulse(GATE_ID_G3, false, g3CheckUs, lengthCm, myEpoch);

    if (emergencyActive || epochExpired(myEpoch)) {
        cancelInspection();
        vTaskDelete(NULL);
        return;
    }

    // 4) Barcode window: only products passing G2/G3 reach the scanner.
    float barcodeCentreDistance = sp.d3 - (lengthCm / 2.0f);
    if (barcodeCentreDistance < 0.0f) barcodeCentreDistance = 0.0f;
    int64_t barcodeCentreUs = edges.fallUs + travelTimeUs(barcodeCentreDistance, sp.v1);
    barcode = readBarcodeAtCentre(barcodeCentreUs, lengthCm, myEpoch);

    if (emergencyActive || epochExpired(myEpoch)) {
        cancelInspection();
        vTaskDelete(NULL);
        return;
    }

    // 5) G4: final route to Area 2 or Area 3.
    if (routeToArea2(lengthCm, weightG)) {
        destination = 2;
        reserveKnownDestination(destination);
        scheduleGatePulse(GATE_ID_G4, false, g4CheckUs, lengthCm, myEpoch);
        queueArrival(seq, barcode, lengthCm, weightG, destination,
                     g4CheckUs + travelTimeUs(sp.d5, sp.v2), myEpoch);
    } else {
        destination = 3;
        reserveKnownDestination(destination);
        scheduleGatePulse(GATE_ID_G4, true, g4CheckUs, lengthCm, myEpoch);
        queueArrival(seq, barcode, lengthCm, weightG, destination,
                     g4CheckUs + travelTimeUs(sp.d6, sp.v3), myEpoch);
    }

    vTaskDelete(NULL);
}

// ============================================================================
// COMMANDS, RESET, CLEAR AND STATUS REPORTING
// ============================================================================

static void resetControllerState(void) {
    controllerEpoch++;
    if (controllerEpoch == 0) controllerEpoch = 1;

    if (edgeQueue != NULL) xQueueReset(edgeQueue);
    if (gateEventQueue != NULL) xQueueReset(gateEventQueue);
    if (arrivalQueue != NULL) xQueueReset(arrivalQueue);

    if (stateMutex != NULL) {
        xSemaphoreTake(stateMutex, portMAX_DELAY);
        memset(&ctrlState, 0, sizeof(ctrlState));
        memset(recentHistory, 0, sizeof(recentHistory));
        recentWriteIndex = 0;
        recentCount = 0;
        nextSequenceNo = 1;
        xSemaphoreGive(stateMutex);
    }

    emergencyActive = false;
    capacityBlocked = false;
    capacityMessageShown = false;
    blockedArea = 0;

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

static void clearAreasByOperator(void) {
    if (stateMutex != NULL) {
        xSemaphoreTake(stateMutex, portMAX_DELAY);
        ctrlState.liveCount[0] = 0;
        ctrlState.liveCount[1] = 0;
        ctrlState.liveCount[2] = 0;
        xSemaphoreGive(stateMutex);
    }

    capacityBlocked = false;
    capacityMessageShown = false;
    blockedArea = 0;
    updateG1CapacityControl();
}

static void handleSerialCommand(const char *command) {
    if (command == NULL || command[0] == '\0') return;

    if (strcmp(command, "stat") == 0) {
        float Lavg = 0.0f, Wavg = 0.0f;
        float L2avg = 0.0f, W2avg = 0.0f;
        float L3avg = 0.0f, W3avg = 0.0f;

        if (stateMutex != NULL) {
            xSemaphoreTake(stateMutex, portMAX_DELAY);
            if (ctrlState.totalProducts > 0) {
                Lavg = ctrlState.totalLength / ctrlState.totalProducts;
                Wavg = ctrlState.totalWeight / ctrlState.totalProducts;
            }
            if (ctrlState.area2Products > 0) {
                L2avg = ctrlState.area2Length / ctrlState.area2Products;
                W2avg = ctrlState.area2Weight / ctrlState.area2Products;
            }
            if (ctrlState.area3Products > 0) {
                L3avg = ctrlState.area3Length / ctrlState.area3Products;
                W3avg = ctrlState.area3Weight / ctrlState.area3Products;
            }
            xSemaphoreGive(stateMutex);
        }

        Serial.printf("sortSTAT: %.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
                      Lavg, L2avg, L3avg, Wavg, W2avg, W3avg);
    } else if (strcmp(command, "clear") == 0) {
        clearAreasByOperator();
    } else if (strcmp(command, "reset") == 0) {
        resetControllerState();
    } else if (strcmp(command, "emergency") == 0) {
        emergencyActive = true;
        controllerEpoch++;
        if (controllerEpoch == 0) controllerEpoch = 1;

        if (edgeQueue != NULL) xQueueReset(edgeQueue);
        if (gateEventQueue != NULL) xQueueReset(gateEventQueue);
        if (arrivalQueue != NULL) xQueueReset(arrivalQueue);

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

        Serial.println("EMERGENCY: System halted");
    } else if (strcmp(command, "diag") == 0) {
        if (stateMutex != NULL) {
            xSemaphoreTake(stateMutex, portMAX_DELAY);
            Serial.printf("sortDIAG: Live=%d,%d,%d Transit=%d,%d,%d UnderInspection=%d G1=%s Emergency=%d Errors=%d,%d,%d\n",
                          ctrlState.liveCount[0], ctrlState.liveCount[1], ctrlState.liveCount[2],
                          ctrlState.inTransit[0], ctrlState.inTransit[1], ctrlState.inTransit[2],
                          ctrlState.underInspection,
                          g_gate1Ctrl ? "OPEN" : "CLOSED",
                          emergencyActive ? 1 : 0,
                          errorGateCNT[0], errorGateCNT[1], errorGateCNT[2]);
            xSemaphoreGive(stateMutex);
        }
    }
}

void commandTask(void *pvParameters) {
    char buffer[COMMAND_BUFFER_LENGTH];
    uint8_t pos = 0;

    while (true) {
        while (Serial.available() > 0) {
            char c = (char)Serial.read();
            if (c == '\r') {
                continue;
            }
            if (c == '\n') {
                buffer[pos] = '\0';
                handleSerialCommand(buffer);
                pos = 0;
            } else if (pos < COMMAND_BUFFER_LENGTH - 1) {
                buffer[pos++] = c;
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

void statusTask(void *pvParameters) {
    TickType_t lastWake = xTaskGetTickCount();

    while (true) {
        updateG1CapacityControl();

        bool printCapacityMessage = false;
        int messageArea = 0;

        if (capacityBlocked && !capacityMessageShown) {
            printCapacityMessage = true;
            messageArea = blockedArea;
            capacityMessageShown = true;
        }

        int liveSnapshot[3] = {0, 0, 0};
        recent_record_t recentSnapshot[RECENT_HISTORY_SIZE];
        int snapshotCount = 0;

        // Keep the critical section short: copy the shared state, release the
        // mutex, then print to Serial outside the protected region. Serial
        // output can be slow, so this avoids blocking product tracking and
        // command handling while the status line is being transmitted.
        if (stateMutex != NULL) {
            xSemaphoreTake(stateMutex, portMAX_DELAY);

            liveSnapshot[0] = ctrlState.liveCount[0];
            liveSnapshot[1] = ctrlState.liveCount[1];
            liveSnapshot[2] = ctrlState.liveCount[2];

            snapshotCount = recentCount;
            if (snapshotCount > RECENT_HISTORY_SIZE) {
                snapshotCount = RECENT_HISTORY_SIZE;
            }

            for (int i = 0; i < snapshotCount; i++) {
                int idx = (recentWriteIndex - snapshotCount + i + RECENT_HISTORY_SIZE) % RECENT_HISTORY_SIZE;
                recentSnapshot[i] = recentHistory[idx];
            }

            xSemaphoreGive(stateMutex);
        }

        if (printCapacityMessage) {
            Serial.printf("Area %d has reached full capacity. G1 is closed. Type clear after the operator empties the area.\n",
                          messageArea);
        }

        Serial.printf("sortRT: %03d,%03d,%03d",
                      liveSnapshot[0], liveSnapshot[1], liveSnapshot[2]);

        for (int i = 0; i < snapshotCount; i++) {
            recent_record_t r = recentSnapshot[i];

            if (r.barcode > 0) {
                Serial.printf(",TN%d,%.1f,%.1f,A%d",
                              r.barcode, r.lengthCm, r.weightG, r.destination);
            } else {
                Serial.printf(",0,%.1f,%.1f,A%d",
                              r.lengthCm, r.weightG, r.destination);
            }
        }
        Serial.println();

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

// ============================================================================
// SETUP AND LOOP
// ============================================================================

void setup() {
    Serial.begin(115200);

    edgeQueue = xQueueCreate(EDGE_QUEUE_LENGTH, sizeof(edge_pair_t));
    gateEventQueue = xQueueCreate(GATE_EVENT_QUEUE_LENGTH, sizeof(gate_event_t));
    arrivalQueue = xQueueCreate(ARRIVAL_QUEUE_LENGTH, sizeof(arrival_event_t));
    stateMutex = xSemaphoreCreateMutex();

    if (edgeQueue == NULL) Serial.println("ERROR: edgeQueue creation failed");
    if (gateEventQueue == NULL) Serial.println("ERROR: gateEventQueue creation failed");
    if (arrivalQueue == NULL) Serial.println("ERROR: arrivalQueue creation failed");
    if (stateMutex == NULL) Serial.println("ERROR: stateMutex creation failed");

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

    xTaskCreatePinnedToCore(
        gateSchedulerTask,
        "GateScheduler",
        4096,
        NULL,
        10,
        NULL,
        0
    );

    xTaskCreatePinnedToCore(
        productProcessingTask,
        "ProductDispatcher",
        4096,
        NULL,
        9,
        NULL,
        0
    );

    xTaskCreatePinnedToCore(
        arrivalSchedulerTask,
        "ArrivalScheduler",
        4096,
        NULL,
        5,
        NULL,
        1
    );

    xTaskCreatePinnedToCore(
        commandTask,
        "CommandTask",
        4096,
        NULL,
        4,
        NULL,
        1
    );

    xTaskCreatePinnedToCore(
        statusTask,
        "StatusTask",
        4096,
        NULL,
        2,
        NULL,
        1
    );

    // Start the provided simulator only after the student's RTOS objects are ready.
    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");
                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();

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