#include "Arduino.h"

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

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


volatile int errorGateCNT[3] = {0};

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

extern void startSimulator(void);

// ============================================================================
// PARAMETER GENERATION (deterministic from student number)
// ============================================================================
static uint32_t paramHash(uint32_t seed, uint32_t idx)
{
  uint32_t v = seed * 2654435761u + idx * 2246822519u;
  v ^= v >> 16;
  v *= 0x45d9f3bu;
  v ^= v >> 16;
  return v;
}

static float paramRangeF(uint32_t seed, uint32_t idx, float lo, float hi)
{
  return lo + (float)(paramHash(seed, idx) % 10001u) / 10000.0f * (hi - lo);
}

static int paramRangeI(uint32_t seed, uint32_t idx, int lo, int hi)
{
  return lo + (int)(paramHash(seed, idx) % (uint32_t)(hi - lo + 1));
}

void generateParameters(uint32_t sn)
{
  if (sn == 0)
  {
    // Default parameters (no student number set)
    sp.v1 = 100.0f;
    sp.v2 = 120.0f;
    sp.v3 = 80.0f;
    sp.d0 = 15.0f;
    sp.d1 = 25.0f;
    sp.d2 = 35.0f;
    sp.d3 = 45.0f;
    sp.d4 = 55.0f;
    sp.d5 = 40.0f;
    sp.d6 = 40.0f;
    sp.L_min = 4.5f;
    sp.L_max = 5.5f;
    sp.L_threshold = 5.0f;
    sp.W_min = 90.0f;
    sp.W_max = 110.0f;
    sp.W_threshold = 100.0f;
    sp.T_gate = 3.0f;
    sp.C1 = 5;
    sp.C2 = 10;
    sp.C3 = 10;
    sp.productInterval = 500;
    return;
  }

  sp.v1 = paramRangeF(sn, 1, 80.0f, 120.0f);
  sp.v2 = paramRangeF(sn, 2, 100.0f, 150.0f);
  sp.v3 = paramRangeF(sn, 3, 60.0f, 100.0f);

  sp.d0 = paramRangeF(sn, 4, 12.0f, 20.0f);
  sp.d1 = sp.d0 + paramRangeF(sn, 5, 8.0f, 15.0f);
  sp.d2 = sp.d1 + paramRangeF(sn, 6, 8.0f, 15.0f);
  sp.d3 = sp.d2 + paramRangeF(sn, 7, 8.0f, 15.0f);
  sp.d4 = sp.d3 + paramRangeF(sn, 8, 8.0f, 15.0f);
  sp.d5 = paramRangeF(sn, 9, 30.0f, 50.0f);
  sp.d6 = paramRangeF(sn, 10, 30.0f, 50.0f);

  sp.L_min = paramRangeF(sn, 11, 4.0f, 5.0f);
  sp.L_max = sp.L_min + paramRangeF(sn, 12, 0.8f, 1.5f);
  sp.L_threshold = (sp.L_min + sp.L_max) / 2.0f;

  sp.W_min = paramRangeF(sn, 13, 80.0f, 100.0f);
  sp.W_max = sp.W_min + paramRangeF(sn, 14, 15.0f, 30.0f);
  sp.W_threshold = (sp.W_min + sp.W_max) / 2.0f;

  sp.T_gate = paramRangeF(sn, 15, 2.0f, 5.0f);

  sp.C1 = paramRangeI(sn, 16, 3, 8);
  sp.C2 = paramRangeI(sn, 17, 8, 15);
  sp.C3 = paramRangeI(sn, 18, 8, 15);

  sp.productInterval = paramRangeI(sn, 19, 400, 600);
}

void printParameters()
{
  Serial.println("========== SmartSort System Parameters ==========");
  Serial.printf("Student Number: %u\n", (unsigned)STUDENT_NUMBER);
  Serial.println("----- Conveyor Speeds -----");
  Serial.printf("  v1 (Conv1): %.1f cm/s\n", sp.v1);
  Serial.printf("  v2 (Conv2): %.1f cm/s\n", sp.v2);
  Serial.printf("  v3 (Conv3): %.1f cm/s\n", sp.v3);
  Serial.println("----- Distances (from obstruction sensor, cm) -----");
  Serial.printf("  d0 (to G2):      %.1f\n", sp.d0);
  Serial.printf("  d1 (to Weight):  %.1f\n", sp.d1);
  Serial.printf("  d2 (to G3):      %.1f\n", sp.d2);
  Serial.printf("  d3 (to Barcode): %.1f\n", sp.d3);
  Serial.printf("  d4 (to G4):      %.1f\n", sp.d4);
  Serial.printf("  d5 (Conv2 len):  %.1f\n", sp.d5);
  Serial.printf("  d6 (Conv3 len):  %.1f\n", sp.d6);
  Serial.println("----- Length Criteria (cm) -----");
  Serial.printf("  L_min=%.2f  L_max=%.2f  L_threshold=%.2f\n", sp.L_min, sp.L_max, sp.L_threshold);
  Serial.println("----- Weight Criteria (g) -----");
  Serial.printf("  W_min=%.2f  W_max=%.2f  W_threshold=%.2f\n", sp.W_min, sp.W_max, sp.W_threshold);
  Serial.println("----- Timing & Capacity -----");
  Serial.printf("  T_gate=%.1f ms  Interval=%d ms\n", sp.T_gate, sp.productInterval);
  Serial.printf("  C1=%d  C2=%d  C3=%d\n", sp.C1, sp.C2, sp.C3);
  Serial.println("=================================================");
}

// ============================================================================
// PRODUCT ARRAY - Generated based on parameters to cover all sorting paths
// Students may modify this array for additional testing
// ============================================================================
#define MAX_TEST_PRODUCTS 20
Product productArray[MAX_TEST_PRODUCTS];
int numProducts = 0;

void generateProductArray()
{
  int idx = 0;
  float Lmid = (sp.L_min + sp.L_max) / 2.0f;
  float Wmid = (sp.W_min + sp.W_max) / 2.0f;

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

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

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

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

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

  // Category 6: Normal, large or heavy -> Area 3
  productArray[idx++] = {sp.L_threshold + 0.1f, sp.W_threshold - 1.0f, 1012};
  productArray[idx++] = {sp.L_min + 0.2f, sp.W_threshold + 1.0f, 1013};
  productArray[idx++] = {sp.L_max - 0.1f, sp.W_max - 1.0f, 1014};
  productArray[idx++] = {sp.L_threshold + 0.2f, sp.W_threshold + 2.0f, 1015};

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

  numProducts = idx;
}

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

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

/*
   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
#define sizeAbnormalPin 25
#define weightAbnormalPin 26
#define g4DebugPin 27

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

volatile bool emergencyActive = 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)
//
// Key APIs:
//   esp_timer_get_time()          - ISR-safe microsecond timestamp
//   xQueueCreate()                - Create FreeRTOS queue
//   xQueueSendFromISR()           - Send data from ISR to task
//   xQueueReceive()               - Receive data in task (blocking)
//   xTaskCreatePinnedToCore()     - Create task on specific core
//   xTimerCreate()                - Create software timer
//   vTaskDelay()                  - Delay task (ms / portTICK_PERIOD_MS)

// ============================================================================
// OBSTRUCTION SENSOR ISR
// This ISR is called by the simulator when the obstruction sensor state changes.
// It runs in hardware timer ISR context — only use ISR-safe functions.
//
// When g_obstructionSensor == true:  rising edge (product front arrived)
// When g_obstructionSensor == false: falling edge (product back passed)
//
// TODO: Implement this ISR to capture timestamps and communicate with tasks
// Ref: Lab session material - Interrupts, Queues
// ============================================================================
/* ================= GLOBAL ================= */

QueueHandle_t sensorQueue;

struct SensorEvent
{
  bool state;
  uint64_t time;
};

struct ProductState
{
  float length;
  float weight;
  int barcode;
  uint64_t t_rise;
  bool weightRead;
  bool barcodeRead;
  int destination; // 0=Area1, 1=Area2, 2=Area3
  bool finished;

  // Gate flags
  bool g2_triggered;
  bool g3_triggered;
  bool g4_triggered;

  // Timing
  uint64_t gate_off_time; // Reuse for all gates
  int active_gate;        // Which gate is currently active (-1, 2, 3, 4)

  bool diverted; // True if product was diverted by G2 or G3

  int trackingNum;

  bool sizeAbnormal;
  bool weightAbnormal;
};

#define MAX_PRODUCTS 20
ProductState products[MAX_PRODUCTS];
int productIndex = 0;

/* ================= ISR ================= */

void IRAM_ATTR obstructionSensorInterrupt()
{
  SensorEvent evt;
  evt.state = g_obstructionSensor;
  evt.time = esp_timer_get_time();

  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  xQueueSendFromISR(sensorQueue, &evt, &xHigherPriorityTaskWoken);
  if (xHigherPriorityTaskWoken)
    portYIELD_FROM_ISR();
}

/* ================= CORE TASK ================= */

void controlTask(void *pvParameters)
{
  SensorEvent evt;
  uint64_t riseTime = 0;
  bool measuring = false;
  float epsilon = 0.0001f;

  while (1)
  {
    if (emergencyActive) {
      vTaskDelay(10 / portTICK_PERIOD_MS);
      continue;
    }

    // Process sensor events
    if (xQueueReceive(sensorQueue, &evt, 0))
    {
      if (evt.state == true)
      {
        riseTime = evt.time;
        measuring = true;
      }
      else if (measuring)
      {
        float duration = (evt.time - riseTime) / 1000000.0;
        float length = duration * sp.v1;
        float epsilon = 0.001;

        if (productIndex < MAX_PRODUCTS)
        {
          ProductState &p = products[productIndex];
          p.length = length;
          p.t_rise = riseTime;
          p.weight = 0;
          p.barcode = 0;
          p.weightRead = false;
          p.barcodeRead = false;
          p.destination = -1; // Unknown - not yet classified
          p.finished = false;
          p.diverted = false;
          p.sizeAbnormal = (length < sp.L_min - epsilon || length > sp.L_max + epsilon);
          p.weightAbnormal = false;

          // Gate flags
          p.g2_triggered = false;
          p.g3_triggered = false;
          p.g4_triggered = false;

          // Timing
          p.gate_off_time = 0;
          p.active_gate = -1;
          p.trackingNum = 0;

          productIndex++;
          measuring = false;
        }
      }
    }

    uint64_t now = esp_timer_get_time();

    for (int i = 0; i < productIndex; i++)
    {
      ProductState &p = products[i];
      if (p.finished)
        continue;

      // === G2 - SIZE DIVERSION ===
      if (!p.g2_triggered && p.destination == -1 && !p.diverted)
      {
        uint64_t d0_time = p.t_rise + (uint64_t)((sp.d0 / sp.v1) * 1000000);
        uint64_t gate_activate_start = d0_time - (uint64_t)(sp.T_gate * 1000) - 30000;
        uint64_t gate_activate_end = d0_time + 30000;

        if (now >= gate_activate_start && now <= gate_activate_end)
        {
          bool sizeAbnormal = (p.length < sp.L_min - epsilon || p.length > sp.L_max + epsilon);

          if (sizeAbnormal)
          {
            g_gate2Ctrl = true;
            p.active_gate = 2;
            p.destination = 0; // Area 1
            p.diverted = true;
            p.gate_off_time = d0_time + (uint64_t)((p.length / sp.v1) * 1000000);
            digitalWrite(sizeAbnormalPin, HIGH);
          }
          else
          {
            g_gate2Ctrl = false;
          }
          p.g2_triggered = true;
        }
      }

      // === RESET G2 ===
      if (p.active_gate == 2 && now >= p.gate_off_time)
      {
        g_gate2Ctrl = false;
        p.active_gate = -1;
        digitalWrite(sizeAbnormalPin, LOW);
      }

      // === WEIGHT MEASUREMENT ===
      if (!p.weightRead && !p.diverted)
      {
        uint64_t d1_center = p.t_rise + (uint64_t)((sp.d1 / sp.v1) * 1000000);
        uint64_t window_start = d1_center - 50000;
        uint64_t window_end = d1_center + 50000;

        if (now >= window_start && now <= window_end)
        {
          if (g_weightSensor > 0 && !p.weightRead)
          {
            p.weight = g_weightSensor;
            p.weightRead = true;
            p.weightAbnormal = (p.weight < sp.W_min || p.weight > sp.W_max);
          }
        }
        else if (now > window_end && !p.weightRead)
        {
          p.weightRead = true;
          p.weightAbnormal = true; // Mark as abnormal if no reading
        }
      }

      // === G3 - WEIGHT DIVERSION ===
      // ONLY run G3 if product is NOT size abnormal, NOT diverted, weight is read, and weight > 0
      if (!p.g3_triggered && !p.diverted && p.weightRead && p.weight > 0 && !p.sizeAbnormal)
      {
        uint64_t d2_time = p.t_rise + (uint64_t)((sp.d2 / sp.v1) * 1000000);
        uint64_t gate_activate_start = d2_time - (uint64_t)(sp.T_gate * 1000) - 20000;
        uint64_t gate_activate_end = d2_time + 20000;

        if (now >= gate_activate_start && now <= gate_activate_end)
        {
          bool weightAbnormal = (p.weight < sp.W_min - epsilon || p.weight > sp.W_max + epsilon);

          if (weightAbnormal)
          {
            g_gate3Ctrl = true;
            p.active_gate = 3;
            p.destination = 0; // Area 1
            p.diverted = true;
            p.gate_off_time = d2_time + (uint64_t)((p.length / sp.v1) * 1000000);
            digitalWrite(weightAbnormalPin, HIGH);
          }
          else
          {
            g_gate3Ctrl = false;
          }
          p.g3_triggered = true;
        }
      }
      else if (!p.g3_triggered && (p.diverted || p.sizeAbnormal))
      {
        // Skip G3 for already diverted or size abnormal products
        p.g3_triggered = true;
      }

      // === RESET G3 ===
      if (p.active_gate == 3 && now >= p.gate_off_time)
      {
        g_gate3Ctrl = false;
        p.active_gate = -1;
        digitalWrite(weightAbnormalPin, LOW);
      }

      // === BARCODE SCANNING ===
      if (!p.barcodeRead && !p.diverted && p.weightRead)
      {
        uint64_t d3_center = p.t_rise + (uint64_t)((sp.d3 / sp.v1) * 1000000);
        uint64_t window_start = d3_center - 20000;
        uint64_t window_end = d3_center + 50000;

        if (now >= window_start && now <= window_end)
        {
          if (g_barcodeReader != 0 && !p.barcodeRead)
          {
            p.barcode = g_barcodeReader;
            p.barcodeRead = true;
            p.trackingNum = g_barcodeReader;
          }
        }
        else if (now > window_end && !p.barcodeRead)
        {
          p.barcode = 0;
          p.barcodeRead = true;
          p.trackingNum = 0;
        }
      }

      // === G4 - FINAL ROUTING ===
      // ONLY run G4 if NOT diverted, G2/G3 done, barcode read
      if (!p.g4_triggered && !p.diverted && p.barcodeRead && p.g2_triggered && p.g3_triggered)
      {
        uint64_t d4_time = p.t_rise + (uint64_t)((sp.d4 / sp.v1) * 1000000);
        uint64_t gate_activate_start = d4_time - (uint64_t)(sp.T_gate * 1000) - 20000;
        uint64_t gate_activate_end = d4_time + 20000;

        if (now >= gate_activate_start && now <= gate_activate_end)
        {
          // Route based on classification
          bool toArea2 = (p.length <= sp.L_threshold + epsilon && p.weight <= sp.W_threshold + epsilon);

          if (toArea2)
          {
            g_gate4Ctrl = false;
            p.destination = 1; // Area 2
            digitalWrite(g4DebugPin, LOW);
          }
          else
          {
            g_gate4Ctrl = true;
            p.destination = 2; // Area 3
            digitalWrite(g4DebugPin, HIGH);
          }
          p.g4_triggered = true;
          p.active_gate = 4;
          p.gate_off_time = d4_time + (uint64_t)((p.length / sp.v1) * 1000000);
        }
      }

      // === RESET G4 ===
      if (p.active_gate == 4 && now >= p.gate_off_time)
      {
        g_gate4Ctrl = false;
        p.active_gate = -1;
        digitalWrite(g4DebugPin, LOW);
      }

      // === MARK PRODUCT AS FINISHED ===
      if (p.g4_triggered && !p.finished)
      {
        uint64_t finish_time = 0;

        if (p.destination == 1) // Area 2
        {
          uint64_t travel_time = (uint64_t)((sp.d5 / sp.v2) * 1000000);
          uint64_t g4_time = p.t_rise + (uint64_t)((sp.d4 / sp.v1) * 1000000);
          finish_time = g4_time + travel_time;
        }
        else if (p.destination == 2) // Area 3
        {
          uint64_t travel_time = (uint64_t)((sp.d6 / sp.v3) * 1000000);
          uint64_t g4_time = p.t_rise + (uint64_t)((sp.d4 / sp.v1) * 1000000);
          finish_time = g4_time + travel_time;
        }
        else if (p.destination == 0) // Area 1 - diverted
        {
          finish_time = p.t_rise + (uint64_t)(((sp.d4 + 30) / sp.v1) * 1000000);
        }

        finish_time += 5000;

        if (now >= finish_time)
        {
          p.finished = true;
          
          // Update in-transit counters (if you have them)
          // if (p.destination == 1 && inTransitToArea2 > 0) inTransitToArea2--;
          // else if (p.destination == 2 && inTransitToArea3 > 0) inTransitToArea3--;
          // else if (p.destination == 0 && inTransitToArea1 > 0) inTransitToArea1--;
        }
      }
    }

    vTaskDelay(1 / portTICK_PERIOD_MS);
  }
}

/* ================= G1 CONTROL ================= */

void gate1Task(void *pvParameters)
{
  while (1)
  {
    if (emergencyActive) {
      vTaskDelay(10 / portTICK_PERIOD_MS);
      continue;
    }

    // Check if there's ANY capacity in the system
    // bool area1HasSpace = (simAreaCount[0] < sp.C1);
    bool area2HasSpace = (simAreaCount[1] < sp.C2);
    bool area3HasSpace = (simAreaCount[2] < sp.C3);

    bool anySpace = /*area1HasSpace ||*/ area2HasSpace || area3HasSpace;

    // Also check total system capacity
    int totalOccupied = simAreaCount[0] + simAreaCount[1] + simAreaCount[2];
    int totalCapacity = sp.C1 + sp.C2 + sp.C3;

    // Only close G1 if NO space available anywhere
    if (!anySpace || totalOccupied >= totalCapacity)
    {
      if (g_gate1Ctrl)
      {
        g_gate1Ctrl = false;
        digitalWrite(gate4DebugPin, HIGH);
        // Serial.printf("[G1] CLOSED - No capacity left\n");
      }
    }
    else
    {
      if (!g_gate1Ctrl)
      {
        g_gate1Ctrl = true;
        digitalWrite(gate4DebugPin, HIGH);
        // Serial.printf("[G1] OPEN - Capacity available\n");
      }
    }

    vTaskDelay(100 / portTICK_PERIOD_MS);
  }
}

void statusOutputTask(void *pvParameters)
{
  while (1) {
    if (emergencyActive) {
      vTaskDelay(100 / portTICK_PERIOD_MS);
      continue;
    }

    vTaskDelay(1000 / portTICK_PERIOD_MS);

    // FIX: Use %03d for 3-digit zero padding
    Serial.printf("sortRT: %03d,%03d,%03d",
                  simAreaCount[0], simAreaCount[1], simAreaCount[2]);

    for (int i = 0; i < productIndex; i++) {
      ProductState &p = products[i];
      if (!p.finished && p.destination != -1) {
        char dest = (p.destination == 0) ? '1' : ((p.destination == 1) ? '2' : '3');
        int tn = (p.trackingNum != 0) ? p.trackingNum : (i + 1);
        Serial.printf(", TN%04d,%.1f,%.1f,A%c",
                      tn, p.length, p.weight, dest);
      }
    }
    Serial.println();
  }
}

void commandTask(void *pvParameters)
{
  while (1)
  {
    if (Serial.available())
    {
      String cmd = Serial.readStringUntil('\n');
      cmd.trim();

      if (cmd == "stat")
      {
        // Variables for overall averages
        float totalLength = 0;
        float totalWeight = 0;
        int totalProducts = 0;

        // Variables for area-specific averages
        float totalLengthArea2 = 0;
        float totalWeightArea2 = 0;
        int area2Count = 0;

        float totalLengthArea3 = 0;
        float totalWeightArea3 = 0;
        int area3Count = 0;

        // Iterate through all products that have finished
        for (int i = 0; i < productIndex; i++)
        {
          ProductState &p = products[i];

          // Only include finished products (already arrived at destination)
          if (p.finished)
          {
            // Overall averages (include all products)
            totalLength += p.length;
            totalWeight += (p.weight > 0 ? p.weight : 0);
            totalProducts++;

            // Area-specific averages (Area2 and Area3 only)
            if (p.destination == 1)
            { // Area 2
              totalLengthArea2 += p.length;
              totalWeightArea2 += (p.weight > 0 ? p.weight : 0);
              area2Count++;
            }
            else if (p.destination == 2)
            { // Area 3
              totalLengthArea3 += p.length;
              totalWeightArea3 += (p.weight > 0 ? p.weight : 0);
              area3Count++;
            }
            // Area 1 products are NOT included in area-specific averages
          }
        }

        // Calculate averages (avoid division by zero)
        float Lavg = (totalProducts > 0) ? (totalLength / totalProducts) : 0;
        float Wavg = (totalProducts > 0) ? (totalWeight / totalProducts) : 0;

        float L2avg = (area2Count > 0) ? (totalLengthArea2 / area2Count) : 0;
        float W2avg = (area2Count > 0) ? (totalWeightArea2 / area2Count) : 0;

        float L3avg = (area3Count > 0) ? (totalLengthArea3 / area3Count) : 0;
        float W3avg = (area3Count > 0) ? (totalWeightArea3 / area3Count) : 0;

        // Output in required format: sortSTAT: Lavg, L2avg, L3avg, Wavg, W2avg, W3avg
        Serial.printf("sortSTAT: %.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
                      Lavg, L2avg, L3avg, Wavg, W2avg, W3avg);
      }
      else if (cmd == "reset")
      {
        // Wait for all active products to finish (200ms should be enough)
        vTaskDelay(500 / portTICK_PERIOD_MS);

        // Reset all system states
        productIndex = 0;
        for (int i = 0; i < 3; i++)
        {
          simAreaCount[i] = 0;
          errorGateCNT[i] = 0;
        }
        // Clear all products
        for (int i = 0; i < MAX_PRODUCTS; i++)
        {
          products[i] = ProductState();
        }
        // Reset gates
        g_gate1Ctrl = true;
        g_gate2Ctrl = false;
        g_gate3Ctrl = false;
        g_gate4Ctrl = false;
        emergencyActive = false;

        // CRITICAL: Clear the sensor queue
        SensorEvent dummy;
        while (xQueueReceive(sensorQueue, &dummy, 0))
        {
          // Empty the queue
        }

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

/* ================= SETUP ================= */

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

  // Initialize sensor pins (inputs)
  pinMode(g_obstructionSensorPin, INPUT_PULLUP);  // Obstruction sensor
  pinMode(g_barcodeSensorPin, INPUT);          // Barcode sensor
  pinMode(g_weightSensorPin, INPUT);          // Weight sensor

  // Initialize gate pins (outputs)
  pinMode(sizeAbnormalPin, OUTPUT);  // G2 LED
  pinMode(weightAbnormalPin, OUTPUT);  // G3 LED
  pinMode(gate3DebugPin, OUTPUT);  // G4 LED
  pinMode(gate4DebugPin, OUTPUT);  // G1 LED

  // Initialize all gates to OFF
  digitalWrite(sizeAbnormalPin, LOW);
  digitalWrite(weightAbnormalPin, LOW);
  digitalWrite(gate3DebugPin, LOW);
  digitalWrite(gate4DebugPin, LOW);

  sensorQueue = xQueueCreate(10, sizeof(SensorEvent));

  xTaskCreatePinnedToCore(controlTask, "Control", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(gate1Task, "Gate1", 2048, NULL, 1, NULL, 1);

  // Create tasks
  xTaskCreatePinnedToCore(statusOutputTask, "Status", 4096, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(commandTask, "Commands", 4096, NULL, 1, NULL, 1);

  startSimulator();
}

void loop()
{
  vTaskDelay(portMAX_DELAY);
}

/******************************************** END OF STUDENT CODE *****************************************/
/*!!!!!!!!!!!!!!!!!!!!!!!!!!! ANY CODE BELOW THIS LINE SHOULD NOT BE MODIFIED !!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*================================ START OF SIMULATOR ENGINE ============================================*/

volatile int currentProductIndex = 0;
const int MAX_SCHEDULERS = 4;
ProductScheduler g_schedulers[MAX_SCHEDULERS];

static inline uint64_t distanceToMicros(float distance_cm, float speed_cm_s)
{
  double tSec = (double)distance_cm / (double)speed_cm_s;
  return (uint64_t)(tSec * 1e6);
}

void fillProductEvents(ProductScheduler &sch)
{
  const Product &p = sch.product;
  sch.totalEvents = 0;
  sch.currentEventIndex = 0;

  auto &events = sch.events;
  int idx = 0;

  uint64_t leadTime = distanceToMicros(DIST_LEAD, sp.v1);

  // Determine product's ground-truth path
  bool sizeOk = (p.length >= sp.L_min && p.length <= sp.L_max);
  bool weightOk = (p.weight >= sp.W_min && p.weight <= sp.W_max);
  bool isSmallLight = sizeOk && weightOk &&
                      (p.length <= sp.L_threshold) && (p.weight <= sp.W_threshold);

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

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

  // 2. Obstruction sensor: back edge leaves
  uint64_t fallTime = distanceToMicros(p.length, sp.v1);
  events[idx].triggerTimeMicros = leadTime + fallTime;
  events[idx].eventType = ProductEventType::OBSTRUCTION_SENSOR_FALL;
  events[idx].gateExpected = 0;
  events[idx].destArea = 0;
  idx++;

  // 3. G2 check (size gate) - front edge at d0
  // G2: true=divert (size abnormal), expected true if !sizeOk
  uint64_t g2Time = distanceToMicros(sp.d0, sp.v1) + leadTime;
  events[idx].triggerTimeMicros = g2Time;
  events[idx].eventType = ProductEventType::G2_CHECK;
  events[idx].gateExpected = sizeOk ? 0 : 1;
  events[idx].destArea = 0;
  idx++;

  if (!sizeOk)
  {
    // Product diverted at G2 -> Area 1
    events[idx].triggerTimeMicros = g2Time + 5000;
    events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
    events[idx].gateExpected = 0;
    events[idx].destArea = 0;
    idx++;

    events[idx].triggerTimeMicros = g2Time + 10000;
    events[idx].eventType = ProductEventType::FINISH;
    events[idx].gateExpected = 0;
    events[idx].destArea = 0;
    idx++;
  }
  else
  {
    // 4. Weight sensor window
    uint64_t centerAtWeight = distanceToMicros(sp.d1 + p.length / 2.0f, sp.v1) + leadTime;
    uint64_t halfWindowW = distanceToMicros(p.length * 0.1f, sp.v1);
    events[idx].triggerTimeMicros = centerAtWeight - halfWindowW;
    events[idx].eventType = ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW;
    events[idx].gateExpected = 0;
    events[idx].destArea = 0;
    idx++;

    events[idx].triggerTimeMicros = centerAtWeight + halfWindowW;
    events[idx].eventType = ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW;
    events[idx].gateExpected = 0;
    events[idx].destArea = 0;
    idx++;

    // 5. G3 check (weight gate) - front edge at d2
    // G3: true=divert (weight abnormal), expected true if !weightOk
    uint64_t g3Time = distanceToMicros(sp.d2, sp.v1) + leadTime;
    events[idx].triggerTimeMicros = g3Time;
    events[idx].eventType = ProductEventType::G3_CHECK;
    events[idx].gateExpected = weightOk ? 0 : 1;
    events[idx].destArea = 0;
    idx++;

    if (!weightOk)
    {
      // Product diverted at G3 -> Area 1
      events[idx].triggerTimeMicros = g3Time + 5000;
      events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
      events[idx].gateExpected = 0;
      events[idx].destArea = 0;
      idx++;

      events[idx].triggerTimeMicros = g3Time + 10000;
      events[idx].eventType = ProductEventType::FINISH;
      events[idx].gateExpected = 0;
      events[idx].destArea = 0;
      idx++;
    }
    else
    {
      // 6. Barcode sensor window
      uint64_t centerAtBarcode = distanceToMicros(sp.d3 + p.length / 2.0f, sp.v1) + leadTime;
      uint64_t halfWindowB = distanceToMicros(p.length * 0.1f, sp.v1);
      events[idx].triggerTimeMicros = centerAtBarcode - halfWindowB;
      events[idx].eventType = ProductEventType::BARCODE_SENSOR_ENTER_WINDOW;
      events[idx].gateExpected = 0;
      events[idx].destArea = 0;
      idx++;

      events[idx].triggerTimeMicros = centerAtBarcode + halfWindowB;
      events[idx].eventType = ProductEventType::BARCODE_SENSOR_EXIT_WINDOW;
      events[idx].gateExpected = 0;
      events[idx].destArea = 0;
      idx++;

      // 7. G4 check (routing gate) - front edge at d4
      // G4: false=Conv2/Area2, true=Conv3/Area3
      uint64_t g4Time = distanceToMicros(sp.d4, sp.v1) + leadTime;
      events[idx].triggerTimeMicros = g4Time;
      events[idx].eventType = ProductEventType::G4_CHECK;
      events[idx].gateExpected = isSmallLight ? 0 : 1;
      events[idx].destArea = 0;
      idx++;

      // 8. Arrive at destination area
      uint64_t arriveTime;
      if (isSmallLight)
      {
        arriveTime = g4Time + distanceToMicros(sp.d5, sp.v2);
      }
      else
      {
        arriveTime = g4Time + distanceToMicros(sp.d6, sp.v3);
      }
      events[idx].triggerTimeMicros = arriveTime;
      events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
      events[idx].gateExpected = 0;
      events[idx].destArea = destArea;
      idx++;

      events[idx].triggerTimeMicros = arriveTime + 5000;
      events[idx].eventType = ProductEventType::FINISH;
      events[idx].gateExpected = 0;
      events[idx].destArea = 0;
      idx++;
    }
  }

  sch.totalEvents = idx;

  // Sort events by time
  for (int i = 0; i < sch.totalEvents - 1; i++)
  {
    for (int j = i + 1; j < sch.totalEvents; j++)
    {
      if (events[i].triggerTimeMicros > events[j].triggerTimeMicros)
      {
        ProductEvent tmp = events[i];
        events[i] = events[j];
        events[j] = tmp;
      }
    }
  }
}

void printProductEvents(const ProductScheduler &sch)
{
  Serial.println("===== Product Event Timeline =====");
  // Serial.printf("Length: %.2f cm  Weight: %.2f g  Barcode: %d\n",
  //               sch.product.length, sch.product.weight, sch.product.barcodeID);

  bool sizeOk = (sch.product.length >= sp.L_min && sch.product.length <= sp.L_max);
  bool weightOk = (sch.product.weight >= sp.W_min && sch.product.weight <= sp.W_max);
  const char *dest = "???";
  if (!sizeOk)
    dest = "Area1(size)";
  else if (!weightOk)
    dest = "Area1(weight)";
  else if (sch.product.length <= sp.L_threshold && sch.product.weight <= sp.W_threshold)
    dest = "Area2";
  else
    dest = "Area3";

  for (int i = 0; i < sch.totalEvents; i++)
  {
    const ProductEvent &e = sch.events[i];
    // Serial.printf("[%10llu µs] Event: ", e.triggerTimeMicros);

    switch (e.eventType)
    {
      case ProductEventType::OBSTRUCTION_SENSOR_RISE:
        Serial.print("Obstruction Rise");
        break;
      case ProductEventType::OBSTRUCTION_SENSOR_FALL:
        Serial.print("Obstruction Fall");
        break;
      case ProductEventType::G2_CHECK:
        // Serial.printf("G2 Check (expected=%d)", e.gateExpected);
        break;
      case ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW:
        Serial.print("Weight Sensor Enter");
        break;
      case ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW:
        Serial.print("Weight Sensor Exit");
        break;
      case ProductEventType::G3_CHECK:
        // Serial.printf("G3 Check (expected=%d)", e.gateExpected);
        break;
      case ProductEventType::BARCODE_SENSOR_ENTER_WINDOW:
        Serial.print("Barcode Sensor Enter");
        break;
      case ProductEventType::BARCODE_SENSOR_EXIT_WINDOW:
        Serial.print("Barcode Sensor Exit");
        break;
      case ProductEventType::G4_CHECK:
        // Serial.printf("G4 Check (expected=%d)", e.gateExpected);
        break;
      case ProductEventType::ARRIVE_DESTINATION:
        // Serial.printf("Arrive %s", dest);
        break;
      case ProductEventType::FINISH:
        Serial.print("Finish");
        break;
    }
    Serial.println();
  }
  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)
  {
    // Serial.printf("[SIM DEBUG] Weight sensor ENTER for product %d, setting weight=%.2f\n",
    //               sch->product.barcodeID, sch->product.weight);
    g_weightSensor = sch->product.weight;
  }
  else if (evtType == (int)ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW)
  {
    // Serial.printf("[SIM DEBUG] Weight sensor EXIT for product %d, setting weight=0\n",
    //               sch->product.barcodeID);
    g_weightSensor = 0.0f;
  }
  else if (evtType == (int)ProductEventType::BARCODE_SENSOR_ENTER_WINDOW)
  {
    // Serial.printf("[SIM DEBUG] Barcode sensor ENTER for product %d, setting barcode=%d\n",
    //   sch->product.barcodeID, sch->product.barcodeID);
    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)
  {
    // 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 ===");
        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");
}