#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

const int servo_pins[4][3] = {{1, 2, 0}, {4, 5, 3}, {8, 7, 9}, {11, 10, 12}};
const int SERVOMIN = 150;  // Minimum pulse width
const int SERVOMAX = 600;  // Maximum pulse width

// Define the delay between function calls (in milliseconds)
const unsigned long FUNCTION_DELAY = 2000; // 2 seconds

const unsigned long t = 2;
const unsigned long tt = 5;


void setup()
{

  Serial.begin(115200);

  // Initialize the PCA9685 library
  pwm.begin();
  pwm.setPWMFreq(50); // Set the PWM frequency of the PCA9685 (you may need to adjust this)

  // Initialize all servos
  for (int i = 0; i < 4; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      // Attach servo to PCA9685 channel
      pwm.setPWM(servo_pins[i][j],0,0); // Initialize servos at their center position
    }
  }
  center_servos();
  Serial.println("center");
  delay(2000);
}

void loop()
{
   //Move Forward 10 step 
  for (int i = 0; i < 10; i++) {
    moveLegServos_Forward();
  }
  center_servos();
  delay(FUNCTION_DELAY);
  
}


       // Move Spider Robot Forward Function
void moveLegServos_Forward()
{

  // Control the servo actions for each leg
  
  // Left side leg - Leg 1
  moveLeg_Left_Forward(servo_pins[0][0], servo_pins[0][2], servo_pins[0][0]);
  
  // Left side leg - Leg 4
  moveLeg_Left_Forward(servo_pins[3][0], servo_pins[3][2], servo_pins[3][0]);
  
  // Left side leg - Legs 1 and 4
  for (int angle = 0; angle <= 90; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(servo_pins[0][2], 0,pulse); // Initialize servos at the center position
    pwm.setPWM(servo_pins[3][2], 0,pulse); // Initialize servos at the center position
    delay(tt);
  }
  
  // Right side leg - Leg 2
  moveLeg_Right_Forward(servo_pins[1][0], servo_pins[1][2], servo_pins[1][0]);
  
  // Right side leg - Leg 3
  moveLeg_Right_Forward(servo_pins[2][0], servo_pins[2][2], servo_pins[2][0]);
  
  // Right side leg - Legs 2 and 3
  for (int angle = 180; angle >= 90; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(servo_pins[1][2], 0,pulse); // Initialize servos at the center position
    pwm.setPWM(servo_pins[2][2], 0,pulse); // Initialize servos at the center position
    delay(tt);
  }
}  
  
      // Move Spider Robot Backward Function
void moveLegServos_Backward()
{
   
   // Control the servo actions for each leg
  // Left side leg - Leg 1
  moveLeg_Left_Backward(servo_pins[3][0], servo_pins[3][2], servo_pins[3][0]);

  // Left side leg - Leg 4
  moveLeg_Left_Backward(servo_pins[0][0], servo_pins[0][2], servo_pins[0][0]);

  // Left side leg - Legs 1 and 4
  for (int angle = 180; angle >= 90; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(servo_pins[0][2], 0,pulse); // Initialize servos at the center position
    pwm.setPWM(servo_pins[3][2], 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Right side leg - Leg 2
  moveLeg_Right_Backward(servo_pins[2][0], servo_pins[2][2], servo_pins[2][0]);

  // Right side leg - Leg 3
  moveLeg_Right_Backward(servo_pins[1][0], servo_pins[1][2], servo_pins[1][0]);

  // Right side leg - Legs 2 and 3
  for (int angle = 0; angle <= 90; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(servo_pins[2][2], 0,pulse); // Initialize servos at the center position
    pwm.setPWM(servo_pins[1][2], 0,pulse); // Initialize servos at the center position
    delay(tt);
  }
}
  
  



          // Move Spider Robot Forward logic

void moveLeg_Left_Forward(int liftServo, int pivotServo , int oppositePivotServo)
{
  // Move the lift servo from 90 to 180
  for (int angle = 90; angle <= 135; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the pivot servo from 90 to 0
  for (int angle = 90; angle >= 45; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(pivotServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the lift servo from 180 to 90
  for (int angle = 135; angle >= 90; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }
}
void moveLeg_Right_Forward(int liftServo, int pivotServo , int oppositePivotServo)
{
  // Move the lift servo from 90 to 180
  for (int angle = 90; angle <= 135; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the pivot servo from 90 to 180
  for (int angle = 90; angle <= 135; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(pivotServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the lift servo from 180 to 90
  for (int angle = 135; angle >= 90; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }
}
        // Move Spider  Robot Backward logic
void moveLeg_Left_Backward(int liftServo, int pivotServo, int oppositePivotServo)
{
  // Move the lift servo from 90 to 180
  for (int angle = 90; angle <= 135; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the pivot servo from 90 to 180
  for (int angle = 90; angle <= 135; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(pivotServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the lift servo from 180 to 90
  for (int angle = 135; angle >= 90; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }
}

void moveLeg_Right_Backward(int liftServo, int pivotServo, int oppositePivotServo)
{
  // Move the lift servo from 90 to 180
  for (int angle = 90; angle <= 135; angle += 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the pivot servo from 90 to 0
  for (int angle = 90; angle >= 45; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(pivotServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }

  // Move the lift servo from 180 to 90
  for (int angle = 135; angle >= 90; angle -= 2) {
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
    pwm.setPWM(liftServo, 0,pulse); // Initialize servos at the center position
    delay(tt);
  }
}
    

      // All Servos Centor function  
void center_servos()
{
  for (int i = 0; i < 4; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      int pulse = map(90, 0, 180, SERVOMIN, SERVOMAX); // Map degrees to pulse length
      pwm.setPWM(servo_pins[i][j], 0,pulse); // Initialize servos at the center position
      delay(20);
    }
  }
}