#include <Arduino.h>
#include <Stepper.h>
#include <Servo.h>

// Define pins for stepper motors
#define STEPPER1_PIN1 2
#define STEPPER1_PIN2 3
#define STEPPER1_PIN3 4
#define STEPPER1_PIN4 5

#define STEPPER2_PIN1 6
#define STEPPER2_PIN2 7
#define STEPPER2_PIN3 8
#define STEPPER2_PIN4 9

#define STEPPER3_PIN1 10
#define STEPPER3_PIN2 11
#define STEPPER3_PIN3 12
#define STEPPER3_PIN4 13

// Define pin for servo motor
#define SERVO_PIN A0

// Define pin for momentary button
#define BUTTON_PIN A1

// Define pins for joystick
#define JOYSTICK_X A2
#define JOYSTICK_Y A3

// Define steps per revolution for stepper motors
#define STEPS_PER_REVOLUTION 2048

// Create stepper motor objects
Stepper stepper1(STEPS_PER_REVOLUTION, STEPPER1_PIN1, STEPPER1_PIN3, STEPPER1_PIN2, STEPPER1_PIN4);
Stepper stepper2(STEPS_PER_REVOLUTION, STEPPER2_PIN1, STEPPER2_PIN3, STEPPER2_PIN2, STEPPER2_PIN4);
Stepper stepper3(STEPS_PER_REVOLUTION, STEPPER3_PIN1, STEPPER3_PIN3, STEPPER3_PIN2, STEPPER3_PIN4);

// Create servo motor object
Servo clawServo;

// Variables to store joystick values
int joystickX, joystickY;

// Variable to store button state
bool buttonPressed = false;
bool clawOpen = false;

void setup() {
  // Set the speed of the stepper motors
  stepper1.setSpeed(10); // Adjust speed as needed
  stepper2.setSpeed(10);
  stepper3.setSpeed(10);

  // Attach the servo to its pin
  clawServo.attach(SERVO_PIN);

  // Set the initial position of the claw (closed)
  clawServo.write(0);

  // Set up the button pin as input with pull-up resistor
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  // Initialize serial communication for debugging
  Serial.begin(9600);
}

void loop() {
  // Read joystick values
  joystickX = analogRead(JOYSTICK_X);
  joystickY = analogRead(JOYSTICK_Y);

  // Control stepper motors based on joystick position
  if (joystickX < 400) {
    stepper1.step(1); // Pan left (now clockwise)
  } else if (joystickX > 600) {
    stepper1.step(-1); // Pan right (now counterclockwise)
  }

  if (joystickY < 400) {
    stepper2.step(-1); // Tilt up (first rod)
    stepper3.step(1); // Compensate for second rod
  } else if (joystickY > 600) {
    stepper2.step(1); // Tilt down (first rod)
    stepper3.step(-1); // Compensate for second rod
  }

  // Check button state
  if (digitalRead(BUTTON_PIN) == LOW && !buttonPressed) {
    buttonPressed = true;
    clawOpen = !clawOpen;
    
    if (clawOpen) {
      clawServo.write(45); // Open claw
    } else {
      clawServo.write(0); // Close claw
    }
    
    delay(50); // Debounce delay
  } else if (digitalRead(BUTTON_PIN) == HIGH) {
    buttonPressed = false;
  }

  // Add a small delay to avoid overwhelming the Arduino
  delay(10);
}