#include <Servo.h>

// Renamed servos for clarity: 001 = pin9, 002 = pin10, 003 = pin11
Servo servo001;  // Servo 1 on pin 9
Servo servo002;  // Servo 2 on pin 10
Servo servo003;  // Servo 3 on pin 11

// Variables for startup sequence
int startupDelay = 500;  // Delay between movements (ms)

// Button variables
const int buttonPin = 7;      // Button on pin 7
int lastButtonState = HIGH;    // Previous button state (HIGH with pullup)
unsigned long lastDebounceTime = 0;  // Last time button state changed
unsigned long debounceDelay = 50;     // Debounce time in ms
bool buttonPressed = false;    // Flag for button press

void setup() {
  // Attach servos to correct pins
  servo001.attach(9);   // Servo 1 on pin 9
  servo002.attach(10);  // Servo 2 on pin 10
  servo003.attach(11);  // Servo 3 on pin 11
  
  // Setup button with internal pullup resistor
  pinMode(buttonPin, INPUT_PULLUP);
  
  Serial.begin(9600);
  Serial.println("Starting up...");
  Serial.println("Running servo calibration sequence");
  Serial.println("-------------------");
  
  // === STARTUP SEQUENCE ===
  // Step 1: Move all servos to 0° (one by one)
  Serial.println("Step 1: Moving to 0°");
  servo001.write(0);
  delay(startupDelay);
  servo002.write(0);
  delay(startupDelay);
  servo003.write(0);
  delay(startupDelay);
  
  // Step 2: Move all servos to 180° (one by one)
  Serial.println("Step 2: Moving to 180°");
  servo001.write(180);
  delay(startupDelay);
  servo002.write(180);
  delay(startupDelay);
  servo003.write(180);
  delay(startupDelay);
  
  // Step 3: Sweep all servos together for visual check
  Serial.println("Step 3: Synchronized sweep");
  for (int pos = 180; pos >= 0; pos -= 10) {
    servo001.write(pos);
    servo002.write(pos);
    servo003.write(pos);
    delay(100);
  }
  
  for (int pos = 0; pos <= 180; pos += 10) {
    servo001.write(pos);
    servo002.write(pos);
    servo003.write(pos);
    delay(100);
  }
  
  // === HOMING SEQUENCE ===
  // Return all servos to center (90°)
  Serial.println("Step 4: Homing to 90° (center)");
  servo001.write(90);
  delay(300);
  servo002.write(90);
  delay(300);
  servo003.write(90);
  delay(500);
  
  // Final confirmation
  Serial.println("-------------------");
  Serial.println("Calibration complete!");
  Serial.println("All servos homed to 90°");
  Serial.println("Ready for commands!");
  Serial.println("");
  Serial.println("Send commands as: [servo number] [angle]");
  Serial.println("Examples:  1 180  - moves Servo 1 (pin 9) to 180°");
  Serial.println("          2 45   - moves Servo 2 (pin 10) to 45°");
  Serial.println("          3 0    - moves Servo 3 (pin 11) to 0°");
  Serial.println("");
  Serial.println("Button on pin 7: Press to home all servos to 90°");
  Serial.println("-------------------");
}

void loop() {
  // === DEBOUNCED BUTTON CHECK ===
  checkButton();
  
  // If button was pressed, perform action
  if (buttonPressed) {
    buttonPressed = false;  // Reset flag
    homeAllServos();        // Home all servos to 90°
  }
  
  // === SERIAL COMMAND CHECK ===
  // Check if there's data waiting
  if (Serial.available() > 0) {
    
    // DECLARE variables INSIDE the loop function
    int servoNum = Serial.parseInt();
    int angle = Serial.parseInt();
    
    // Check if we received valid numbers
    if (servoNum >= 1 && servoNum <= 3) {
      
      // Make sure angle is within valid range (0-180)
      angle = constrain(angle, 0, 180);
      
      // Move the appropriate servo
      switch(servoNum) {
        case 1:
          servo001.write(angle);
          Serial.print("Servo 1 (pin 9) -> ");
          Serial.print(angle);
          Serial.println("°");
          break;
          
        case 2:
          servo002.write(angle);
          Serial.print("Servo 2 (pin 10) -> ");
          Serial.print(angle);
          Serial.println("°");
          break;
          
        case 3:
          servo003.write(angle);
          Serial.print("Servo 3 (pin 11) -> ");
          Serial.print(angle);
          Serial.println("°");
          break;
      }
    } else {
      Serial.println("Invalid servo! Use 1, 2, or 3");
    }
    
    // Clear any remaining serial data
    while (Serial.available() > 0) {
      Serial.read();
    }
    
    // Small delay to prevent overwhelming the servos
    delay(15);
  }
}

// === FUNCTION: Debounced button check ===
void checkButton() {
  int reading = digitalRead(buttonPin);  // Read button state (LOW when pressed)
  
  // If button state changed, reset debounce timer
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }
  
  // If stable for debounce delay, consider it valid
  if ((millis() - lastDebounceTime) > debounceDelay) {
    // Check if button was pressed (LOW with pullup)
    if (reading == LOW) {
      buttonPressed = true;  // Set flag for main loop
      Serial.println("Button pressed - Homing all servos!");
    }
  }
  
  lastButtonState = reading;  // Save for next loop
}

// === FUNCTION: Home all servos to 90° ===
void homeAllServos() {
  Serial.println("Homing all servos to 90°...");
  
  servo001.write(90);
  delay(300);
  servo002.write(90);
  delay(300);
  servo003.write(90);
  delay(300);
  
  Serial.println("All servos homed to 90°");
}