#include <ESP32Servo.h>
Servo servo1;
Servo servo2;


// Utilities
#define SETINTERVAL(block, interval_ms) do { \
    static unsigned long last_ms_##__LINE__ = 0; \
    if (millis() - last_ms_##__LINE__ >= interval_ms) { \
      last_ms_##__LINE__ += interval_ms; \
      do { block; } while (0); \
    } \
  } while (0)

#define PI 3.141592654
#define RAD(deg)  (deg*PI/180.0)
#define DEG(rad)  (rad*180.0/PI)

#define PIN_SERVO1  4
#define PIN_SERVO2  16

const float L1 = 1.0;
const float L2 = 1.0;

int pos1;
int pos2;

void move(double shoulder, double elbow) {
  Serial.printf("Shoulder %3.1f, Elbow %3.1f\n", shoulder, elbow);
  servo1.write(shoulder);
  servo2.write(elbow);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  servo1.attach(PIN_SERVO1);
  servo2.attach(PIN_SERVO2);

  // Self-test sequence
  move(0, 0);
  delay(3000);

  move(0, 90);
  delay(3000);

  move(0, 180);
  delay(3000);

  move(90, 180);
  delay(3000);

  move(180, 180);
  delay(3000);

  move(0, 0);
  delay(3000);
}

void loop() {
  static float y = 0.0;
  static float x = 0.0;
  static int dir = 0;
  const float vel = 0.002;
  
  // draw a rectangle
  switch(dir) {
    case 0: x += vel; if (x >= 0.0) ++dir; break;
    case 1: y += vel; if (y >= 1.5) ++dir; break;
    case 2: x -= vel; if (x <= -1.0) ++dir; break;
    default: y -= vel; if (y <= .5) dir = 0; break;
  }

  // or draw a circle ?
  float cx = -0.5, cy = 1.0, r = 0.5;
  static float alpha = 0.0;
  alpha += 2*PI / 1000;
  x = cx + r * cos(alpha); 
  y = cy + r * sin(alpha);

  Serial.print(dir); Serial.print(", "); Serial.print(x); Serial.print(", "); Serial.println(y);

  float t1, t2;
  inverse_kinematics(x, y, &t1, &t2);

  move(DEG(t1), DEG(t2));

  delay(10);
}


int is_reachable(float x, float y) {
  float distance = sqrt(x * x + y * y);
  return (distance <= (L1 + L2)) && (distance >= fabs(L1 - L2));
}

// Function to compute theta2
int inverse_kinematics(float x, float y, float *t1, float *t2) {
  if (!is_reachable(x, y)) {
    return false;
  }
  float theta2 = acos((x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2));

  float k1 = L1 + L2 * cos(theta2);
  float k2 = L2 * sin(theta2);
  float theta1 = atan2(y, x) - atan2(k2, k1);
  *t1 = theta1;
  *t2 = theta2;
  return true;
}