// sketch.ino
// Main sorting loop (Sequential Blocking Architecture)

#include <Arduino.h>
#include "user_interface.h"
#include "bin_sensors.h" 
#include "stepper_motor.h"
#include "electromagnet.h"
#include "rotation_servo.h"
#include "tray_servo.h"

bool isCalibrated = false;

// --- EMERGENCY STOP FUNCTION ---
void checkPause() {
  if (isEStopPressed()) {
    deactivateMagnet(); // Safety first: Release scrap immediately
    Serial.println("--- EMERGENCY STOP ACTIVATED (Magnet OFF)---");
    
    while (isEStopPressed()) { delay(10); }
    delay(500); // Debounce
    
    while (!isStartPressed()) {
      // System holds current motor positions but does not advance
      delay(100);
    }
    
    Serial.println("--- RESUMING CYCLE... ---");
    delay(200); // Cooldown delay after resuming
  }
}

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

  setupUI();
  setupSensors(); 
  setupStepper();
  setupElectromagnet();
  setupRotationServo();
  setupTrayServo();

  // Initial calibration is required once after powering on the system
  Serial.println("--- System Standby ---");
  Serial.println("Step 1A: Press the calibration button before starting sorting.");
}

void loop() {
  // --- Step 1: System Initialization & Calibration ---
  // Homing sequence: The stepper lifts the arm until the limit switch is triggered
  if (isCalibratePressed()) {
    Serial.println("Step 1B: Calibrating Linear actuator (Lifting arm with electromagnet)");
    
    calibrateActuator(limitSwitchPin);
    
    isCalibrated = true;
    Serial.println("Step 2: System Ready. Push the button to start."); 

    delay(200); // Cooldown delay after calibration
  }

  // --- Step 2: Sorting Process Initiation ---
  else if (isStartPressed()) {
    delay(200); // Cooldown delay to prevent double detection/prints
    if (!isCalibrated) {
      Serial.println("[ERROR] System not calibrated. Press the calibrate button.");
    } else {
      // Turn off the LEDs
      turnOffFerroLed();
      turnOffNonFerroLed();

      executeSortingSequence();

      delay(200); // Cooldown delay after finishing cycle
      
      Serial.println(">> System ready for next batch.");
    }
  }
  
  // --- Step 7: Continuous Bin Level Monitoring ---
  // Sensors monitor fill levels only when the system is in Standby mode
  else if (isCalibrated) {
    updateBinStatus(); 
    updateLedAlerts(isFerroFull, isNonFerroFull); 
  }
}

void executeSortingSequence() {
  Serial.println("\n--- SORTING CYCLE STARTED ---");

  // --- Step 3A: Lowering actuator into scrap pile ---
  Serial.println("Step 3A: Lowering actuator into scrap pile...");
  moveActuatorDown();
  checkPause();

  // --- Step 3B: Safety Gap and Electromagnet Activation ---
  // Wait (2s) for stability before turning the magnet ON
  Serial.println("Step 3B: Activating the ELECTROMAGNET..."); 
  unsigned long prepTimer = millis();
  while (millis() - prepTimer < prepTime) { 
    checkPause();
    delay(10);
  }

  // --- Step 3B: Magnet Activation ---
  // Actual grab with responsive pause
  Serial.println("Step 3B: ELECTROMAGNET ON (Picking up scrap)");
  activateMagnet(); 
  
  unsigned long grabTimer = millis();
  while (millis() - grabTimer < grabTime) { 
    checkPause();
    delay(10);
  }

  // --- Step 4A: Retracting actuator to elevated position. ---
  Serial.println("Step 4A: Retracting actuator to elevated position...");
  moveActuatorUp(); 
  checkPause();

  // --- Step 4B: Routing Rotation_Servo to bin 1 (ferromagnetic scrap) ---
  Serial.println("Step 4B: Rotating arm to Ferromagnetic Bin...");
  moveArmToBin();   
  checkPause();

  // --- Step 4C: Deactivating Electromagnet above the ferromagnetic bin 1 & dropping scrap ---
  Serial.println("Step 4C: Deactivating the ELECTROMAGNET...");
  Serial.println("Step 4C: ELECTROMAGNET OFF (Dropping scrap)...");
  deactivateMagnet(); 

  // Wait for scrap to fall (Responsive drop time)
  unsigned long dropTimer = millis();
  while (millis() - dropTimer < dropTime) {
    checkPause();
    delay(10);
  }

  // --- Step 5A: Opening main tray to dump non-ferromagnetic residue scrap into Bin 2 ---
  Serial.println("Step 5A: Opening main tray (Dumping to Bin 2)...");
  openTray();
  checkPause();

  // --- Step 5B: Returning tray to horizontal lock... ---
  Serial.println("Step 5B: Closing main tray...");
  closeTray();

  // --- Step 6A: Returning arm to clear Home position ---
  Serial.println("Step 6A: Returning arm to Home position...");
  moveArmToHome();

  // --- Step 7: Activating the sensor for scanning (message only since the sensors are continuously monitoring the bin levels) ---
  Serial.println("Step 7: Scanning bin levels...");
  Serial.println("--- SORTING CYCLE COMPLETE ---\n");
}