#include <Wire.h>                      // Include ESP32 Wire library for I2C 
#include <Adafruit_PWMServoDriver.h>   // Include Adafruit library for PCA9685

// Define I2C pins for ESP32 (can be adjusted if needed)
#define SDA_PIN 21
#define SCL_PIN 22

// Instances for each PCA9685 board using the specified Wire object
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40, Wire); // Board 1 at address 0x40
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41, Wire); // Board 2 at address 0x41
Adafruit_PWMServoDriver pwm3 = Adafruit_PWMServoDriver(0x42, Wire); // Board 3 at address 0x42

// Define min and max pulse lengths for each servo type (mapping)
#define SERVOMIN_A  500 // Servo type A min pulse length (0 degrees)
#define SERVOMAX_A  2500 // Servo type A max pulse length (180 degrees)
#define SERVOMIN_B  500 // Servo type B min pulse length (0 degrees)
#define SERVOMAX_B  2400 // Servo type B max pulse length (180 degrees)
#define SERVOMIN_C  1000 // Servo type C min pulse length (0 degrees)
#define SERVOMAX_C  2000 // Servo type C max pulse length (180 degrees)
#define SERVOMIN_D  800 // Servo type D min pulse length (0 degrees)
#define SERVOMAX_D  2200 // Servo type D max pulse length (180 degrees)

#define SERVOS_PER_BOARD 16 // Number of servos per PCA9685 board
#define TOTAL_SERVOS 40     // Total number of servos

// Servo type mapping: adjustable arrays
int servoTypeA[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; // Servos of type A
int servoTypeB[] = {11, 12, 13, 14, 15, 16, 17, 18, 19, 20}; // Servos of type B
int servoTypeC[] = {21, 22, 23, 24, 25, 26, 27, 28, 29, 30}; // Servos of type C
int servoTypeD[] = {31, 32, 33, 34, 35, 36, 37, 38, 39, 40}; // Servos of type D

// Struct to hold servo state
struct ServoState {
  int angle;       // Current angle of the servo
  int targetAngle; // Target angle to move towards
  int delayTime;   // Delay before next movement
  unsigned long lastUpdate; // Timestamp of last update
  char type;       // Servo type ('A', 'B', 'C', 'D')
};

// Array to hold state for all servos
ServoState servos[TOTAL_SERVOS];

void setup() {
  Wire.begin(SDA_PIN, SCL_PIN); // Start I2C communication with defined SDA, SCL pins

  // Initialize each PCA9685 board
  pwm1.begin();
  pwm2.begin();
  pwm3.begin();

  // Set PWM frequency to 60 Hz for all boards (good for servos)
  pwm1.setPWMFreq(60);
  pwm1.setOscillatorFrequency(27000000);

  pwm2.setPWMFreq(60);
  pwm2.setOscillatorFrequency(27000000);

  pwm3.setPWMFreq(60);
  pwm3.setOscillatorFrequency(27000000);

  // Initialize servos with random values
  for (int i = 0; i < TOTAL_SERVOS; i++) {
    servos[i].angle = 0;
    servos[i].targetAngle = random(0, 180);
    servos[i].delayTime = random(100, 1000); // Random delay between movements
    servos[i].lastUpdate = millis();
    servos[i].type = getServoType(i + 1); // Determine type based on index
  }
}

void loop() {
  // Continuously control all servos independently
  updateServos(pwm1, 0);   // Control first 16 servos (Board 1)
  updateServos(pwm2, 16);  // Control next 16 servos (Board 2)
  updateServos(pwm3, 32);  // Control final 8 servos (Board 3)
}

// Function to update servos on a specific board
void updateServos(Adafruit_PWMServoDriver& pwm, int startServo) {
  for (int i = 0; i < SERVOS_PER_BOARD; i++) {
    int servoIndex = startServo + i;

    // Stop if servo index exceeds the total number of servos
    if (servoIndex >= TOTAL_SERVOS) {
      return;
    }

    // Get the current servo state
    ServoState& servo = servos[servoIndex];

    // Check if it's time to update the servo
    if (millis() - servo.lastUpdate >= servo.delayTime) {
      // Move the servo closer to the target angle
      if (servo.angle < servo.targetAngle) {
        servo.angle++;
      } else if (servo.angle > servo.targetAngle) {
        servo.angle--;
      } else {
        // If reached the target, set a new random target and delay
        servo.targetAngle = random(-100, 100);
        servo.delayTime = random(0, 10);
      }

      // Update the servo position
      pwm.setPWM(i, 0, degreesToPulse(servo.angle, servo.type));

      // Update the last update time
      servo.lastUpdate = millis();
    }
  }
}

// Function to convert degrees to pulse length based on servo type
int degreesToPulse(int degrees, char servoType) {
  switch (servoType) {
    case 'A':
      return map(degrees, 0, 180, SERVOMIN_A, SERVOMAX_A);
    case 'B':
      return map(degrees, 0, 180, SERVOMIN_B, SERVOMAX_B);
    case 'C':
      return map(degrees, 0, 180, SERVOMIN_C, SERVOMAX_C);
    case 'D':
      return map(degrees, 0, 180, SERVOMIN_D, SERVOMAX_D);
    default:
      return map(degrees, 0, 180, SERVOMIN_A, SERVOMAX_A); // Default to A if type not found
  }
}

// Function to determine servo type based on servo index
char getServoType(int servoIndex) {
  for (int i = 0; i < 10; i++) {
    if (servoIndex == servoTypeA[i]) return 'A';
    if (servoIndex == servoTypeB[i]) return 'B';
    if (servoIndex == servoTypeC[i]) return 'C';
    if (servoIndex == servoTypeD[i]) return 'D';
  }
  return 'A'; // Default to type A if servo not found in mapping
}