#include <Servo.h>

// Pin setup
const int metalSensorPin = 7;  // Metal detector input
const int servoPin = 9;
const int stepPin = 4;  // Step pin for A4988
const int dirPin = 3;   // Direction pin for A4988

Servo servo;

// Stepper motor configuration
const int STEPS_PER_REV = 200;     // Standard for many NEMA stepper motors
const int STEP_DELAY = 8000;       // 8ms between steps (slower speed)
                                  // This gives roughly 1.6 seconds per revolution
                                  // (200 steps * 0.008 seconds = 1.6 seconds per rev)

// Servo positions
const int SORT_POSITION = 90;
const int RESET_POSITION = 0;

// Metal detection timing
const int SORTING_TIME = 3000;    // Time for servo to stay in sorting position (5 seconds)

void setup() {
  servo.attach(servoPin);
  pinMode(metalSensorPin, INPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  servo.write(RESET_POSITION);
  Serial.begin(9600);
  
  // Set direction for continuous rotation
  digitalWrite(dirPin, LOW);
}

void step_conveyor() {
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(4000);      // Half of STEP_DELAY for HIGH state
  digitalWrite(stepPin, LOW);
  delayMicroseconds(4000);      // Half of STEP_DELAY for LOW state
}

void loop() {
  // Continuous conveyor movement
  step_conveyor();
  
  if (digitalRead(metalSensorPin) == HIGH) {
    Serial.println("Metal detected! Sorting...");
    
    // Move servo to sorting position
    servo.write(SORT_POSITION);
    
    // Keep belt running for 5 seconds while servo holds position
    unsigned long sortStartTime = millis();
    while (millis() - sortStartTime < SORTING_TIME) {
      step_conveyor();
    }
    
    // Return servo to initial position
    servo.write(RESET_POSITION);
    Serial.println("Sorting complete, arm reset.");
  }
}