#include <Servo.h>

// ROBOT DIMENSIONS
double dim_a = 6.0;   // d1
double dim_b = 1.7;   // a1
double dim_c = 8.0;   // a2
double dim_d = 15.5;  // a3

extern "C" {
  void roboInvKinV2(double x, double y, double z, double a, double b, double c, double d, double *theta_1_deg, double *theta_2_deg, double *theta_3_deg);
}

double q1, q2, q3;
int pos_1, pos_2, pos_3;

// PICK AND PLACE POSITIONS
double pos_pick[3] = {11, 11, 1};
double pos_place[3] = {-11, 11, 1};

Servo servo1; // Joint 1
Servo servo2; // Joint 2
Servo servo3; // Joint 2 Mirror
Servo servo4; // Joint 3
Servo servo5; // Grabber

void setup() {
  servo1.attach(10);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);
  servo5.attach(11);

  Serial.begin(9600);

  initJointPositions();

  delay(2000);
  pickObject();
  delay(2000);
  placeObject();
}

void loop() {
  // int angle1, angle2, angle3;
  // readSerialAngles(angle1, angle2, angle3);
  // correctAngles(angle1, angle2, angle3);

  // servo1.write(angle1);
  // servo2.write(angle2);
  // servo3.write(angle3);
}

void pickObject() {
  calculatePositions(pos_pick[0], pos_pick[1], pos_pick[2]);
  delay(100);
  setJointPosition_1(20);
  delay(500);
  setJointPosition_2(20);
  delay(500);
  grabber_open();
  setJointPosition_3(20);
  grabber_close();
  pos_2 += 30;
  setJointPosition_2(20);
}

void placeObject() {
  calculatePositions(pos_place[0], pos_place[1], pos_place[2]);
  delay(100);
  setJointPosition_1(20);
  delay(500);
  setJointPosition_2(20);
  delay(500);
  setJointPosition_3(20);
  grabber_open();
  pos_2 += 30;
  setJointPosition_2(20);
  grabber_close();
}

void calculatePositions(double x, double y, double z) {
  roboInvKinV2(x, y, z, dim_a, dim_b, dim_c, dim_d, &q1, &q2, &q3);

  Serial.print("Joint Angles: ");
  Serial.print(q1);
  Serial.print(", ");
  Serial.print(q2);
  Serial.print(", ");
  Serial.println(q3);

  correctAngles();

  Serial.print("Servo Position: ");
  Serial.print(pos_1);
  Serial.print(", ");
  Serial.print(pos_2);
  Serial.print(", ");
  Serial.println(pos_3);
}

void correctAngles() {
  // q1 += 90;
  q2 = 180 - q2;
  q3 = -q3;

  pos_1 = constrain(static_cast<int>(q1), 0, 180);
  pos_2 = constrain(static_cast<int>(q2), 0, 180);
  pos_3 = constrain(static_cast<int>(q3), 0, 180);
}

void initJointPositions() {
  servo1.write(90);
  servo2.write(50);
  servo3.write(130);
  servo4.write(45);
  grabber_close();
}

void setJointPosition_1(int delayTime) {
  int currentAngle = servo1.read();
  
  if (currentAngle < pos_1) {
    for (int i = currentAngle; i <= pos_1; i++) {
      servo1.write(i);
      delay(delayTime);
    }
  } else {
    for (int i = currentAngle; i >= pos_1; i--) {
      servo1.write(i);
      delay(delayTime);
    }
  }
}

void setJointPosition_2(int delayTime) {
  int currentAngle = servo2.read();
  
  if (currentAngle < pos_2) {
    for (int i = currentAngle; i <= pos_2; i++) {
      servo2.write(i);
      servo3.write(180 - i);
      delay(delayTime);
    }
  } else {
    for (int i = currentAngle; i >= pos_2; i--) {
      servo2.write(i);
      servo3.write(180 - i);
      delay(delayTime);
    }
  }
}

void setJointPosition_3(int delayTime) {
  int currentAngle = servo3.read();
  
  if (currentAngle < pos_3) {
    for (int i = currentAngle; i <= pos_3; i++) {
      servo4.write(i);
      delay(delayTime);
    }
  } else {
    for (int i = currentAngle; i >= pos_3; i--) {
      servo4.write(i);
      delay(delayTime);
    }
  }
}

void grabber_close() {
  servo5.write(112);
  delay(500);

  servo5.write(95);
  delay(500);
}

void grabber_open() {
  servo5.write(90);
  delay(500);

  servo5.write(95);
  delay(500);
}


// void readSerialAngles(int &angle1, int &angle2, int &angle3) {
//   if (Serial.available() > 0) {
//     String input = Serial.readStringUntil('\n');

//     int parsedValues = sscanf(input.c_str(), "%d,%d,%d", &angle1, &angle2, &angle3);
 
//     if (parsedValues == 3) {
//       Serial.print("Received Angles: ");
//       Serial.print("Joint 1: ");
//       Serial.print(angle1);
//       Serial.print(", Joint 2: ");
//       Serial.print(angle2);
//       Serial.print(", Joint 3: ");
//       Serial.println(angle3);
//     } else {
//       Serial.println("Invalid input format. Please provide angles in the format 'angle1,angle2,angle3'");
//     }
//   }
// }