#include <ESP32Servo.h>
#include <WiFi.h>
#include <cmath>
#include <tuple>
#include "ESPAsyncWebServer.h"

Servo leftServo;

Servo rightServo;

const uint8_t LEFT_SERVO_PIN = 18;

const uint8_t RIGHT_SERVO_PIN = 19;

const char* ssid = "Tufts-Robot";
const char* password = ""; 

AsyncWebServer server(80);

const uint8_t armLength = 100;
const uint8_t endEffectorOffset = 20;
const uint8_t servoDistance = 40;

const uint8_t L1 = armLength;
const uint8_t L2 = armLength;
const uint8_t L3 = armLength;
const uint8_t L4 = armLength;
const uint8_t L5 = servoDistance;
const uint8_t L6 = endEffectorOffset;

const double RADIAN_TO_DEGREE_MULTIPLIER = 180/M_PI;

std::tuple<double, double> getAnglesFromEndEffectorPoint(double x, double y) {
  // double a = 2 * x * armLength;
  // double b = 2 * y * armLength;
  double a = 2 * x * L1;
  double b = 2 * y * L1;
  // double c = pow(x, 2) + pow(y, 2) + pow(armLength, 2) - pow(armLength + endEffectorOffset, 2);
  // double alpha1 = 2 * atan((b + sqrt(pow(a, 2) + pow(b, 2) - pow(c, 2)))/(a + c));
  double c = pow(x, 2) + pow(y, 2) + pow(L1, 2) - pow(L2 + L6, 2);
  double alpha1 = 2 * atan((b + sqrt(pow(a, 2) + pow(b, 2) - pow(c, 2)))/(a + c));
  // double Ax = armLength * cos(alpha1);
  // double Ay = armLength * sin(alpha1);
  double Ax = L1 * cos(alpha1);
  double Ay = L1 * sin(alpha1);
  // double cosTheta = (x - Ax)/(armLength + endEffectorOffset);
  // double sinTheta = (y - Ay)/(armLength + endEffectorOffset);
  double cosTheta = (x - Ax)/(L2 + L6);
  double sinTheta = (y - Ay)/(L2 + L6);
  // double xb = x - endEffectorOffset * cosTheta;
  // double yb = y - endEffectorOffset * sinTheta;
  double xb = x - L6 * cosTheta;
  double yb = y - L6 * sinTheta;
  // double d = 2 * armLength * (xb - servoDistance);
  // double e = 2 * armLength * yb;
  double d = 2 * L4 * (xb - L5);
  double e = 2 * L4 * yb;
  // double f = pow(xb - servoDistance, 2) + pow(armLength, 2) + pow(yb, 2) - pow(armLength, 2);
  double f = pow(xb - L5, 2) + pow(L4, 2) + pow(yb, 2) - pow(L3, 2);
  // 2*atan((e(2)+sqrt((d(2))^2+(e(2))^2-(f(2))^2))/(d(2)+f(2)))
  double alpha2 = 2 * atan((e - sqrt(pow(d, 2) + pow(e, 2) - pow(f, 2)))/(d + f));
  double alpha2prime = 2 * atan((e + sqrt(pow(d, 2) + pow(e, 2) - pow(f, 2)))/(d + f));
  Serial.printf("alpha2 with addition: %f\n", alpha2prime * (180/M_PI));
  // Serial.printf("Values:\n\tx: %f\n\ty: %f\n\ta: %f\n\tb: %f\n\tc: %f\n\talpha1: %f\n\tAx: %f\n\tAy: %f\n\tsinTheta: %f\n\tcosTheta: %f\n\txb: %f\n\tyb: %f\n\td: %f\n\te: %f\n\tf: %f\n\talpha2: %f\n",
  //   x, y, a, b, c, alpha1, Ax, Ay, sinTheta, cosTheta, xb, yb, d, e, f, alpha2
  // );
  return std::make_tuple(alpha1 * RADIAN_TO_DEGREE_MULTIPLIER, alpha2 * RADIAN_TO_DEGREE_MULTIPLIER);
}

void setup() {
  Serial.begin(115200);
  // Allow allocation of all timers for servo library
  // ESP32PWM::allocateTimer(0);
  // ESP32PWM::allocateTimer(1);
  // ESP32PWM::allocateTimer(2);
  // ESP32PWM::allocateTimer(3);
  Serial.println("Initializing...");
  leftServo.attach(LEFT_SERVO_PIN);
  rightServo.attach(RIGHT_SERVO_PIN);
  // leftServo.write(90);
  // rightServo.write(90);

  Serial.printf("Connecting to WiFi AP %s\n", ssid);
  WiFi.softAP(ssid, password);
  IPAddress IP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(IP); // displays the IP address for the ESP32, by default it is 192.168.4.1
  // you can confirm the IP by plugging in the ESP32, opening the serial monitor then resetting the ESP32, the above lines print the IP address
  server.begin();
}

void loop() {
  // leftServo.write(-45);
  // rightServo.write(45);
  // delay(500);
  // leftServo.write(90);
  // rightServo.write(90);
  // delay(500);
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');

    if ((input.toLowerCase(), input) == "calibrate") {
      leftServo.write(90);
      rightServo.write(90);
      delay(200); //wait for servos to arrive
    }
    else if ((input.toLowerCase(), input) == "45") {
      leftServo.write(45);
      rightServo.write(45);
      delay(200); //wait for servos to arrive
    }
    else if (input.startsWith("r=")) {
      String rightServoAngleString = input.substring(2, input.length());
      int rightServoAngle = std::stod(rightServoAngleString.c_str());
      Serial.printf("Setting right servo angle to %d\n", rightServoAngle);
      rightServo.write(rightServoAngle);
    }
    else if (input.startsWith("l=")) {
      String leftServoAngleString = input.substring(2, input.length());
      int leftServoAngle = std::stod(leftServoAngleString.c_str());
      Serial.printf("Setting left servo angle to %d\n", leftServoAngle);
      leftServo.write(leftServoAngle);
    }
    else if (input.startsWith("p=")) {
      String pair = input.substring(2, input.length());
      int commaIndex = pair.indexOf(',');
      if (commaIndex >= 0) {
        double x = std::stod(pair.substring(0, commaIndex).c_str());
        double y = std::stod(pair.substring(commaIndex + 1, pair.length()).c_str());
        Serial.printf("Getting alpha1, alpha2 from point (%f, %f)\n", x, y);
        auto [leftServoAngle, rightServoAngle] = getAnglesFromEndEffectorPoint(x, y);
        Serial.printf("leftServoAngle: %f\nrightServoAngle: %f\n", leftServoAngle, rightServoAngle);
        leftServo.write(leftServoAngle);
        rightServo.write(rightServoAngle);
      }
    }
    else if (input == "arc") {
      for (double x = -20; x < 75; x += 1) {
        for (double y = 110; y < 205; y += 1) {
          auto [leftServoAngle, rightServoAngle] = getAnglesFromEndEffectorPoint(x, y);
          leftServo.write(leftServoAngle);
          rightServo.write(rightServoAngle);
          delay(100);
        }
      }
    }
  }
}