// sketch.ino
// Scrap_metal_sorterV4.ino
// Main sorting loop (Sequential Blocking Architecture) // [cite: 1]

#include <Arduino.h>
#include "config.h"

// --- TEST MODE SELECTOR ---
// Uncomment the following line to enable Manual Joystick Calibration
#define MODO_TEST
// ----------------

#ifdef MODO_TEST
  #include "config_test.h"
#endif

#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 --- // [cite: 2]
void checkPause() {
  if (isEStopPressed()) {
    deactivateMagnet(); // Safety first: Release scrap immediately // [cite: 3]
    Serial.println("--- EMERGENCY STOP ACTIVATED (Magnet OFF)---");
    
    while (isEStopPressed()) { delay(10); } // Wait while button is pressed // [cite: 4]
    delay(500); // Debounce
    
    while (!isStartPressed()) {
      // System holds current motor positions but does not advance // [cite: 5]
      delay(100);
    }
    
    Serial.println("--- RESUMING CYCLE... ---");
    delay(200); // Cooldown delay after resuming // [cite: 6]
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(PIN_JOY_SEL, INPUT_PULLUP); // Keeps the button from auto-triggering

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

  #ifdef MODO_TEST
    Serial.println("--- SYSTEM: TEST MODE INITIALIZED ---");
  #else
    // Initial calibration is required once after powering on the system // [cite: 7]
    Serial.println("--- System Standby ---");
    Serial.println("Step 1A: Press the calibration button before starting sorting."); // [cite: 8]
  #endif
}

void loop() {
  #ifdef MODO_TEST
    runCalibrationWizard(); // Test specific logic
  #else
    // --- Step 1: System Initialization & Calibration --- // [cite: 9]
    if (isCalibratePressed()) {
      Serial.println("Step 1B: Calibrating Linear actuator (Lifting arm with electromagnet)");
      calibrateActuator(limitSwitchPin); // [cite: 9]
      
      isCalibrated = true;
      Serial.println("Step 2: System Ready. Push the button to start."); 
      delay(200); // Cooldown delay after calibration // [cite: 10]
    }

    // --- Step 2: Sorting Process Initiation --- // [cite: 11]
    else if (isStartPressed()) {
      delay(200); // Cooldown delay // [cite: 11]
      if (!isCalibrated) {
        Serial.println("[ERROR] System not calibrated. Press the calibrate button."); // [cite: 12]
      } else {
        turnOffFerroLed();
        turnOffNonFerroLed(); // [cite: 13]
        executeSortingSequence(); // [cite: 13]
        delay(200); // Cooldown delay after cycle // [cite: 14]
        Serial.println(">> System ready for next batch.");
      }
    }
    
    // --- Step 7: Bin Level Monitoring --- // [cite: 15]
    else if (isCalibrated) {
      updateBinStatus(); 
      updateLedAlerts(isFerroFull, isNonFerroFull); // [cite: 15]
    }
  #endif
}

void executeSortingSequence() {
  Serial.println("\n--- SORTING CYCLE STARTED ---"); // [cite: 16]

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

  // --- Step 3B: Safety Gap and Electromagnet Activation --- // [cite: 17]
  Serial.println("Step 3B: Activating the ELECTROMAGNET..."); 
  unsigned long prepTimer = millis(); // [cite: 18]
  while (millis() - prepTimer < prepTime) { 
    checkPause();
    delay(10); // [cite: 19]
  }

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

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

  // --- Step 4B: Rotating arm to Ferromagnetic Bin... --- // [cite: 23]
  Serial.println("Step 4B: Rotating arm to Ferromagnetic Bin...");
  moveArmToBin();   
  checkPause(); // [cite: 24]

  // --- Step 4C: Deactivating Electromagnet and dropping scrap --- // [cite: 24]
  Serial.println("Step 4C: Deactivating the ELECTROMAGNET...");
  Serial.println("Step 4C: ELECTROMAGNET OFF (Dropping scrap)...");
  deactivateMagnet(); 

  unsigned long dropTimer = millis(); // [cite: 25]
  while (millis() - dropTimer < dropTime) {
    checkPause();
    delay(10); // [cite: 26]
  }

  // --- Step 5A: Opening main tray (Dumping to Bin 2) --- // [cite: 27]
  Serial.println("Step 5A: Opening main tray (Dumping to Bin 2)...");
  openTray();
  checkPause();

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

  // --- Step 6A: Returning arm to Home position --- // [cite: 29]
  Serial.println("Step 6A: Returning arm to Home position...");
  moveArmToHome();

  // --- Step 7: Scanning bin levels --- // [cite: 29]
  Serial.println("Step 7: Scanning bin levels...");
  Serial.println("--- SORTING CYCLE COMPLETE ---\n"); // [cite: 30]
}