// Magnetic Robot Arm V1
#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;

const int joystickX = A0;
const int joystickY = A1;
const int joystickButton = 8;
const int magnetPin = 13;

bool magnetState = false;
bool lastButtonState = HIGH;

// Variables to store current angles of the servos
int angle1 = 90; // Initial angle for servo1
int angle2 = 90 - 60; // Initial angle for servo2
int angle3 = 90 - 60; // Initial angle for servo3

// Increment/decrement amount for servo angles
int angleStep = 4; // Increase this for quicker movement

void setup() { 
  servo1.attach(9);
  servo2.attach(10);
  servo3.attach(11);
  pinMode(joystickButton, INPUT_PULLUP);
  pinMode(magnetPin, OUTPUT);
  Serial.begin(9600);
}

void loop() { 
  // Constantly read analog values of joystick
  int xVal = analogRead(joystickX);
  int yVal = analogRead(joystickY);

  // Adjust angle1 based on joystickY position
  if (yVal > 550) { // LEFT
    angle1 = max(angle1 - angleStep, 0);
  } else if (yVal < 470) { // RIGHT
    angle1 = min(angle1 + angleStep, 180);
  }

  // Adjust angle2 and angle3 based on joystickX position
  if (xVal > 550) { // UP/FORWARD
    angle2 = max(angle2 - angleStep, 0);
    angle3 = max(angle3 - angleStep, 0);
  } else if (xVal < 470) { // DOWNWARD
    angle2 = min(angle2 + angleStep, 180);
    angle3 = min(angle3 + angleStep, 180);
  }

  // Apply the updated angles to the servos
  servo1.write(angle1);
  servo2.write(angle2);
  servo3.write(angle3);

  // Toggle magnet state based on button press
  bool currentButtonState = digitalRead(joystickButton);
  if (currentButtonState == LOW && lastButtonState == HIGH) {
    magnetState = !magnetState;
    digitalWrite(magnetPin, magnetState ? HIGH : LOW);
  }
  lastButtonState = currentButtonState;

  // Serial output for debugging
  Serial.print("Angle1: ");
  Serial.print(angle1);
  Serial.print(" | Angle2: ");
  Serial.print(angle2);
  Serial.print(" | Angle3: ");
  Serial.println(angle3);

  delay(25); // Delay to give the servos time to react
}