#include <Wire.h>                // Required for I2C communication with the PCA9685
#include <Adafruit_PWMServoDriver.h> // Library for controlling the PCA9685 PWM driver

// Create an instance of the Adafruit_PWMServoDriver class
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); // Address of the PCA9685

// Define pulse width constants for the servos
#define SERVOMIN 125 // Minimum pulse length (in pulse-width units) for servo to be at 0 degrees
#define SERVOMAX 625 // Maximum pulse length (in pulse-width units) for servo to be at 180 degrees

// Create servo objects
#include <Servo.h>
Servo Servo1, Servo2, Servo3, Servo4, Servo5;

// Function to convert angle to pulse length
int angleToPulse(int angle) {
  return map(angle, 0, 180, SERVOMIN, SERVOMAX);
}

// Function to set the angle of all servos simultaneously
void set_allservo(int value) {
  // Set the same angle for all servos
  Servo1.write(value);
  Servo2.write(value);
  Servo3.write(value);
  Servo4.write(value);
  Servo5.write(value);
  Servo6.write(value);
  delay(2); // Small delay to ensure servos have time to move
}

// Function to set the angle of individual servos by special commands of each slider on the app
void set_servo(int values) {
  if (values >= 0 && values <= 180) {
    // Set the angle for the first servo by the slider number1 special commands
    int myservo1 = values;
    pwm.setPWM(0, 0, angleToPulse(myservo1)); // Set PWM for servo 1
    delay(2); // Small delay to ensure servo has time to move
  }
  else if (values >= 181 && values <= 361) {
    // Set the angle for the second servo  by the slider number2 special commands
    int myservo2 = values;
    myservo2 = map(myservo2, 181, 361, 0, 180); // Map range to 0-180 degrees
    pwm.setPWM(1, 0, angleToPulse(myservo2)); // Set PWM for servo 2
    delay(2);
  }
  else if (values >= 362 && values <= 542) {
    // Set the angle for the third servo by the slider number3 special commands
    int myservo3 = values;
    myservo3 = map(myservo3, 362, 542, 0, 180); // Map range to 0-180 degrees
    pwm.setPWM(2, 0, angleToPulse(myservo3)); // Set PWM for servo 3
    delay(2);
  }
  else if (values >= 543 && values <= 723) {
    // Set the angle for the fourth servo  by the slider number4 special commands
    int myservo4 = values;
    myservo4 = map(myservo4, 543, 723, 0, 180); // Map range to 0-180 degrees
    pwm.setPWM(3, 0, angleToPulse(myservo4)); // Set PWM for servo 4
    delay(2);
  }
  else if (values >= 724 && values <= 904) {
    // Set the angle for the fifth servo  by the slider number5 special commands
    int myservo5 = values;
    myservo5 = map(myservo5, 724, 904, 0, 180); // Map range to 0-180 degrees
    pwm.setPWM(4, 0, angleToPulse(myservo5)); // Set PWM for servo 5
    delay(2);
  }
  /*This section of the code allows you to use a specific range of values to set the same angle for all servos.
   This can be useful if you want to quickly synchronize all servos to the same position.*/
  else if (values >= 11000 && values <= 11180) {
    // Set the same angle for all servos based on a special range
    set_allservo(values - 11000); // Adjust the angle for all servos based on the input
  }
}

void setup() {
  Serial.begin(9600); // Initialize Serial communication at 9600 baud rate for debugging and Bluetooth
  pwm.begin(); // Initialize the PCA9685 PWM driver
  pwm.setPWMFreq(60); // Set the PWM frequency to 60 Hz (typical for servos)

  // Attach servo objects to PWM channels
  Servo1.attach(3);
  Servo2.attach(5);
  Servo3.attach(6);
  Servo4.attach(9);
  Servo5.attach(10);
  Servo6.attach(11);

  delay(5); // Small delay to ensure servos are initialized
  // Initialize servos to 0 degrees
  Servo1.write(0);
  Servo2.write(0);
  Servo3.write(0);
  Servo4.write(0);
  Servo5.write(0);
  Servo6.write(0);
  delay(5); // Small delay to ensure servos are in position
}

void loop() {
  if (Serial.available() > 1) {
    // Read two bytes from Serial input (Bluetooth)
    unsigned int temp_value1 = Serial.read();
    unsigned int temp_value2 = Serial.read();
    // Combine the bytes to form a single value
    set_servo((temp_value2 * 256) + temp_value1);
  }
}