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

// Define servo objects for all 8 servos (4 legs with 2 servos each)
Servo leg1_lower, leg1_upper, leg2_lower, leg2_upper, leg3_lower, leg3_upper, leg4_lower, leg4_upper;

// Leg dimensions (in millimeters)
float l1 =100; // Length of lower leg
float l2 = 100; // Length of upper leg

// Define leg positions (in millimeters)
float leg1_x, leg1_y;
float leg2_x, leg2_y;
float leg3_x, leg3_y;
float leg4_x, leg4_y;

// Gait parameters
int numSteps = 4; // Number of steps in a complete gait cycle
int stepDuration = 1000; // Duration of each step in milliseconds

void setup() {
  // Attach the servos to their respective pins for all legs
  leg1_lower.attach(9);
  leg1_upper.attach(10);
  leg2_lower.attach(5);
  leg2_upper.attach(6);
  leg3_lower.attach(8);
  leg3_upper.attach(11);
  leg4_lower.attach(2);
  leg4_upper.attach(3);

  // Initialize leg positions for all legs
  leg1_x = 0;
  leg1_y = 0;
  leg2_x = 0;
  leg2_y = 0;
  leg3_x = 0;
  leg3_y = 0;
  leg4_x = 0;
  leg4_y = 0;
}

void loop() {
  // Perform a complete gait cycle for all legs
  for (int i = 0; i < numSteps; i++) {
    // Move each leg forward
    walkStep(leg1_x, leg1_y, 100, 100, leg1_lower, leg1_upper);
    walkStep(leg2_x, leg2_y, 100, 100, leg2_lower, leg2_upper);
    walkStep(leg3_x, leg3_y, 100, 100, leg3_lower, leg3_upper);
    walkStep(leg4_x, leg4_y, 100, 100, leg4_lower, leg4_upper);

    // Delay to control step duration
    delay(stepDuration);
  }
}

void walkStep(float startX, float startY, float endX, float endY, Servo lower, Servo upper) {
  int numPoints = 50; // Number of points in the trajectory

  for (int i = 0; i <= numPoints; i++) {
    // Interpolate leg position
    float x = map(i, 0, numPoints, startX, endX);
    float y = map(i, 0, numPoints, startY, endY);

    // Calculate IK for leg
    inverseKinematics(x, y, lower, upper);
    
    // Delay to control step speed
    delay(stepDuration / numPoints);
  }
}

void inverseKinematics(float x, float y, Servo lower, Servo upper) {
  // Calculate the required angles using trigonometry
  float D = sqrt(x * x + y * y);
  float theta2 = acos((l1 * l1 + l2 * l2 - D * D) / (2 * l1 * l2));
  float theta1 = atan2(y, x) - atan2((l2 * sin(theta2)), (l1 + l2 * cos(theta2)));
  
  // Convert radians to degrees and set servo angles
  int angle1 = 90+degrees(theta1); //lower servo angle
  int angle2 = degrees(theta2);
    Serial.begin(9600);
  Serial.print("theta1: ");
  Serial.println(angle1);
  Serial.print("theta2: ");
  Serial.println(angle2);
  
  lower.write(angle1);
  upper.write(angle2);
}