#include "Arduino.h"

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

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

  // The validation list is generated using the current capacity values instead
  // of assuming the original fixed 18-product sequence. For student 2163428,
  // C1 is 7, so the number of Area 1 reject products is kept below capacity
  // while still covering G2 size rejection, G3 weight rejection, A2 routing,
  // A3 routing and boundary cases.

  int a1Budget = (sp.C1 > 1) ? min(6, sp.C1 - 1) : 0;
  int a2Budget = (sp.C2 > 1) ? min(5, sp.C2 - 1) : 0;
  int a3Budget = (sp.C3 > 1) ? min(5, sp.C3 - 1) : 0;

  int a1Used = 0;
  int a2Used = 0;
  int a3Used = 0;

  // --------------------------------------------------------------------------
  // Area 2 products: normal products that are small AND light.
  // These are placed first so the run demonstrates normal flow before capacity
  // is stressed by reject products.
  // --------------------------------------------------------------------------
  if (a2Used < a2Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_min + 0.10f, sp.W_min + 1.0f, 1009};
    a2Used++;
  }

  if (a2Used < a2Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_threshold, sp.W_threshold, 1010};  // exact threshold -> A2
    a2Used++;
  }

  if (a2Used < a2Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_threshold - 0.10f, sp.W_threshold - 1.0f, 1011};
    a2Used++;
  }

  if (a2Used < a2Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_min, sp.W_min, 1016};              // exact lower limits -> A2
    a2Used++;
  }

  if (a2Used < a2Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {Lmid, Wmid, 1018};                      // middle normal case -> A2
    a2Used++;
  }

  // --------------------------------------------------------------------------
  // Area 3 products: normal products that are large OR heavy.
  // --------------------------------------------------------------------------
  if (a3Used < a3Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_threshold + 0.10f, sp.W_threshold - 1.0f, 1012};
    a3Used++;
  }

  if (a3Used < a3Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_min + 0.20f, sp.W_threshold + 1.0f, 1013};
    a3Used++;
  }

  if (a3Used < a3Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_max - 0.10f, sp.W_max - 1.0f, 1014};
    a3Used++;
  }

  if (a3Used < a3Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_threshold + 0.20f, sp.W_threshold + 2.0f, 1015};
    a3Used++;
  }

  if (a3Used < a3Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_max, sp.W_max, 1017};              // exact upper limits -> A3
    a3Used++;
  }

  // --------------------------------------------------------------------------
  // Area 1 products: reject cases.
  // Kept within C1 - 1 so the default validation run can complete without a
  // permanent G1 capacity hold.
  // --------------------------------------------------------------------------
  if (a1Used < a1Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_min - 0.50f, Wmid, 1001};          // too short -> G2
    a1Used++;
  }

  if (a1Used < a1Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_max + 0.50f, Wmid, 1003};          // too long -> G2
    a1Used++;
  }

  if (a1Used < a1Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {Lmid, sp.W_min - 5.0f, 1005};           // too light -> G3
    a1Used++;
  }

  if (a1Used < a1Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {Lmid, sp.W_max + 5.0f, 1007};           // too heavy -> G3
    a1Used++;
  }

  if (a1Used < a1Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_min + 0.10f, sp.W_min - 3.0f, 1006}; // normal size, light -> G3
    a1Used++;
  }

  if (a1Used < a1Budget && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {sp.L_max - 0.10f, sp.W_max + 3.0f, 1008}; // normal size, heavy -> G3
    a1Used++;
  }

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

// ESP32 Board Library (ESP32 by Espressif) v3.3.7  —  Arduino-ESP32 FreeRTOS
//
// =============================================================================
// SmartSort RTOS Controller   (Student 2163428 — M21410 2025/26)
// =============================================================================
//
//  Dual-core architecture
//  ----------------------
//    Core 0  ─  Conveyor 1 inspection pipeline
//               • obstructionSensorInterrupt (HW timer ISR context)
//               • inspectionDispatcher  (consumes edge timestamps)
//               • productPipelineTask   (one short-lived worker per product;
//                                        walks the product through G2, weigh,
//                                        G3, barcode, G4 using just-in-time
//                                        sleeps anchored to the rise-edge
//                                        timestamp captured by the ISR)
//
//    Core 1  ─  Conveyors 2/3 + system management
//               • conv23TrackerTask     (commits arrivals in the right area)
//               • g1ControlTask         (intake gate / capacity guard)
//               • rtStatusReportTask      (1 Hz "sortRT:" telemetry)
//               • serialCommandTask     ("stat" / "reset" / "emergency")
//
//  Inter-task plumbing
//  -------------------
//    Queue   g_edgeQueue       ISR  →  inspectionDispatcher        (Core 0)
//    Queue   g_arrivalQueue    pipeline  →  conv23TrackerTask      (Core 1)
//    Mutex   g_stateMutex      guards stats / counts / recent list
//    Mutex   g_serialMutex     serialises Serial writes between tasks
//    Flag    g_emergency       latches halt across all tasks
//
//  Timing strategy
//  ---------------
//  Every per-product action is anchored to the rise-edge timestamp captured
//  by the obstruction-sensor ISR. The simulator schedules each event
//  deterministically off the same rise-edge, so as long as we wake at the
//  matching absolute time minus T_gate (for gates) or at the window centre
//  (for sensors), the timing aligns to the µs.
// =============================================================================

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

// ----- Tunables ---------------------------------------------------------------
#define BAUD_RATE            115200
#define STATUS_PERIOD_MS     1000     // sortRT cadence (spec: every second)
#define G1_REFRESH_MS        25       // capacity check faster than productInterval
#define SERIAL_POLL_MS       10       // command-loop responsiveness
#define MAX_ACTIVE_PRODUCTS  8        // pool size — Conv1 fits ~3 products at once
#define RECENT_LIST_CAP      24       // newly-completed products per 1-s window
#define EMERGENCY_POLL_MS    20       // bound on emergency reaction inside pipeline

// Floating-point tolerances for boundary comparisons.
//   Lengths are derived from durUs*v1/1e6 where durUs is integer microseconds;
//   the worst-case rounding error is ~v1·1µs ≈ 1e-4 cm. We use a generous
//   ±10 µm window to defeat any IEEE-754 round-trip surprises near L_min,
//   L_max, and L_threshold (e.g. product 1010 = exactly at L_threshold).
//   Weights are reported by the simulator with float precision, so a 0.5 g
//   tolerance covers any FP wobble while staying well inside any realistic
//   load-cell drift.
#define L_EPS                0.01f
#define W_EPS                0.5f

// Compile-time switch: 1 = verbose dev prints; 0 = clean output for marking.
// Final submission MUST leave this 0 so the only chatter on the serial line
// is the simulator's own status lines plus our spec-mandated sortRT/sortSTAT
// telemetry.
#define DEBUG_MODE           0

#if DEBUG_MODE
  #define DBG_PRINTF(...)  do { Serial.printf(__VA_ARGS__); } while (0)
  #define DBG_PRINTLN(s)   do { Serial.println(s); }         while (0)
#else
  #define DBG_PRINTF(...)  do { } while (0)
  #define DBG_PRINTLN(s)   do { } while (0)
#endif

// =============================================================================
// Per-product context (lives in g_active pool for the product's lifetime)
// =============================================================================
typedef enum {
    ST_FREE = 0,
    ST_INSPECTING,
    ST_TRANSIT,
    ST_DONE
} prod_state_t;

typedef struct {
    int           slot;
    volatile prod_state_t state;
    int64_t       riseEdgeUs;     // ESP timer time at rising edge
    int64_t       fallEdgeUs;     // ESP timer time at falling edge
    float         length;         // measured length (cm)
    float         weight;         // measured weight (g)   — 0.0 if G2-diverted
    int           barcode;        // 0 if failed / not scanned
    int           destArea;       // 1, 2 or 3
    bool          sizeOk;
    bool          weightOk;
} active_product_t;

// Posted to Core 1 once destination is known.
typedef struct {
    int       slot;
    int       destArea;           // 1 = A1, 2 = A2, 3 = A3
    int64_t   arriveAtUs;         // absolute esp_timer_get_time() of arrival
    int       barcode;
    float     length;
    float     weight;
} arrival_event_t;

// Compact record copied into the rolling buffer printed in sortRT.
typedef struct {
    int    barcode;
    float  length;
    float  weight;
    int    destArea;
} recent_product_t;

// Timestamp pair captured by the obstruction-sensor ISR.
typedef struct {
    int64_t riseEdgeUs;
    int64_t fallEdgeUs;
} product_timestamp_t;

// =============================================================================
// Globals
// =============================================================================
static QueueHandle_t      g_edgeQueue     = NULL;
static QueueHandle_t      g_arrivalQueue  = NULL;
static SemaphoreHandle_t  g_stateMutex    = NULL;
static SemaphoreHandle_t  g_serialMutex   = NULL;

// ISR scratchpad — completed pair sent to the queue on the falling edge.
static volatile product_timestamp_t g_currentTimestamp = {0, 0};

// Per-product pool (mutex-protected)
static active_product_t g_active[MAX_ACTIVE_PRODUCTS];

// Area-count bookkeeping — our own view, updated on commit.
// Note: simAreaCount[] (from the simulator) is the ground truth used for
// grading; we maintain a parallel counter so we can apply capacity guards
// without polling the simulator's volatile array.
static int  g_areaCount[3]     = {0, 0, 0};   // [A1,A2,A3]
static int  g_committed[3]     = {0, 0, 0};   // classified & in transit
static int  g_unknownOnConv1   = 0;           // measured-but-unclassified

// Per-area / overall accumulators used by the "stat" command.
// Spec rule "A1 included in overall, excluded from per-area" is enforced:
//   • Lavg  numerator/denominator: ALL products (A1 + A2 + A3).
//   • Wavg  numerator/denominator: ALL products (G2-rejected items
//                                  contribute weight = 0 to the sum and 1
//                                  to the count — see commitArrival()).
//   • L2/L3/W2/W3:                 only their respective area's products.
static double   g_sumL  = 0.0, g_sumL2 = 0.0, g_sumL3 = 0.0;
static double   g_sumW  = 0.0, g_sumW2 = 0.0, g_sumW3 = 0.0;
static uint32_t g_cntL  = 0,   g_cntL2 = 0,   g_cntL3 = 0;
static uint32_t g_cntW  = 0,   g_cntW2 = 0,   g_cntW3 = 0;

// Recent arrivals queue, drained each second by rtStatusReportTask.
static recent_product_t g_recent[RECENT_LIST_CAP];
static int              g_recentCount = 0;

// Emergency latch — atomically polled by every task in its hot loop.
static volatile bool g_emergency = false;
static volatile uint32_t g_edgeQueueDrops = 0;

// =============================================================================
// Forward declarations
// =============================================================================
static int  acquireSlot(void);
static void releaseSlot(int slot, bool decUnknown);
static void commitArrival(const arrival_event_t *evt);
static void handleSerialLine(const char *line);
static void sleepUntilAbsUs(int64_t targetUs);
static bool gateCapacityAllowsAdmission(void);
// Forward declarations for simulator-side reset support.
extern volatile int currentProductIndex;
extern ProductScheduler g_schedulers[];
// =============================================================================
// OBSTRUCTION-SENSOR ISR  (must keep this name — invoked by simulator engine)
//
// Runs from a hardware-timer ISR context. We do the minimum necessary:
// stamp the edge with the µs-resolution monotonic clock and, on the falling
// edge, hand the complete pair to the dispatcher via a FreeRTOS queue.
// =============================================================================
void IRAM_ATTR obstructionSensorInterrupt() {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    if (g_obstructionSensor) {
        // Rising edge — record front-edge time, defer everything else.
        g_currentTimestamp.riseEdgeUs = esp_timer_get_time();
    } else {
        // Falling edge — pair is complete; ship it.
        g_currentTimestamp.fallEdgeUs = esp_timer_get_time();
        product_timestamp_t snapshot;
        snapshot.riseEdgeUs = g_currentTimestamp.riseEdgeUs;
        snapshot.fallEdgeUs = g_currentTimestamp.fallEdgeUs;

        if (g_edgeQueue != NULL) {
            BaseType_t sent = xQueueSendFromISR(g_edgeQueue,
                                                &snapshot,
                                                &xHigherPriorityTaskWoken);

            if (sent != pdTRUE) {
                g_edgeQueueDrops++;
            }
        }

        if (xHigherPriorityTaskWoken) {
            portYIELD_FROM_ISR();
        }
    }
}

// =============================================================================
// Precise sleep helper — coarse vTaskDelay, fine spin with taskYIELD
//
// Rationale: ESP32 tick is 1 ms by default. For T_gate values down to 2 ms
// we need sub-tick precision around the gate-set moment. We coarse-sleep
// to within ~1 ms of the target then spin-yield the rest.
// =============================================================================
static void sleepUntilAbsUs(int64_t targetUs) {
    while (!g_emergency) {
        int64_t deltaUs = targetUs - esp_timer_get_time();

        if (deltaUs <= 0) {
            return;
        }

        // For long waits, wake up repeatedly so emergency is not delayed.
        if (deltaUs > (EMERGENCY_POLL_MS * 1000)) {
            vTaskDelay(pdMS_TO_TICKS(EMERGENCY_POLL_MS));
        }
        // For medium waits, yield in 1 ms steps.
        else if (deltaUs > 2000) {
            vTaskDelay(pdMS_TO_TICKS(1));
        }
        // For the final short interval, spin-yield for timing precision.
        else {
            taskYIELD();
        }
    }
}

// =============================================================================
// Slot pool helpers
// =============================================================================
static int acquireSlot(void) {
    if (xSemaphoreTake(g_stateMutex, pdMS_TO_TICKS(20)) != pdTRUE) return -1;
    int found = -1;
    for (int i = 0; i < MAX_ACTIVE_PRODUCTS; ++i) {
        if (g_active[i].state == ST_FREE) {
            g_active[i].state     = ST_INSPECTING;
            g_active[i].slot      = i;
            g_active[i].length    = 0.0f;
            g_active[i].weight    = 0.0f;
            g_active[i].barcode   = 0;
            g_active[i].destArea  = 0;
            g_active[i].sizeOk    = false;
            g_active[i].weightOk  = false;
            found = i;
            break;
        }
    }
    xSemaphoreGive(g_stateMutex);
    return found;
}

static void releaseSlot(int slot, bool decUnknown) {
    if (slot < 0 || slot >= MAX_ACTIVE_PRODUCTS) return;
    xSemaphoreTake(g_stateMutex, portMAX_DELAY);
    g_active[slot].state = ST_FREE;
    if (decUnknown && g_unknownOnConv1 > 0) g_unknownOnConv1--;
    xSemaphoreGive(g_stateMutex);
}

// =============================================================================
// PRODUCT PIPELINE TASK  (Core 0 — one spawned per detected product)
//
// Each instance walks one product through its Conv1 stages. The task uses
// absolute timestamps to compute exact wake-up moments, so multiple
// pipeline tasks for overlapping products coexist cleanly.
// =============================================================================
static void productPipelineTask(void *pvParameters) {
    active_product_t *ctx = (active_product_t *)pvParameters;
    const int slot = ctx->slot;
    const int64_t rUs = ctx->riseEdgeUs;
    const float   L   = ctx->length;

    bool divertedAtG2 = false;
    bool divertedAtG3 = false;
    int64_t pivotUs   = 0;

    // Time (µs) for this product's body to clear a gate after the check
    // point, plus a small safety pad. Used to schedule gate-release events
    // so the gate returns to its idle state once the product has passed.
    // We rely on productInterval (>= 400 ms) being far larger than this
    // clearance window (typically 40–80 ms), so the release for product N
    // always lands well before the next product's gate-set.
    const int64_t clearanceUs = (int64_t)((double)L / sp.v1 * 1e6) + 5000;

    // ------------------------------ STAGE A: G2 (size) -----------------------
    const int64_t g2CheckUs   = rUs + (int64_t)((double)sp.d0 / sp.v1 * 1e6);
    const int64_t g2SetUs     = g2CheckUs - (int64_t)((double)sp.T_gate * 1000.0);
    const int64_t g2ReleaseUs = g2CheckUs + clearanceUs;
    sleepUntilAbsUs(g2SetUs);
    if (g_emergency) { releaseSlot(slot, true); vTaskDelete(NULL); return; }

    const bool divertSize = !ctx->sizeOk;
    g_gate2Ctrl = divertSize;
    digitalWrite(gate2DebugPin, divertSize ? HIGH : LOW);

    if (divertSize) {
        divertedAtG2     = true;
        ctx->destArea    = 1;
        pivotUs          = g2CheckUs;
        xSemaphoreTake(g_stateMutex, portMAX_DELAY);
        if (g_unknownOnConv1 > 0) g_unknownOnConv1--;
        g_committed[0]++;
        xSemaphoreGive(g_stateMutex);

        // Release G2 once the product body has cleared the gate.
        // The pipeline will jump straight to STAGE F after this, so the
        // inline sleep adds only the clearance window to the task lifetime.
        sleepUntilAbsUs(g2ReleaseUs);
        if (!g_emergency) {
            g_gate2Ctrl = false;
            digitalWrite(gate2DebugPin, LOW);
        }
    }
    // If we did NOT divert, the gate was already false (and we set it to
    // false again above) — no release needed because it is already idle.

    if (!divertedAtG2) {
        // ----------------------- STAGE B: weight read -----------------------
        const int64_t wCenterUs = rUs + (int64_t)((double)(sp.d1 + L/2.0f) / sp.v1 * 1e6);
        const int64_t halfWinUs = (int64_t)((double)(L * 0.1f) / sp.v1 * 1e6);
        const int64_t wOpenUs   = wCenterUs - halfWinUs;
        const int64_t wCloseUs  = wCenterUs + halfWinUs;

        sleepUntilAbsUs(wOpenUs);
        if (g_emergency) { releaseSlot(slot, true); vTaskDelete(NULL); return; }

        // Sample throughout the window — take the first non-zero reading
        // (the simulator only exposes valid weight inside the window).
        float w = 0.0f;
        while (esp_timer_get_time() < wCloseUs) {
            float w_now = g_weightSensor;
            if (w_now > 0.0f) { w = w_now; break; }
            taskYIELD();
        }
        // Safety net — one more grab at the centre if we missed the window.
        if (w == 0.0f) {
            sleepUntilAbsUs(wCenterUs);
            w = g_weightSensor;
        }
        ctx->weight   = w;
        ctx->weightOk = (w >= sp.W_min - W_EPS && w <= sp.W_max + W_EPS);

        // ----------------------- STAGE C: G3 (weight) -----------------------
        const int64_t g3CheckUs   = rUs + (int64_t)((double)sp.d2 / sp.v1 * 1e6);
        const int64_t g3SetUs     = g3CheckUs - (int64_t)((double)sp.T_gate * 1000.0);
        const int64_t g3ReleaseUs = g3CheckUs + clearanceUs;
        sleepUntilAbsUs(g3SetUs);
        if (g_emergency) { releaseSlot(slot, true); vTaskDelete(NULL); return; }

        const bool divertWeight = !ctx->weightOk;
        g_gate3Ctrl = divertWeight;
        digitalWrite(gate3DebugPin, divertWeight ? HIGH : LOW);

        if (divertWeight) {
            divertedAtG3   = true;
            ctx->destArea  = 1;
            pivotUs        = g3CheckUs;
            xSemaphoreTake(g_stateMutex, portMAX_DELAY);
            if (g_unknownOnConv1 > 0) g_unknownOnConv1--;
            g_committed[0]++;
            xSemaphoreGive(g_stateMutex);

            // Release G3 once the product has cleared.
            sleepUntilAbsUs(g3ReleaseUs);
            if (!g_emergency) {
                g_gate3Ctrl = false;
                digitalWrite(gate3DebugPin, LOW);
            }
        }
    }

    if (!divertedAtG2 && !divertedAtG3) {
        // ----------------------- STAGE D: barcode read ----------------------
        const int64_t bcCenterUs = rUs + (int64_t)((double)(sp.d3 + L/2.0f) / sp.v1 * 1e6);
        const int64_t halfWinUs  = (int64_t)((double)(L * 0.1f) / sp.v1 * 1e6);
        const int64_t bcOpenUs   = bcCenterUs - halfWinUs;
        const int64_t bcCloseUs  = bcCenterUs + halfWinUs;

        sleepUntilAbsUs(bcOpenUs);
        if (g_emergency) { releaseSlot(slot, true); vTaskDelete(NULL); return; }

        int bc = 0;
        while (esp_timer_get_time() < bcCloseUs) {
            int bc_now = g_barcodeReader;
            if (bc_now != 0) { bc = bc_now; break; }
            taskYIELD();
        }
        if (bc == 0) {                                   // last-chance re-read
            sleepUntilAbsUs(bcCenterUs);
            bc = g_barcodeReader;
        }
        ctx->barcode = bc;

        // ----------------------- STAGE E: G4 (routing) ----------------------
        const bool smallLight = (ctx->length <= sp.L_threshold + L_EPS) &&
                                (ctx->weight <= sp.W_threshold + W_EPS);

        const int64_t g4CheckUs   = rUs + (int64_t)((double)sp.d4 / sp.v1 * 1e6);
        const int64_t g4SetUs     = g4CheckUs - (int64_t)((double)sp.T_gate * 1000.0);
        const int64_t g4ReleaseUs = g4CheckUs + clearanceUs;
        sleepUntilAbsUs(g4SetUs);
        if (g_emergency) { releaseSlot(slot, true); vTaskDelete(NULL); return; }

        const bool routeConv3 = !smallLight;
        g_gate4Ctrl = routeConv3;
        digitalWrite(gate4DebugPin, routeConv3 ? HIGH : LOW);

        ctx->destArea = smallLight ? 2 : 3;
        pivotUs       = g4CheckUs;

        xSemaphoreTake(g_stateMutex, portMAX_DELAY);
        if (g_unknownOnConv1 > 0) g_unknownOnConv1--;
        g_committed[ctx->destArea - 1]++;
        xSemaphoreGive(g_stateMutex);

        // Release G4 once the product has cleared the diverter blade.
        // Unlike G2/G3 we ALWAYS release because the gate may have been
        // set true (route to Conv3) — leaving it set would mis-route the
        // following product if its destination differs.
        sleepUntilAbsUs(g4ReleaseUs);
        if (!g_emergency) {
            g_gate4Ctrl = false;
            digitalWrite(gate4DebugPin, LOW);
        }
    }

    // ------------------------- STAGE F: hand off to Core 1 -------------------
    arrival_event_t evt;
    evt.slot      = slot;
    evt.destArea  = ctx->destArea;
    evt.barcode   = ctx->barcode;
    evt.length    = ctx->length;
    evt.weight    = ctx->weight;
    if (divertedAtG2 || divertedAtG3) {
        evt.arriveAtUs = pivotUs + 8000;                 // ~8 ms after divert
    } else if (ctx->destArea == 2) {
        evt.arriveAtUs = pivotUs + (int64_t)((double)sp.d5 / sp.v2 * 1e6);
    } else {
        evt.arriveAtUs = pivotUs + (int64_t)((double)sp.d6 / sp.v3 * 1e6);
    }

    ctx->state = ST_TRANSIT;
    if (xQueueSend(g_arrivalQueue, &evt, pdMS_TO_TICKS(50)) != pdTRUE) {
        // Queue full — extremely unlikely with depth 16. Fail gracefully:
        // commit immediately under the mutex so accounts stay balanced.
        commitArrival(&evt);
    }

    vTaskDelete(NULL);
}

// =============================================================================
// INSPECTION DISPATCHER  (Core 0)
//
// Receives ISR timestamp pairs, performs length calculation and size
// pre-classification, then spawns a pipeline worker. Kept very short so
// it can drain the edge queue at well above the arrival rate.
// =============================================================================
static void inspectionDispatcherTask(void *pvParameters) {
    product_timestamp_t ts;
    while (true) {
        if (xQueueReceive(g_edgeQueue, &ts, portMAX_DELAY) != pdTRUE) continue;
        if (g_emergency) continue;

        const int64_t durUs = ts.fallEdgeUs - ts.riseEdgeUs;
        if (durUs <= 0) {
            DBG_PRINTLN("[WARN] Spurious edge pair — skipping");
            continue;
        }
        const float length = (float)durUs * sp.v1 / 1e6f;

        // Plausibility check — guards against any clock glitch.
        if (length <= 0.0f || length > 100.0f) {
            DBG_PRINTF("[WARN] Implausible length %.2f cm — skipping\n", length);
            continue;
        }

        int slot = acquireSlot();
        if (slot < 0) {
            DBG_PRINTLN("[WARN] Slot pool exhausted — G1 should have closed");
            continue;
        }

        active_product_t *ctx = &g_active[slot];
        ctx->riseEdgeUs = ts.riseEdgeUs;
        ctx->fallEdgeUs = ts.fallEdgeUs;
        ctx->length     = length;
        ctx->sizeOk     = (length >= sp.L_min - L_EPS && length <= sp.L_max + L_EPS);

        xSemaphoreTake(g_stateMutex, portMAX_DELAY);
        g_unknownOnConv1++;
        xSemaphoreGive(g_stateMutex);

        DBG_PRINTF("[INSP] slot=%d  L=%.2f cm  sizeOk=%d\n",
                   slot, length, (int)ctx->sizeOk);

        char nm[16];
        snprintf(nm, sizeof(nm), "pipe%d", slot);
        BaseType_t ok = xTaskCreatePinnedToCore(
            productPipelineTask, nm, 3072, (void*)ctx,
            tskIDLE_PRIORITY + 5, NULL, 0 /* Core 0 */);
        if (ok != pdPASS) {
            DBG_PRINTLN("[ERR] Failed to spawn pipeline task");
            releaseSlot(slot, true);
        }
    }
}

// =============================================================================
// commitArrival — finalises a product's accounting
//
// Called from conv23TrackerTask (Core 1) at the moment the product reaches
// its destination, and as a fallback from the pipeline if the handoff
// queue is somehow full.
// =============================================================================
static void commitArrival(const arrival_event_t *evt) {
    xSemaphoreTake(g_stateMutex, portMAX_DELAY);

    const int idx = evt->destArea - 1;                   // 0,1,2
    if (idx >= 0 && idx < 3) {
        g_areaCount[idx]++;
        if (g_committed[idx] > 0) g_committed[idx]--;
    }

    // Length: every product contributes (overall + per-area when A2/A3).
    g_sumL += evt->length; g_cntL++;
    if (evt->destArea == 2) { g_sumL2 += evt->length; g_cntL2++; }
    if (evt->destArea == 3) { g_sumL3 += evt->length; g_cntL3++; }

    // Weight: every product also contributes to the overall Wavg (per spec
    // "A1 included in overall"). G2-rejected products were diverted before
    // the weigh sensor, so their stored weight is 0.0 and contributes 0.0 to
    // both g_sumW and the denominator g_cntW — this drags Wavg downward,
    // which is the documented behaviour: any item that bypasses the weigh
    // stage is treated as having no measurable mass for averaging purposes.
    // Per-area W2avg/W3avg (excluding A1 by spec) only count products that
    // actually reached A2/A3 — they all have weight > 0 by construction.
    g_sumW += evt->weight; g_cntW++;
    if (evt->weight > 0.0f) {
        if (evt->destArea == 2) { g_sumW2 += evt->weight; g_cntW2++; }
        if (evt->destArea == 3) { g_sumW3 += evt->weight; g_cntW3++; }
    }

    if (g_recentCount < RECENT_LIST_CAP) {
        g_recent[g_recentCount].barcode  = evt->barcode;
        g_recent[g_recentCount].length   = evt->length;
        g_recent[g_recentCount].weight   = evt->weight;
        g_recent[g_recentCount].destArea = evt->destArea;
        g_recentCount++;
    }

    if (evt->slot >= 0 && evt->slot < MAX_ACTIVE_PRODUCTS) {
        g_active[evt->slot].state = ST_FREE;
    }
    xSemaphoreGive(g_stateMutex);
}

// =============================================================================
// CONV23 TRACKER TASK  (Core 1)
//
// Maintains a tiny sorted list of pending arrivals. Sleeps with timeout =
// "time until next arrival" so it wakes exactly when needed. Handles both
// genuine Conv2/3 arrivals and the short-hop "arrivals" in Area 1 from
// G2/G3 diversion — keeps area-count bookkeeping in a single place.
// =============================================================================
static void conv23TrackerTask(void *pvParameters) {
    arrival_event_t pending[MAX_ACTIVE_PRODUCTS * 2];
    int pendingCount = 0;

    while (true) {
        TickType_t timeout = portMAX_DELAY;
        if (pendingCount > 0) {
            int64_t delta = pending[0].arriveAtUs - esp_timer_get_time();
            if (delta <= 0)            timeout = 0;
            else if (delta < 1000)     timeout = 1;
            else                       timeout = pdMS_TO_TICKS((uint32_t)(delta / 1000));
        }

        arrival_event_t in;
        if (xQueueReceive(g_arrivalQueue, &in, timeout) == pdTRUE) {
            // Insertion-sort: keep pending[] in ascending order of arriveAtUs.
            int i = pendingCount;
            while (i > 0 && pending[i - 1].arriveAtUs > in.arriveAtUs) {
                pending[i] = pending[i - 1];
                i--;
            }
            pending[i] = in;
            if (pendingCount < (int)(sizeof(pending) / sizeof(pending[0]))) {
                pendingCount++;
            }
        }

        // Drain everything that has come due (within a 0.5 ms tolerance).
        const int64_t now = esp_timer_get_time();
        while (pendingCount > 0 && pending[0].arriveAtUs <= now + 500) {
            if (!g_emergency) {
                commitArrival(&pending[0]);
            } else {
                // While halted, still release slot so we don't leak.
                xSemaphoreTake(g_stateMutex, portMAX_DELAY);
                g_active[pending[0].slot].state = ST_FREE;
                xSemaphoreGive(g_stateMutex);
            }
            for (int k = 1; k < pendingCount; ++k) pending[k - 1] = pending[k];
            pendingCount--;
        }
    }
}

// =============================================================================
// G1 CAPACITY GUARD  (Core 1)
//
// Conservative admission rule:
//   For every area i, the worst case after admitting one more product is
//   areaCount[i] + committed[i] + unknownOnConv1 + 1   ≤   capacity[i]
//
// The "+ unknownOnConv1" treats every measured-but-unclassified product as
// if it could end up in *any* area; this is safe (never overflows) and is
// quickly relaxed once classification happens at G2/G3/G4.
// =============================================================================
static bool gateCapacityAllowsAdmission(void) {
    bool ok = true;
    xSemaphoreTake(g_stateMutex, portMAX_DELAY);
    const int caps[3] = { sp.C1, sp.C2, sp.C3 };
    for (int i = 0; i < 3 && ok; ++i) {
        const int wc = g_areaCount[i] + g_committed[i] + g_unknownOnConv1 + 1;
        if (wc > caps[i]) ok = false;
    }
    xSemaphoreGive(g_stateMutex);
    return ok;
}

static void g1ControlTask(void *pvParameters) {
    TickType_t lastWake = xTaskGetTickCount();
    while (true) {
        if (g_emergency) {
            g_gate1Ctrl = false;
        } else {
            g_gate1Ctrl = gateCapacityAllowsAdmission();
        }
        vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(G1_REFRESH_MS));
    }
}

// =============================================================================
// STATUS REPORTER  (Core 1) — 1 Hz "sortRT:" telemetry
//
// Output format (exactly):
//   sortRT: AAA,BBB,CCC,TNxxx,L,W,A?,TNxxx,L,W,A?,...
// Area counts: 3-digit zero-padded.  Length / weight: one decimal place.
// Failed barcode → printed as "0" instead of "TN###".
// =============================================================================
static void rtStatusReportTask(void *pvParameters) {
    TickType_t lastWake = xTaskGetTickCount();
    static char buf[1100];

    while (true) {
        vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(STATUS_PERIOD_MS));

        // Snapshot under the mutex; drain recent[] for the next interval.
        recent_product_t local[RECENT_LIST_CAP];
        int  localN = 0;
        int  a1, a2, a3;
        xSemaphoreTake(g_stateMutex, portMAX_DELAY);
        a1 = g_areaCount[0];
        a2 = g_areaCount[1];
        a3 = g_areaCount[2];
        localN = g_recentCount;
        for (int i = 0; i < localN; ++i) local[i] = g_recent[i];
        g_recentCount = 0;
        xSemaphoreGive(g_stateMutex);

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

        for (int i = 0; i < localN && pos < (int)sizeof(buf) - 48; ++i) {
            const char *dest = (local[i].destArea == 2) ? "A2"
                             : (local[i].destArea == 3) ? "A3" : "A1";

            if (local[i].barcode != 0) {
                pos += snprintf(buf + pos, sizeof(buf) - pos,
                                ",TN%03d,%.1f,%.1f,%s",
                                local[i].barcode, local[i].length,
                                local[i].weight, dest);
            } else {
                pos += snprintf(buf + pos, sizeof(buf) - pos,
                                ",0,%.1f,%.1f,%s",
                                local[i].length, local[i].weight, dest);
            }
        }

        xSemaphoreTake(g_serialMutex, portMAX_DELAY);
        Serial.println(buf);
        xSemaphoreGive(g_serialMutex);
    }
}

// =============================================================================
// SERIAL COMMAND HANDLER  (Core 1)
//
// Accepts "stat", "reset", "emergency" (case-sensitive per spec).
// Tolerates CR, LF or CRLF terminators and trims surrounding whitespace.
// =============================================================================
// =============================================================================
// resetAllState — SCOPED reset (statistics + visible counters only)
//
// What this clears:
//   • g_areaCount[] — sortRT's AAA/BBB/CCC field resets to 000,000,000
//   • all stat accumulators (sumL/L2/L3, sumW/W2/W3, cntL/L2/L3, cntW/W2/W3)
//   • the rolling recent-arrivals buffer drained each second by sortRT
//
// What this DELIBERATELY does NOT clear:
//   • g_active[]            — slots owned by mid-pipeline tasks; clobbering
//                             them races on every field those tasks read/write
//   • g_committed[]         — how many already-classified products are still
//                             on Conv2/Conv3 heading to their destination;
//                             wiping this breaks the G1 capacity-guard
//                             invariant (admission could overflow C2 or C3)
//   • g_unknownOnConv1      — same argument, for products between inspection
//                             and their classification gate
//
// Practical consequence: if you issue 'reset' while products are still on
// the conveyor, the AAA/BBB/CCC counters go to zero immediately but those
// in-flight items will arrive a few hundred ms later and bump them back up.
// This is the honest behaviour of a soft reset — for a true clean slate,
// invoke 'reset' only when the conveyor is empty (typically end-of-batch).
//
// The g_emergency latch is cleared so that after an emergency-halt the
// operator can 'reset' to resume admission. Gates are not forced here —
// g1ControlTask reopens G1 on its next 25 ms tick once capacity allows,
// and divert gates are owned by per-product pipelines (or are already
// closed if no pipeline is active).
// =============================================================================
static void resetAllState(void) {
   
    g_emergency = true;

    // Clear pending controller queues so old events do not continue after reset.
    if (g_edgeQueue != NULL) {
        xQueueReset(g_edgeQueue);
    }

    if (g_arrivalQueue != NULL) {
        xQueueReset(g_arrivalQueue);
    }

    xSemaphoreTake(g_stateMutex, portMAX_DELAY);

    // Clear controller-side occupancy and reservations.
    for (int i = 0; i < 3; ++i) {
        g_areaCount[i] = 0;
        g_committed[i] = 0;
    }

    g_unknownOnConv1 = 0;

    // Clear product slots.
    for (int i = 0; i < MAX_ACTIVE_PRODUCTS; ++i) {
        g_active[i].state = ST_FREE;
        g_active[i].slot = i;
        g_active[i].length = 0.0f;
        g_active[i].weight = 0.0f;
        g_active[i].barcode = 0;
        g_active[i].destArea = 0;
        g_active[i].sizeOk = false;
        g_active[i].weightOk = false;
    }

    // Clear statistics.
    g_sumL = g_sumL2 = g_sumL3 = 0.0;
    g_sumW = g_sumW2 = g_sumW3 = 0.0;
    g_cntL = g_cntL2 = g_cntL3 = 0;
    g_cntW = g_cntW2 = g_cntW3 = 0;

    // Clear recent output buffer.
    g_recentCount = 0;

    xSemaphoreGive(g_stateMutex);
    // Reset simulator-side counters used for screenshots and validation.
    // This is only for the Wokwi scaffold environment.
    currentProductIndex = 0;

    for (int i = 0; i < 3; ++i) {
        simAreaCount[i] = 0;
        errorGateCNT[i] = 0;
    }

    for (int i = 0; i < 4; ++i) {
        if (g_schedulers[i].timerHandle != nullptr) {
            timerEnd(g_schedulers[i].timerHandle);
            g_schedulers[i].timerHandle = nullptr;
        }

        g_schedulers[i].currentEventIndex = 0;
        g_schedulers[i].totalEvents = 0;
    }
    // Put gates back to default states.
    g_gate1Ctrl = true;
    g_gate2Ctrl = false;
    g_gate3Ctrl = false;
    g_gate4Ctrl = false;

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

    // Allow operation again.
    g_emergency = false;
}

static void handleSerialLine(const char *raw) {
    // Trim leading/trailing whitespace into a local buffer.
    char cmd[32];
    int  n = 0;

    while (*raw && (*raw == ' ' || *raw == '\t')) {
        raw++;
    }

    while (*raw && *raw != '\r' && *raw != '\n' && n < (int)sizeof(cmd) - 1) {
        cmd[n++] = *raw++;
    }

    while (n > 0 && (cmd[n - 1] == ' ' || cmd[n - 1] == '\t')) {
        n--;
    }

    cmd[n] = '\0';

    if (n == 0) {
        return;
    }

    if (strcmp(cmd, "stat") == 0) {
        double Lavg, L2avg, L3avg, Wavg, W2avg, W3avg;

        xSemaphoreTake(g_stateMutex, portMAX_DELAY);

        Lavg  = g_cntL  ? g_sumL  / g_cntL  : 0.0;
        L2avg = g_cntL2 ? g_sumL2 / g_cntL2 : 0.0;
        L3avg = g_cntL3 ? g_sumL3 / g_cntL3 : 0.0;
        Wavg  = g_cntW  ? g_sumW  / g_cntW  : 0.0;
        W2avg = g_cntW2 ? g_sumW2 / g_cntW2 : 0.0;
        W3avg = g_cntW3 ? g_sumW3 / g_cntW3 : 0.0;

        xSemaphoreGive(g_stateMutex);

        xSemaphoreTake(g_serialMutex, portMAX_DELAY);
        Serial.printf("sortSTAT: %.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
                      Lavg, L2avg, L3avg, Wavg, W2avg, W3avg);
        xSemaphoreGive(g_serialMutex);
    }
    else if (strcmp(cmd, "reset") == 0) {
        resetAllState();
        // Spec: no response output required.
    }
    else if (strcmp(cmd, "emergency") == 0) {
        // Latch emergency first so timing tasks stop at their next checkpoint.
        g_emergency  = true;

        // Force every gate to a safe off/default state.
        g_gate1Ctrl  = false;
        g_gate2Ctrl  = false;
        g_gate3Ctrl  = false;
        g_gate4Ctrl  = false;

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

        xSemaphoreTake(g_serialMutex, portMAX_DELAY);
        Serial.println("EMERGENCY: System halted");
        xSemaphoreGive(g_serialMutex);
    }
    else {
        DBG_PRINTF("[CMD] unknown command: \"%s\"\n", cmd);
    }
}

static void serialCommandTask(void *pvParameters) {
    static char line[64];
    int len = 0;
    while (true) {
        while (Serial.available() > 0) {
            char c = (char)Serial.read();
            if (c == '\n' || c == '\r') {
                if (len > 0) {
                    line[len] = '\0';
                    handleSerialLine(line);
                    len = 0;
                }
            } else if (len < (int)sizeof(line) - 1) {
                line[len++] = c;
            } else {
                len = 0;                                 // overflow → reset
            }
        }
        vTaskDelay(pdMS_TO_TICKS(SERIAL_POLL_MS));
    }
}

// =============================================================================
// setup() — bring up sync primitives, tasks, then hand to the simulator
// =============================================================================
void setup() {
    Serial.begin(BAUD_RATE);
    delay(200);                                          

    #if DEBUG_MODE
    Serial.println();
    Serial.println("=== SmartSort Controller — Student 2163428 ===");
    #endif

    // Init pool
    for (int i = 0; i < MAX_ACTIVE_PRODUCTS; ++i) {
        g_active[i].state = ST_FREE;
        g_active[i].slot  = i;
    }

    // FreeRTOS primitives
    g_edgeQueue    = xQueueCreate(16, sizeof(product_timestamp_t));
    g_arrivalQueue = xQueueCreate(16, sizeof(arrival_event_t));
    g_stateMutex   = xSemaphoreCreateMutex();
    g_serialMutex  = xSemaphoreCreateMutex();

    if (!g_edgeQueue || !g_arrivalQueue || !g_stateMutex || !g_serialMutex) {
        Serial.println("[FATAL] FreeRTOS primitive allocation failed");
        while (true) { delay(1000); }
    }

    // Debug-pin pinModes (extra to the obstruction pin set by the simulator).
    pinMode(gate2DebugPin, OUTPUT);
    pinMode(gate3DebugPin, OUTPUT);
    pinMode(gate4DebugPin, OUTPUT);
    digitalWrite(gate2DebugPin, LOW);
    digitalWrite(gate3DebugPin, LOW);
    digitalWrite(gate4DebugPin, LOW);

    // ----- Core 0: Conv1 inspection dispatcher -----
    xTaskCreatePinnedToCore(inspectionDispatcherTask, "inspect",
                            4096, NULL, tskIDLE_PRIORITY + 6, NULL, 0);

    // ----- Core 1: Conv2/3 tracking, G1, status, commands -----
    xTaskCreatePinnedToCore(conv23TrackerTask,  "conv23",
                            3072, NULL, tskIDLE_PRIORITY + 5, NULL, 1);
    xTaskCreatePinnedToCore(g1ControlTask,      "g1ctrl",
                            2048, NULL, tskIDLE_PRIORITY + 4, NULL, 1);
    xTaskCreatePinnedToCore(rtStatusReportTask,   "status",
                            4096, NULL, tskIDLE_PRIORITY + 3, NULL, 1);
    xTaskCreatePinnedToCore(serialCommandTask,  "cmd",
                            3072, NULL, tskIDLE_PRIORITY + 2, NULL, 1);

    // Hand off to the simulator (must be the last thing in setup()).
    startSimulator();
}

void loop() {
    // Nothing to do here — every responsibility lives in a FreeRTOS task.
    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");
}