#include <ESP32Servo.h>

// Attach motors after writing 0 and keep it in the maximum position the link can go mechanically
// The limits are there in the Left Leg Assembly file's angle limit mates
// Also make sure the rotation is in the correct direction
Servo servo_1; // Hip
Servo servo_2; // Knee
Servo servo_3; // Ankle

void setup() {
  servo_1.attach(4);
  servo_2.attach(16);
  servo_3.attach(17);
  Serial.begin(9600);
}

void loop() {
  walk();
}

void walk() {
  int num_pnts = 10; 
  float angles[num_pnts][3] = {
    {57.68, 78.38, 84.3},
    {52.5674, 58.9042, 98.6632},
    {53.97444, 53.78144, 105.193},
    {56.71499, 51.67867, 110.0363},
    {60.17904, 51.58377, 113.5953},
    {64.09272, 53.1879, 115.9048},
    {68.33131, 56.47549, 116.8558},
    {72.91026, 61.74365, 116.1666},
    {78.12995, 70.08565, 113.0443},
    {88.1862, 94.4058, 98.7804}
  };

  Serial.println("STARTED STEP");
  for (int i = 0; i < num_pnts; i++) {
    servo_1.write(angles[i][0]);
    servo_2.write(angles[i][1]);
    servo_3.write(angles[i][2]);
    delay(200);
    Serial.print("MOVING TO PNT ");
    Serial.println(i);
  }
  Serial.println("FINISHED STEP");

  delay(1000);

  Serial.println("STARTED REVERSE STEP");
  for (int i = num_pnts-1; i >= 0; i--) {
    servo_1.write(angles[i][0]);
    servo_2.write(angles[i][1]);
    servo_3.write(angles[i][2]);
    delay(200);
    Serial.print("MOVING TO PNT ");
    Serial.println(i);
  }
  Serial.println("FINISHED REVERSE STEP");
}