#include "Arduino.h"

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

// ============================================================================
// 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 default test set is ordered to finish automatically for student 2308420.
  // Normal products are loaded first; Area 1 rejects are placed at the end so the
  // small C1 capacity is not filled before the remaining valid products enter.

  // Area 2: normal products that are small AND light.
  productArray[idx++] = {sp.L_min + 0.10f, sp.W_min + 1.0f, 1001};
  productArray[idx++] = {sp.L_threshold, sp.W_threshold, 1002};      // exactly on A2/A3 threshold
  productArray[idx++] = {sp.L_threshold - 0.10f, sp.W_threshold - 1.0f, 1003};
  productArray[idx++] = {sp.L_min, sp.W_min, 1004};                  // exactly on lower limits

  // Area 3: normal products that are large OR heavy.
  productArray[idx++] = {sp.L_threshold + 0.10f, sp.W_threshold - 1.0f, 1005};
  productArray[idx++] = {sp.L_min + 0.20f, sp.W_threshold + 1.0f, 1006};
  productArray[idx++] = {sp.L_max - 0.10f, sp.W_max - 1.0f, 1007};
  productArray[idx++] = {sp.L_threshold + 0.20f, sp.W_threshold + 2.0f, 1008};
  productArray[idx++] = {sp.L_max, sp.W_max, 1009};                  // exactly on upper limits

  // Area 1: reject products. Three are used because C1 can be as low as 3.
  productArray[idx++] = {sp.L_min - 0.50f, Wmid, 1010};              // too short -> G2
  productArray[idx++] = {sp.L_max + 0.50f, Wmid, 1011};              // too long  -> G2
  productArray[idx++] = {Lmid, sp.W_min - 5.0f, 1012};               // weight fault -> G3

  // If this student's generated C1 is larger than 3, add one more G3 reject to
  // test the high-weight fault path without risking a default capacity stall.
  if (sp.C1 > 3 && idx < MAX_TEST_PRODUCTS) {
    productArray[idx++] = {Lmid, sp.W_max + 5.0f, 1013};             // heavy fault -> G3
  }

  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) version: V3.3.7
//
// Your controller must:
// 1. Detect products via the obstruction sensor interrupt
// 2. Calculate product length from sensor blocking duration
// 3. Control G2 to divert size-abnormal products to Area 1
// 4. Read weight sensor when product is centered at d1
// 5. Control G3 to divert weight-abnormal products to Area 1
// 6. Read barcode when product is centered at d3
// 7. Control G4 to route products to Conv2 (Area 2) or Conv3 (Area 3)
// 8. Control G1 to prevent area overflow (capacity management)
// 9. Output real-time status and respond to serial commands
//
// Classification rules:
//   - Size abnormal (L < L_min or L > L_max) -> Area 1 via G2
//   - Weight abnormal (W < W_min or W > W_max) -> Area 1 via G3
//   - L <= L_threshold AND W <= W_threshold -> Area 2 via G4 (Conv2)
//   - Otherwise -> Area 3 via G4 (Conv3)
//
// Access parameters via: sp.v1, sp.d0, sp.L_min, sp.T_gate, sp.C1, etc.
//
// Timing hint: after obstruction sensor fall edge, the time for product center
// to reach the weight sensor is approximately:
//   T = (sp.d1 - product_length/2) / sp.v1 * 1000  (in ms, from sensor position)

// ============================================================================
// CONFIGURATION CONSTANTS
// ============================================================================

#define LENGTH_EPSILON_CM       0.005f
#define WEIGHT_EPSILON_G        0.005f

#define STATUS_BUFFER_SIZE      6
#define GATE_COMMAND_QUEUE_LEN  32
#define ARRIVAL_QUEUE_LEN       24
#define COMMAND_POLL_MS         10
#define STATUS_PERIOD_MS        1000
#define EMERGENCY_TARGET_MS     50

// ============================================================================
// ENUMERATIONS
// ============================================================================

typedef enum {
  PRODUCT_STATE_NEW = 0,
  PRODUCT_STATE_LENGTH_READY,
  PRODUCT_STATE_WAIT_WEIGHT,
  PRODUCT_STATE_WEIGHT_READY,
  PRODUCT_STATE_WAIT_BARCODE,
  PRODUCT_STATE_BARCODE_READY,
  PRODUCT_STATE_DONE,
  PRODUCT_STATE_CANCELLED
} ProductState;

typedef enum {
  AREA_NONE = 0,
  AREA_1    = 1,
  AREA_2    = 2,
  AREA_3    = 3
} AreaId;

typedef enum {
  SORT_GATE_2 = 2,
  SORT_GATE_3 = 3,
  SORT_GATE_4 = 4
} SortGateId;


// ============================================================================
// DATA STRUCTURES
// ============================================================================

// Timestamp pair from the obstruction sensor.
typedef struct {
  int64_t riseTimeUs;       // Product front edge reaches obstruction sensor
  int64_t fallTimeUs;       // Product back edge leaves obstruction sensor
} SensorBlockEvent;

// Product state-machine context.
typedef struct {
  uint32_t sequenceNumber;
  uint32_t runId;

  SensorBlockEvent sensorEvent;

  ProductState state;
  AreaId destination;

  float lengthCm;
  float weightG;
  int barcode;

  int64_t g2ArrivalUs;
  int64_t g3ArrivalUs;
  int64_t g4ArrivalUs;

  bool unknownReserved;       // True while the product is still counted as worst-case capacity.
} ProductFSM;

// Recent completed product for sortRT output.
typedef struct {
  uint32_t sequenceNumber;
  int barcode;
  float lengthCm;
  float weightG;
  AreaId destination;
} StatusProduct;

// Runtime counters, occupancy, and statistics.
typedef struct {
  int areaCount[3];         // Current area occupancy: A1, A2, A3
  int inTransit[3];         // Products with a confirmed destination but not arrived yet.
  int unknownInSystem;      // Products admitted but not classified yet; counted against all areas.

  float totalLength;
  float totalWeight;
  int totalProducts;

  float area2Length;
  float area2Weight;
  int area2Products;

  float area3Length;
  float area3Weight;
  int area3Products;
} SystemRuntime;
// Small diagnostic block used for testing and report evidence.
// It is not printed during normal operation, so it does not disturb sortRT.
typedef struct {
  uint32_t weightAttempts;
  uint32_t weightSuccess;

  uint32_t barcodeAttempts;
  uint32_t barcodeSuccess;

  uint32_t gateQueueDrops;
  uint32_t arrivalQueueDrops;
  uint32_t sensorQueueDrops;
  uint32_t taskCreateFails;

  uint32_t emergencyCount;
  uint32_t lastEmergencyResponseUs;
} DiagnosticData;
// Gate command handled by the dedicated gate-control system.
typedef struct {
  SortGateId gateId;
  bool targetState;
  int64_t activateAtUs;
  int64_t releaseAtUs;
  uint32_t runId;
} GateCommand;

// Arrival command handled by the Core 1 arrival tracker.
typedef struct {
  uint32_t sequenceNumber;
  int barcode;
  float lengthCm;
  float weightG;
  AreaId destination;
  int64_t arrivalAtUs;
  uint32_t runId;
} ArrivalCommand;


// ============================================================================
// GLOBAL RTOS OBJECTS
// ============================================================================

static QueueHandle_t sensorEventQueue = NULL;
static QueueHandle_t gateCommandQueue = NULL;
static QueueHandle_t arrivalCommandQueue = NULL;

static SemaphoreHandle_t runtimeMutex = NULL;
static TaskHandle_t productTaskHandle = NULL;
static TaskHandle_t gateManagerTaskHandle = NULL;
static TaskHandle_t arrivalTaskHandle = NULL;
static TaskHandle_t commandTaskHandle = NULL;
static TaskHandle_t statusTaskHandle = NULL;

// ============================================================================
// GLOBAL STATE
// ============================================================================

// Built by ISR.
static volatile SensorBlockEvent currentSensorEvent = {0, 0};

// Main runtime state.
static SystemRuntime runtimeData = {0};
static DiagnosticData diagData = {0};
// Recent products circular buffer.
static StatusProduct statusBuffer[STATUS_BUFFER_SIZE];
static int statusWriteIndex = 0;
static int statusCount = 0;

// Internal product sequence number.
static volatile uint32_t nextProductSequence = 1;

// Run ID increments on reset to invalidate old tasks/events.
static volatile uint32_t activeRunId = 1;

// System flags.
static volatile bool emergencyActive = false;
static volatile bool intakeBlockedByCapacity = false;
static volatile bool capacityWarningPrinted = false;
static volatile int capacityBlockedArea = 0;


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

static int64_t cmToMicroseconds(float distanceCm, float speedCmPerSec);
static bool isCurrentRun(uint32_t runId);
static void waitUntilTimestamp(int64_t targetUs, uint32_t runId);

static bool isLengthAccepted(float lengthCm);
static bool isWeightAccepted(float weightG);
static bool isArea2Product(float lengthCm, float weightG);

static void refreshIntakeGate();
static void reserveUnknownSlot(ProductFSM *product);
static void commitReservedProductToArea(ProductFSM *product, AreaId area);
static void releaseUnknownSlot(ProductFSM *product);
static void addProductToStatusBuffer(const ArrivalCommand *arrival);
static void finalizeProductArrival(const ArrivalCommand *arrival);

static float readWeightAtCorrectTime(int64_t centreUs, float lengthCm, uint32_t runId);
static int readBarcodeAtCorrectTime(int64_t centreUs, float lengthCm, uint32_t runId);

static volatile bool* getGateOutput(SortGateId gateId);
static void scheduleGateCommand(SortGateId gateId, bool targetState, int64_t gateArrivalUs, float productLengthCm, uint32_t runId);

static void scheduleArrivalCommand(const ProductFSM *product, int64_t arrivalUs);

static void resetControllerState();
static void clearAreaOccupancy();

static void forceSafeStop();
static void printSortStat();
static void printRealTimeStatus();
static void printDiagnosticLine();
static void handleSerialCommand(const String &cmd);

void productStateMachineTask(void *pvParameters);
void gateCommandManagerTask(void *pvParameters);
void gatePulseTask(void *pvParameters);
void arrivalTrackingTask(void *pvParameters);
void commandTask(void *pvParameters);
void statusOutputTask(void *pvParameters);

// Forward access to simulator state is used only for the reset command, so a
// user reset clears both the controller counters and the simulator counters.
extern volatile int currentProductIndex;
extern ProductScheduler g_schedulers[4];


// ============================================================================
// TIME UTILITIES
// ============================================================================

static int64_t cmToMicroseconds(float distanceCm, float speedCmPerSec) {
  if (speedCmPerSec <= 0.0f) {
    return 0;
  }

  return (int64_t)((distanceCm / speedCmPerSec) * 1000000.0f);
}

static bool isCurrentRun(uint32_t runId) {
  return runId == activeRunId;
}

static void waitUntilTimestamp(int64_t targetUs, uint32_t runId) {
  while (!emergencyActive && isCurrentRun(runId)) {
    int64_t nowUs = esp_timer_get_time();
    int64_t remainingUs = targetUs - nowUs;

    if (remainingUs <= 0) {
      return;
    }

    if (remainingUs > 4000) {
      int delayMs = (int)((remainingUs - 2000) / 1000);

      if (delayMs < 1) {
        delayMs = 1;
      }

      vTaskDelay(pdMS_TO_TICKS(delayMs));
    }
    else {
      delayMicroseconds((uint32_t)remainingUs);
      return;
    }
  }
}


// ============================================================================
// CLASSIFICATION UTILITIES
// ============================================================================

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

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

static bool isArea2Product(float lengthCm, float weightG) {
  return (lengthCm <= (sp.L_threshold + LENGTH_EPSILON_CM) && weightG <= (sp.W_threshold + WEIGHT_EPSILON_G));
}


// ============================================================================
// CAPACITY AND STATISTICS
// ============================================================================

static void refreshIntakeGate() {
  if (runtimeMutex == NULL) {
    return;
  }

  // Startup guard: before startSimulator() generates parameters, capacities are zero.
  if (sp.C1 <= 0 || sp.C2 <= 0 || sp.C3 <= 0) {
    g_gate1Ctrl = true;
    intakeBlockedByCapacity = false;
    capacityWarningPrinted = false;
    capacityBlockedArea = 0;
    return;
  }

  xSemaphoreTake(runtimeMutex, portMAX_DELAY);

  int fullArea = 0;

  // Conservative capacity rule:
  // an unclassified product could still become A1, A2, or A3, so it is counted
  // against every destination until its true destination is known.
  int projectedA1 = runtimeData.areaCount[0] + runtimeData.inTransit[0] + runtimeData.unknownInSystem;
  int projectedA2 = runtimeData.areaCount[1] + runtimeData.inTransit[1] + runtimeData.unknownInSystem;
  int projectedA3 = runtimeData.areaCount[2] + runtimeData.inTransit[2] + runtimeData.unknownInSystem;

  if (projectedA1 >= sp.C1) {
    fullArea = 1;
  }
  else if (projectedA2 >= sp.C2) {
    fullArea = 2;
  }
  else if (projectedA3 >= sp.C3) {
    fullArea = 3;
  }

  if (emergencyActive) {
    g_gate1Ctrl = false;
  }
  else if (fullArea != 0) {
    g_gate1Ctrl = false;
    intakeBlockedByCapacity = true;
    capacityBlockedArea = fullArea;
  }
  else {
    g_gate1Ctrl = true;
    intakeBlockedByCapacity = false;
    capacityWarningPrinted = false;
    capacityBlockedArea = 0;
  }

  xSemaphoreGive(runtimeMutex);
}

static void reserveUnknownSlot(ProductFSM *product) {
  if (product == NULL || runtimeMutex == NULL || product->unknownReserved) {
    return;
  }

  xSemaphoreTake(runtimeMutex, portMAX_DELAY);
  runtimeData.unknownInSystem++;
  product->unknownReserved = true;
  xSemaphoreGive(runtimeMutex);

  refreshIntakeGate();
}

static void commitReservedProductToArea(ProductFSM *product, AreaId area) {
  int index = ((int)area) - 1;

  if (product == NULL || index < 0 || index > 2 || runtimeMutex == NULL) {
    return;
  }

  xSemaphoreTake(runtimeMutex, portMAX_DELAY);

  if (product->unknownReserved && runtimeData.unknownInSystem > 0) {
    runtimeData.unknownInSystem--;
  }

  product->unknownReserved = false;
  runtimeData.inTransit[index]++;

  xSemaphoreGive(runtimeMutex);

  refreshIntakeGate();
}

static void releaseUnknownSlot(ProductFSM *product) {
  if (product == NULL || runtimeMutex == NULL || !product->unknownReserved) {
    return;
  }

  xSemaphoreTake(runtimeMutex, portMAX_DELAY);

  if (runtimeData.unknownInSystem > 0) {
    runtimeData.unknownInSystem--;
  }

  product->unknownReserved = false;

  xSemaphoreGive(runtimeMutex);

  refreshIntakeGate();
}

static void addProductToStatusBuffer(const ArrivalCommand *arrival) {
  if (arrival == NULL || runtimeMutex == NULL) {
    return;
  }

  xSemaphoreTake(runtimeMutex, portMAX_DELAY);

  statusBuffer[statusWriteIndex].sequenceNumber = arrival->sequenceNumber;
  statusBuffer[statusWriteIndex].barcode = arrival->barcode;
  statusBuffer[statusWriteIndex].lengthCm = arrival->lengthCm;
  statusBuffer[statusWriteIndex].weightG = arrival->weightG;
  statusBuffer[statusWriteIndex].destination = arrival->destination;

  statusWriteIndex = (statusWriteIndex + 1) % STATUS_BUFFER_SIZE;

  if (statusCount < STATUS_BUFFER_SIZE) {
    statusCount++;
  }

  xSemaphoreGive(runtimeMutex);
}

static void finalizeProductArrival(const ArrivalCommand *arrival) {
  if (arrival == NULL || runtimeMutex == NULL) {
    return;
  }

  int index = ((int)arrival->destination) - 1;

  if (index < 0 || index > 2) {
    return;
  }

  xSemaphoreTake(runtimeMutex, portMAX_DELAY);

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

  runtimeData.areaCount[index]++;

  runtimeData.totalLength += arrival->lengthCm;
  runtimeData.totalWeight += arrival->weightG;
  runtimeData.totalProducts++;

  if (arrival->destination == AREA_2) {
    runtimeData.area2Length += arrival->lengthCm;
    runtimeData.area2Weight += arrival->weightG;
    runtimeData.area2Products++;
  }
  else if (arrival->destination == AREA_3) {
    runtimeData.area3Length += arrival->lengthCm;
    runtimeData.area3Weight += arrival->weightG;
    runtimeData.area3Products++;
  }

  xSemaphoreGive(runtimeMutex);

  addProductToStatusBuffer(arrival);
  refreshIntakeGate();
}


// ============================================================================
// SENSOR READING HELPERS
// ============================================================================

static float readWeightAtCorrectTime(int64_t centreUs, float lengthCm, uint32_t runId) {
  float weight = 0.0f;

  diagData.weightAttempts++;

  int64_t halfWindowUs = cmToMicroseconds(lengthCm * 0.10f, sp.v1);
  int64_t startUs = centreUs - halfWindowUs + 300;
  int64_t endUs   = centreUs + halfWindowUs - 300;

  waitUntilTimestamp(startUs, runId);

  while (!emergencyActive && isCurrentRun(runId) && esp_timer_get_time() <= endUs) {
    if (g_weightSensor > 0.0f) {
      weight = g_weightSensor;
      diagData.weightSuccess++;
      break;
    }

    delayMicroseconds(300);
  }

  return weight;
}

static int readBarcodeAtCorrectTime(int64_t centreUs, float lengthCm, uint32_t runId) {
  int barcode = 0;

  diagData.barcodeAttempts++;

  int64_t halfWindowUs = cmToMicroseconds(lengthCm * 0.10f, sp.v1);
  int64_t startUs = centreUs - halfWindowUs + 300;
  int64_t endUs   = centreUs + halfWindowUs - 300;

  waitUntilTimestamp(startUs, runId);

  while (!emergencyActive && isCurrentRun(runId) && esp_timer_get_time() <= endUs) {
    if (g_barcodeReader != 0) {
      barcode = g_barcodeReader;
      diagData.barcodeSuccess++;
      break;
    }

    delayMicroseconds(300);
  }

  return barcode;
}


// ============================================================================
// DEDICATED GATE CONTROL
// ============================================================================

static volatile bool* getGateOutput(SortGateId gateId) {
  switch (gateId) {
    case SORT_GATE_2:
      return &g_gate2Ctrl;

    case SORT_GATE_3:
      return &g_gate3Ctrl;

    case SORT_GATE_4:
      return &g_gate4Ctrl;

    default:
      return NULL;
  }
}

static void scheduleGateCommand(SortGateId gateId, bool targetState, int64_t gateArrivalUs, float productLengthCm, uint32_t runId) {
  if (gateCommandQueue == NULL || !isCurrentRun(runId)) {
    return;
  }

  GateCommand command;

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

  command.gateId = gateId;
  command.targetState = targetState;
  command.activateAtUs = gateArrivalUs - leadUs;
  command.releaseAtUs = gateArrivalUs + cmToMicroseconds(productLengthCm, sp.v1) + 2000;
  command.runId = runId;

  if (xQueueSend(gateCommandQueue, &command, pdMS_TO_TICKS(10)) != pdTRUE) {
  diagData.gateQueueDrops++;
}
}

void gateCommandManagerTask(void *pvParameters) {
  GateCommand command;

  while (true) {
    if (xQueueReceive(gateCommandQueue, &command, portMAX_DELAY) == pdTRUE) {
      if (!isCurrentRun(command.runId)) {
        continue;
      }

      GateCommand *commandCopy = (GateCommand *)pvPortMalloc(sizeof(GateCommand));

      if (commandCopy == NULL) {
        continue;
      }

      *commandCopy = command;

      BaseType_t created = xTaskCreatePinnedToCore(
                             gatePulseTask,
                             "GatePulse",
                             2048,
                             commandCopy,
                             9,
                             NULL,
                             0
                           );

      if (created != pdPASS) {
        vPortFree(commandCopy);
      }
    }
  }
}

void gatePulseTask(void *pvParameters) {
  GateCommand *commandPtr = (GateCommand *)pvParameters;

  if (commandPtr == NULL) {
    vTaskDelete(NULL);
    return;
  }

  GateCommand command = *commandPtr;
  vPortFree(commandPtr);

  volatile bool *gateOutput = getGateOutput(command.gateId);

  if (gateOutput == NULL || !isCurrentRun(command.runId)) {
    vTaskDelete(NULL);
    return;
  }

  waitUntilTimestamp(command.activateAtUs, command.runId);

  if (!emergencyActive && isCurrentRun(command.runId)) {
    *gateOutput = command.targetState;
  }

  waitUntilTimestamp(command.releaseAtUs, command.runId);

  if (!emergencyActive && isCurrentRun(command.runId)) {
    // Safe/default gate state after the product has passed.
    *gateOutput = false;
  }

  vTaskDelete(NULL);
}


// ============================================================================
// ARRIVAL TRACKING ON CORE 1
// ============================================================================

static void scheduleArrivalCommand(const ProductFSM *product, int64_t arrivalUs) {
  if (product == NULL || arrivalCommandQueue == NULL || !isCurrentRun(product->runId)) {
    return;
  }

  ArrivalCommand arrival;

  arrival.sequenceNumber = product->sequenceNumber;
  arrival.barcode = product->barcode;
  arrival.lengthCm = product->lengthCm;
  arrival.weightG = product->weightG;
  arrival.destination = product->destination;
  arrival.arrivalAtUs = arrivalUs;
  arrival.runId = product->runId;

  xQueueSend(arrivalCommandQueue, &arrival, pdMS_TO_TICKS(10));
}

void arrivalTrackingTask(void *pvParameters) {
  ArrivalCommand arrival;

  while (true) {
    if (xQueueReceive(arrivalCommandQueue, &arrival, portMAX_DELAY) == pdTRUE) {
      if (!isCurrentRun(arrival.runId) || emergencyActive) {
        continue;
      }

      waitUntilTimestamp(arrival.arrivalAtUs, arrival.runId);

      if (isCurrentRun(arrival.runId) && !emergencyActive) {
        finalizeProductArrival(&arrival);
      }
    }
  }
}


// ============================================================================
// RESET AND CLEAR COMMANDS
// ============================================================================

static void resetControllerState() {
  activeRunId++;

  if (activeRunId == 0) {
    activeRunId = 1;
  }

  if (runtimeMutex != NULL) {
    xSemaphoreTake(runtimeMutex, portMAX_DELAY);

    memset(&runtimeData, 0, sizeof(runtimeData));
    memset(statusBuffer, 0, sizeof(statusBuffer));

    statusWriteIndex = 0;
    statusCount = 0;
    nextProductSequence = 1;

    xSemaphoreGive(runtimeMutex);
  }

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

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

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

  emergencyActive = false;
  intakeBlockedByCapacity = false;
  capacityWarningPrinted = false;
  capacityBlockedArea = 0;

  // Keep the simulator-side counters consistent with the controller reset.
  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;
  }

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

static void clearAreaOccupancy() {
  if (runtimeMutex != NULL) {
    xSemaphoreTake(runtimeMutex, portMAX_DELAY);

    runtimeData.areaCount[0] = 0;
    runtimeData.areaCount[1] = 0;
    runtimeData.areaCount[2] = 0;

    xSemaphoreGive(runtimeMutex);
  }

  intakeBlockedByCapacity = false;
  capacityWarningPrinted = false;
  capacityBlockedArea = 0;

  refreshIntakeGate();
}


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

void IRAM_ATTR obstructionSensorInterrupt() {
  BaseType_t shouldYield = pdFALSE;

  if (g_obstructionSensor == true) {
    currentSensorEvent.riseTimeUs = esp_timer_get_time();
  }
  else {
    currentSensorEvent.fallTimeUs = esp_timer_get_time();

    if (sensorEventQueue != NULL) {
      BaseType_t sent = xQueueSendFromISR(sensorEventQueue,
                                          (void*)&currentSensorEvent,
                                          &shouldYield);

      if (sent != pdTRUE) {
        diagData.sensorQueueDrops++;
      }
    }

    if (shouldYield) {
      portYIELD_FROM_ISR();
    }
  }
}


// ============================================================================
// PRODUCT DISPATCH TASK - CORE 0
// ============================================================================

void productProcessingTask(void *pvParameters) {
  SensorBlockEvent sensorEvent;

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

      ProductFSM *product =
        (ProductFSM *)pvPortMalloc(sizeof(ProductFSM));

      if (product == NULL) {
        continue;
      }

      memset(product, 0, sizeof(ProductFSM));

      product->sequenceNumber = nextProductSequence++;
      product->runId = activeRunId;
      product->sensorEvent = sensorEvent;
      product->state = PRODUCT_STATE_NEW;
      product->destination = AREA_NONE;
      product->barcode = 0;
      product->weightG = 0.0f;
      product->unknownReserved = false;

      BaseType_t created = xTaskCreatePinnedToCore(
                             productStateMachineTask,
                             "ProductFSM",
                             4096,
                             product,
                             8,
                             NULL,
                             0
                           );

      if (created != pdPASS) {
        vPortFree(product);
      }
    }
  }
}


// ============================================================================
// PRODUCT STATE MACHINE TASK - CORE 0
// ============================================================================

void productStateMachineTask(void *pvParameters) {
  ProductFSM *product = (ProductFSM *)pvParameters;

  if (product == NULL) {
    vTaskDelete(NULL);
    return;
  }

  while (!emergencyActive && isCurrentRun(product->runId)) {
    switch (product->state) {

      // ------------------------------------------------------------
      // Calculate length and gate arrival timestamps.
      // ------------------------------------------------------------
      case PRODUCT_STATE_NEW: {
          int64_t blockedUs =
            product->sensorEvent.fallTimeUs - product->sensorEvent.riseTimeUs;

          if (blockedUs <= 0) {
            product->state = PRODUCT_STATE_CANCELLED;
            break;
          }

          product->lengthCm = (float)blockedUs * sp.v1 / 1000000.0f;

          // The destination is still unknown at this point. Count this product
          // conservatively against all areas until G2/G3/G4 classification is known.
          reserveUnknownSlot(product);

          product->g2ArrivalUs =
            product->sensorEvent.riseTimeUs + cmToMicroseconds(sp.d0, sp.v1);

          product->g3ArrivalUs =
            product->sensorEvent.riseTimeUs + cmToMicroseconds(sp.d2, sp.v1);

          product->g4ArrivalUs =
            product->sensorEvent.riseTimeUs + cmToMicroseconds(sp.d4, sp.v1);

          product->state = PRODUCT_STATE_LENGTH_READY;
          break;
        }

      // ------------------------------------------------------------
      // G2 decision: abnormal size goes to Area 1.
      // ------------------------------------------------------------
      case PRODUCT_STATE_LENGTH_READY: {
          if (!isLengthAccepted(product->lengthCm)) {
            product->destination = AREA_1;
            product->weightG = 0.0f;
            product->barcode = 0;

            commitReservedProductToArea(product, AREA_1);

            scheduleGateCommand(SORT_GATE_2,
                                true,
                                product->g2ArrivalUs,
                                product->lengthCm,
                                product->runId);

            scheduleArrivalCommand(product, product->g2ArrivalUs + 5000);

            product->state = PRODUCT_STATE_DONE;
          }
          else {
            scheduleGateCommand(SORT_GATE_2,
                                false,
                                product->g2ArrivalUs,
                                product->lengthCm,
                                product->runId);

            product->state = PRODUCT_STATE_WAIT_WEIGHT;
          }

          break;
        }

      // ------------------------------------------------------------
      // Read weight at product centre alignment.
      // ------------------------------------------------------------
      case PRODUCT_STATE_WAIT_WEIGHT: {
          float centreDistanceCm = sp.d1 - (product->lengthCm / 2.0f);

          if (centreDistanceCm < 0.0f) {
            centreDistanceCm = 0.0f;
          }

          int64_t centreUs =
            product->sensorEvent.fallTimeUs +
            cmToMicroseconds(centreDistanceCm, sp.v1);

          product->weightG =
            readWeightAtCorrectTime(centreUs,
                                    product->lengthCm,
                                    product->runId);

          product->state = PRODUCT_STATE_WEIGHT_READY;
          break;
        }

      // ------------------------------------------------------------
      // G3 decision: abnormal weight goes to Area 1.
      // ------------------------------------------------------------
      case PRODUCT_STATE_WEIGHT_READY: {
          if (!isWeightAccepted(product->weightG)) {
            product->destination = AREA_1;
            product->barcode = 0;

            commitReservedProductToArea(product, AREA_1);

            scheduleGateCommand(SORT_GATE_3,
                                true,
                                product->g3ArrivalUs,
                                product->lengthCm,
                                product->runId);

            scheduleArrivalCommand(product, product->g3ArrivalUs + 5000);

            product->state = PRODUCT_STATE_DONE;
          }
          else {
            scheduleGateCommand(SORT_GATE_3,
                                false,
                                product->g3ArrivalUs,
                                product->lengthCm,
                                product->runId);

            product->state = PRODUCT_STATE_WAIT_BARCODE;
          }

          break;
        }

      // ------------------------------------------------------------
      // Read barcode at product centre alignment.
      // ------------------------------------------------------------
      case PRODUCT_STATE_WAIT_BARCODE: {
          float centreDistanceCm = sp.d3 - (product->lengthCm / 2.0f);

          if (centreDistanceCm < 0.0f) {
            centreDistanceCm = 0.0f;
          }

          int64_t centreUs =
            product->sensorEvent.fallTimeUs +
            cmToMicroseconds(centreDistanceCm, sp.v1);

          product->barcode =
            readBarcodeAtCorrectTime(centreUs,
                                     product->lengthCm,
                                     product->runId);

          product->state = PRODUCT_STATE_BARCODE_READY;
          break;
        }

      // ------------------------------------------------------------
      // G4 decision: Area 2 if small/light, otherwise Area 3.
      // ------------------------------------------------------------
      case PRODUCT_STATE_BARCODE_READY: {
          if (isArea2Product(product->lengthCm, product->weightG)) {
            product->destination = AREA_2;

            commitReservedProductToArea(product, AREA_2);

            scheduleGateCommand(SORT_GATE_4,
                                false,
                                product->g4ArrivalUs,
                                product->lengthCm,
                                product->runId);

            int64_t arrivalUs =
              product->g4ArrivalUs + cmToMicroseconds(sp.d5, sp.v2);

            scheduleArrivalCommand(product, arrivalUs);
          }
          else {
            product->destination = AREA_3;

            commitReservedProductToArea(product, AREA_3);

            scheduleGateCommand(SORT_GATE_4,
                                true,
                                product->g4ArrivalUs,
                                product->lengthCm,
                                product->runId);

            int64_t arrivalUs =
              product->g4ArrivalUs + cmToMicroseconds(sp.d6, sp.v3);

            scheduleArrivalCommand(product, arrivalUs);
          }

          product->state = PRODUCT_STATE_DONE;
          break;
        }

      // ------------------------------------------------------------
      // End states.
      // ------------------------------------------------------------
      case PRODUCT_STATE_DONE:
      case PRODUCT_STATE_CANCELLED:
      default:
        releaseUnknownSlot(product);
        vPortFree(product);
        vTaskDelete(NULL);
        return;
    }
  }

  releaseUnknownSlot(product);
  vPortFree(product);
  vTaskDelete(NULL);
}


// ============================================================================
// SERIAL AND STATUS TASK - CORE 1
// ============================================================================

void backgroundTask(void *pvParameters) {
  TickType_t lastStatusTick = xTaskGetTickCount();

  while (true) {
    // Fast command handling: this keeps emergency/reset/stat responsive instead
    // of waiting for the one-second status period.
    while (Serial.available() > 0) {
      String cmd = Serial.readStringUntil('\n');
      cmd.trim();

      if (cmd == "reset") {
        resetControllerState();
      }
      else if (cmd == "emergency") {
        emergencyActive = true;

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

        Serial.println("EMERGENCY: System halted");
      }
      else if (cmd == "stat") {
        float Lavg = 0.0f;
        float Wavg = 0.0f;
        float L2avg = 0.0f;
        float W2avg = 0.0f;
        float L3avg = 0.0f;
        float W3avg = 0.0f;

        if (runtimeMutex != NULL) {
          xSemaphoreTake(runtimeMutex, portMAX_DELAY);

          if (runtimeData.totalProducts > 0) {
            Lavg = runtimeData.totalLength / runtimeData.totalProducts;
            Wavg = runtimeData.totalWeight / runtimeData.totalProducts;
          }

          if (runtimeData.area2Products > 0) {
            L2avg = runtimeData.area2Length / runtimeData.area2Products;
            W2avg = runtimeData.area2Weight / runtimeData.area2Products;
          }

          if (runtimeData.area3Products > 0) {
            L3avg = runtimeData.area3Length / runtimeData.area3Products;
            W3avg = runtimeData.area3Weight / runtimeData.area3Products;
          }

          xSemaphoreGive(runtimeMutex);
        }

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

    refreshIntakeGate();

    TickType_t nowTick = xTaskGetTickCount();

    if ((nowTick - lastStatusTick) >= pdMS_TO_TICKS(1000)) {
      lastStatusTick = nowTick;

      if (runtimeMutex != NULL) {
        xSemaphoreTake(runtimeMutex, portMAX_DELAY);

        // Required real-time status line is always printed, including during
        // capacity hold and emergency states, so the output format remains stable.
        Serial.printf("sortRT: %03d,%03d,%03d",
                      runtimeData.areaCount[0],
                      runtimeData.areaCount[1],
                      runtimeData.areaCount[2]);

        for (int i = 0; i < statusCount; i++) {
          int idx =
            (statusWriteIndex - statusCount + i + STATUS_BUFFER_SIZE)
            % STATUS_BUFFER_SIZE;

          StatusProduct product = statusBuffer[idx];

          if (product.barcode != 0) {
            Serial.printf(",TN%d,%.1f,%.1f,A%d",
                          product.barcode,
                          product.lengthCm,
                          product.weightG,
                          (int)product.destination);
          }
          else {
            Serial.printf(",0,%.1f,%.1f,A%d",
                          product.lengthCm,
                          product.weightG,
                          (int)product.destination);
          }
        }

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

    vTaskDelay(pdMS_TO_TICKS(10));
  }
}


// ============================================================================
// SETUP
// ============================================================================

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

  sensorEventQueue = xQueueCreate(16, sizeof(SensorBlockEvent));
  if (sensorEventQueue == NULL) {
    Serial.println("ERROR: sensorEventQueue creation failed");
  }

  gateCommandQueue = xQueueCreate(GATE_COMMAND_QUEUE_LEN, sizeof(GateCommand));
  if (gateCommandQueue == NULL) {
    Serial.println("ERROR: gateCommandQueue creation failed");
  }

  arrivalCommandQueue = xQueueCreate(ARRIVAL_QUEUE_LEN, sizeof(ArrivalCommand));
  if (arrivalCommandQueue == NULL) {
    Serial.println("ERROR: arrivalCommandQueue creation failed");
  }

  runtimeMutex = xSemaphoreCreateMutex();
  if (runtimeMutex == NULL) {
    Serial.println("ERROR: runtimeMutex creation failed");
  }

  // Initial gate states.
  g_gate1Ctrl = true;
  g_gate2Ctrl = false;
  g_gate3Ctrl = false;
  g_gate4Ctrl = false;

  // Core 0: dedicated gate manager.
  xTaskCreatePinnedToCore(
    gateCommandManagerTask,
    "GateManager",
    4096,
    NULL,
    9,
    NULL,
    0
  );

  // Core 0: product inspection and state-machine dispatch.
  xTaskCreatePinnedToCore(
    productProcessingTask,
    "ProductDispatch",
    4096,
    NULL,
    10,
    NULL,
    0
  );

  // Core 1: downstream arrival tracking and area occupancy.
  xTaskCreatePinnedToCore(
    arrivalTrackingTask,
    "ArrivalTracker",
    4096,
    NULL,
    4,
    NULL,
    1
  );

  // Core 1: serial commands, status output, and G1 monitoring.
  xTaskCreatePinnedToCore(
    backgroundTask,
    "SerialStatus",
    4096,
    NULL,
    2,
    NULL,
    1
  );

  // Start simulator after all RTOS objects/tasks are ready.
  startSimulator();
}


// ============================================================================
// LOOP
// ============================================================================

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