#include <Servo.h>
#include <Adafruit_LiquidCrystal.h>
Servo servo1; // First servo
Servo servo2; // Second servo
Servo servo3; // Third servo
Servo servo4; // Hey, why am I doing this!? I'm a genius, I don't NEED labels!
Servo servo5;
Servo servo6;

void setup() {
  servo1.attach(2); // Attach the first servo to digital pin 9
  servo2.attach(3); // Attach the second servo to digital pin 10
  servo3.attach(4); // You get it. At least, you better.
  servo4.attach(5);
  servo5.attach(6);
  servo6.attach(7);
  Serial.begin(9600); // Initialize serial communication for debugging
}

void loop() {
  int joystick1VValue = analogRead(A0); // Read the joystick's vertical value
  int joystick1HValue = analogRead(A1); // And horizontal, of course
  int joystick1trigger = digitalRead(12);
  int joystick2VValue = analogRead(A3);
  int joystick2HValue = analogRead(A4);
  int joystick2trigger = digitalRead(13);

  // Map the joystick value (0-1023) to the first servo angle (0-180)
  int servoAngle1 = map(joystick1VValue, 0, 1023, 0, 180);
  int servoAngle4 = map(joystick2VValue, 0, 1023, 0, 180);

  // Calculate the angle for the second servo in the opposite direction
  int servoAngle2 = 180 - servoAngle1;
  int servoAngle5 = 180 - servoAngle4;

  // Calculate the angle for the third servo from the joystick's HORZ value
  int servoAngle3 = map(joystick1HValue, 0, 1023, 0, 180);
  int servoAngle6 = map(joystick2HValue, 0, 1023, 0, 180);
  // Move all servos to their respective angles
  servo1.write(servoAngle1);
  servo2.write(servoAngle2);
  servo3.write(servoAngle3);
  servo4.write(servoAngle4);
  servo5.write(servoAngle5);
  servo6.write(servoAngle6);


  // For debugging, print the joystick and servo values to the serial monitor
  // Serial.print("Joystick Vertical Value: ");
  //Serial.print(joystickVValue);
  //Serial.print("   Servo 1 Angle: ");
  //Serial.print(servoAngle1);
  //Serial.print("   Servo 2 Angle: ");
  //Serial.println(servoAngle2);

  delay(50); // Add a small delay to avoid rapid servo movements
}