#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <math.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define BASE_SERVO 0
#define SHOULDER_SERVO 1
#define ELBOW_SERVO 2
#define WRIST_SERVO 3
#define HAND_SERVO 4

#define BASE_POT_MASTER 36     
#define SHOULDER_POT_MASTER 39  
#define ELBOW_POT_MASTER 34     
#define WRIST_POT_MASTER 35    
#define HAND_POT_MASTER 32    

float Kp[5] = {2.0, 2.0, 2.0, 2.0, 2.0};
float Ki[5] = {0.5, 0.5, 0.5, 0.5, 0.5};
float Kd[5] = {1.0, 1.0, 1.0, 1.0, 1.0};

float previousError[5] = {0, 0, 0, 0, 0};
float integral[5] = {0, 0, 0, 0, 0};

const float L1 = 90.0; // Base to shoulder length
const float L2 = 90.0; // Shoulder to elbow length
const float L3 = 90.0; // Elbow to wrist length
const float L4 = 30.0; // Wrist to end-effector length

float previousPotValues[5] = {0, 0, 0, 0, 0};

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22); // Initialize I2C with SDA and SCL pins
  pwm.begin();
  pwm.setPWMFreq(60); // Set frequency to 60 Hz for servos
}

float getCoordinateFromPot(int potPin) {
  int potValue = analogRead(potPin);
  return map(potValue, 0, 4095, 0, 180); // Mapping potentiometer value to 0-180 degrees
}

void setServoAngle(uint8_t servoNum, float angle) {
  uint16_t pulseLength = map(angle, 0, 180, 150, 600); // Map angle to pulse length
  pwm.setPWM(servoNum, 0, pulseLength);
}

void inverseKinematics(float x, float y, float z, float pitch, float roll, float* angles) {
  // Step 1: Calculate the wrist center position
  float wx = x - L4 * cos(radians(pitch)) * cos(radians(roll));
  float wy = y - L4 * cos(radians(pitch)) * sin(radians(roll));
  float wz = z - L4 * sin(radians(pitch));

  angles[0] = atan2(wy, wx) * 180.0 / PI; // Base angle

  float r = sqrt(wx * wx + wy * wy); // Horizontal distance
  float d = sqrt(r * r + (wz - L1) * (wz - L1)); // Full distance to wrist center

  float cosTheta2 = (d * d - L2 * L2 - L3 * L3) / (2 * L2 * L3);
  float sinTheta2 = sqrt(1.0 - cosTheta2 * cosTheta2);
  angles[2] = atan2(sinTheta2, cosTheta2) * 180.0 / PI; 

  float theta1Prime = atan2(wz - L1, r);
  float theta1Correction = atan2(L3 * sinTheta2, L2 + L3 * cosTheta2);
  angles[1] = (theta1Prime + theta1Correction) * 180.0 / PI; 

  // Step 4: Calculate wrist pitch and roll
  angles[3] = pitch;
  angles[4] = roll;
}

void controlServo(uint8_t servoNum, float targetAngle) {
  setServoAngle(servoNum, targetAngle);
}

void loop() {
  float potValues[5];
  potValues[0] = getCoordinateFromPot(BASE_POT_MASTER);
  potValues[1] = getCoordinateFromPot(SHOULDER_POT_MASTER);
  potValues[2] = getCoordinateFromPot(ELBOW_POT_MASTER);
  potValues[3] = getCoordinateFromPot(WRIST_POT_MASTER);
  potValues[4] = getCoordinateFromPot(HAND_POT_MASTER);

  bool valueChanged = false;
  for (int i = 0; i < 5; i++) {
    if (potValues[i] != previousPotValues[i]) {
      valueChanged = true;
      previousPotValues[i] = potValues[i];
    }
  }

  if (valueChanged) {
    Serial.print("Pot Values - base: ");
    Serial.print(potValues[0]);
    Serial.print(", shoulder: ");
    Serial.print(potValues[1]);
    Serial.print(", elbow: ");
    Serial.print(potValues[2]);
    Serial.print(", wrist: ");
    Serial.print(potValues[3]); 
    Serial.print(", hand: ");
    Serial.println(potValues[4]); 

    float targetAngles[5];
    inverseKinematics(potValues[0], potValues[1], potValues[2], potValues[3], potValues[4], targetAngles);

    Serial.print("Target Angles - Base: ");
    Serial.print(targetAngles[0]);
    Serial.print(", Shoulder: ");
    Serial.print(targetAngles[1]);
    Serial.print(", Elbow: ");
    Serial.print(targetAngles[2]);
    Serial.print(", Wrist Pitch: ");
    Serial.print(targetAngles[3]);
    Serial.print(", Wrist Roll: ");
    Serial.println(targetAngles[4]);

    controlServo(BASE_SERVO, targetAngles[0]);
    controlServo(SHOULDER_SERVO, targetAngles[1]);
    controlServo(ELBOW_SERVO, targetAngles[2]);
    controlServo(WRIST_SERVO, targetAngles[3]);
    controlServo(HAND_SERVO, targetAngles[4]);
  }

  delay(100); // Adjust the delay as needed
}