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

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

    waitForCalibrateRelease(); // Prevent re-triggering calibration if button is held
  }

  // --- Step 2: Sorting Process Initiation ---
  else if (isStartPressed()) {
    if (!isCalibrated) {
      Serial.println("[ERROR] System not calibrated. Press the calibrate button.");
      waitForStartRelease(); 
    } else {
      turnOffFerroLed();
      turnOffNonFerroLed();

      executeSortingSequence();

      // Ensure the cycle only runs once per button press
      waitForStartRelease(); 
      
      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: Move actuator into the scrap pile ---
  Serial.println("Step 3A: Lowering actuator into scrap pile...");
  moveActuatorDown();

  // --- Step 3B: Wait 3s for the magnetic field to fully secure the scrap ---
  Serial.println("Step 3B: Activating the ELECTROMAGNET...");
  Serial.println("Step 3B: ELECTROMAGNET ON (Picking up scrap)");
  activateMagnet(); 

  // --- Step 4A: Raise the arm to safe travel height ---
  Serial.println("Step 4A: Retracting actuator to elevated position...");
  moveActuatorUp(); 

  // --- Step 4B: Rotate arm toward the Ferromagnetic Bin (Bin 1) ---
  Serial.println("Step 4B: Rotating arm to Ferromagnetic Bin...");
  moveArmToBin();   

  // --- Step 4C: Deactivate magnet to drop ferromagnetic scrap ---
  Serial.println("Step 4C: Deactivating the ELECTROMAGNET...");
  Serial.println("Step 4C: ELECTROMAGNET OFF (Dropping scrap)...");
  deactivateMagnet(); 

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

  // --- Step 5B: Return tray to horizontal locked position ---
  Serial.println("Step 5B: Closing main tray...");
  closeTray();

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

  // --- Step 7: Final fill level check for both bins ---
  Serial.println("Step 7: Scanning bin levels...");
  Serial.println("--- SORTING CYCLE COMPLETE ---\n");
}