#include <Servo.h>

// Define the pins for servo motors
#define servoPin1 9
#define servoPin2 10
#define servoPin3 11

// Define the pins for joystick 1
#define joy1X A0
#define joy1Y A1

// Define the pins for joystick 2
#define joy2X A2
#define joy2Y A3

// Define the pin for the BLDC motor signal
#define motorPin 8

// Create servo objects
Servo servo1;
Servo servo2;
Servo servo3;

// Define variables for storing joystick values
int joy1XValue, joy1YValue;
int joy2XValue, joy2YValue;

void setup() {
  // Initialize Serial communication for debugging
  Serial.begin(9600);

  // Attach servos to their pins
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);

  // Initialize motor pin as output
  pinMode(motorPin, OUTPUT);
}

void loop() {
  // Read the joystick values for joystick 1
  joy1XValue = analogRead(joy1X);
  joy1YValue = analogRead(joy1Y);

  // Map joystick values to servo angles for servo motors
  int servo1Angle = map(joy1XValue, 0, 1023, 0, 180);
  int servo2Angle = map(joy1YValue, 0, 1023, 0, 180);
  
  // Move the servo motors
  servo1.write(servo1Angle);
  servo2.write(servo2Angle);

  // Add a small delay to prevent jitter
  delay(20);

  // Read the joystick values for joystick 2
  joy2XValue = analogRead(joy2X);
  joy2YValue = analogRead(joy2Y);

  int servo3Angle = map(joy2XValue, 0, 1023, 180, 0); // Inverse control

  // Move the servo motors
  servo3.write(servo3Angle);

  // Map joystick values to motor speed for BLDC motor
  int motorSpeed = map(joy2YValue, 0, 1023, 0, 255);

  // Control the BLDC motor using PWM signal
  analogWrite(motorPin, motorSpeed);

  // Print joystick and motor values for debugging
  Serial.print("Joystick 1 X: ");
  Serial.print(joy1XValue);
  Serial.print(", 1 Y: ");
  Serial.print(joy1YValue);
  Serial.print(" | Joystick 2 X: ");
  Serial.print(joy2XValue);
  Serial.print(", 2 Y: ");
  Serial.print(joy2YValue);
  Serial.print(" | Motor Speed: ");
  Serial.println(motorSpeed);

  // Add a small delay
  delay(40);
}