#include <Servo.h>
#include <math.h>

// Define servo objects
Servo hipServo1, kneeServo1, ankleServo1;
Servo hipServo2, kneeServo2, ankleServo2;
Servo hipServo3, kneeServo3, ankleServo3;
Servo hipServo4, kneeServo4, ankleServo4;

// Constants for leg dimensions (lengths in millimeters)
const float femurLength = 50.0;
const float tibiaLength = 70.0;
const float coxaLength = 30.0;

// Variables for joint angles
float hipAngle, kneeAngle, ankleAngle;

// Time variables for sinusoidal function
unsigned long startTime;
const float stepDuration = 1000.0;  // Time for one step in milliseconds

void setup() {
  // Attach servos to respective pins
  // int servoPins[numServos] = {42, 13, 7, 45, 46, 47, 48, 25,50, 51, 33, 53}; // Replace with your actual pin numbers

  hipServo1.attach(11);
  kneeServo1.attach(12);
  ankleServo1.attach(13);

  hipServo2.attach(8);
  kneeServo2.attach(9);
  ankleServo2.attach(10);

  hipServo3.attach(7);
  kneeServo3.attach(6);
  ankleServo3.attach(5);

  hipServo4.attach(51);
  kneeServo4.attach(53);
  ankleServo4.attach(52);

  hipServo1.write(90);
  kneeServo1.write(90);
  ankleServo1.write(90);

  hipServo2.write(90);
  kneeServo2.write(90);
  ankleServo2.write(90);

  hipServo3.write(90);
  kneeServo3.write(90);
  ankleServo3.write(90);

  hipServo4.write(90);
  kneeServo4.write(90);
  ankleServo4.write(90);
delay(2000);
  // Set initial leg positions
  //resetLegs();
  startTime = millis();
}

void loop() {
  // Calculate the elapsed time since the last step
  unsigned long elapsedTime = millis() - startTime;

  // Move all legs forward in a sinusoidal pattern
  moveLeg(hipServo1, kneeServo1, ankleServo1, sinMovement(elapsedTime));
  moveLeg(hipServo2, kneeServo2, ankleServo2, sinMovement(elapsedTime));
  moveLeg(hipServo3, kneeServo3, ankleServo3, sinMovement(elapsedTime));
  moveLeg(hipServo4, kneeServo4, ankleServo4, sinMovement(elapsedTime));
    //If one step is completed, reset the start time
  if (elapsedTime > stepDuration) {
    startTime = millis();
  }


}

void moveLeg(Servo hip, Servo knee, Servo ankle, float hipOffset) {
  // Calculate servo angles using inverse kinematics with sinusoidal function
  inverseKinematics(hipOffset, -45, 0);  // targetHip, targetKnee, targetAnkle

  // Move servos to the calculated angles
  hip.write(hipAngle);
  knee.write(kneeAngle);
  ankle.write(ankleAngle);
  delay(250);
}

// }
float sinMovement(unsigned long elapsedTime) {
  // Use a sinusoidal function to generate hip joint movement
  float period = 2 * PI * elapsedTime / stepDuration;
  return 30 * sin(period);  // Adjust amplitude and frequency for desired motion
}

void inverseKinematics(float targetHip, float targetKnee, float targetAnkle) {
  // Implement inverse kinematics equations here
  // This can vary depending on your robot's leg structure
  // For a simple example, you can use trigonometric functions

  // Calculate joint angles using trigonometry
  hipAngle = atan2(-femurLength * sin(targetHip), femurLength * cos(targetHip) - coxaLength) * RAD_TO_DEG;
  kneeAngle = atan2(femurLength * sin(targetHip), femurLength * cos(targetHip) - coxaLength - tibiaLength) * RAD_TO_DEG;
  //ankleAngle = targetAnkle;  // Assuming direct control of ankle angle
  ankleAngle = atan2(femurLength * sin(targetHip), femurLength * cos(targetHip) - coxaLength - tibiaLength) * RAD_TO_DEG;}

void resetLegs() {
  // Reset all legs to their initial positions
  moveLeg(hipServo1, kneeServo1, ankleServo1, 120);
  moveLeg(hipServo2, kneeServo2, ankleServo2, 120);
  moveLeg(hipServo3, kneeServo3, ankleServo3, 120);
  moveLeg(hipServo4, kneeServo4, ankleServo4, 120);
  delay(2000);  // Delay for stability
}