#include <Wire.h>
#include <ESP32Servo.h>  // ESP32-compatible Servo library

Servo servo_1;
Servo servo_2;
Servo servo_3;
Servo servo_4;
Servo servo_pot;  // New servo for potentiometer control

const int MPU_addr = 0x68;  // MPU6050 I2C address
int16_t axis_X, axis_Y, axis_Z;
int minVal = 265;
int maxVal = 402;

double x, y, z;

#define FLEX_SENSOR_PIN 34  // Analog-capable pin on ESP32
#define POTENTIOMETER_PIN 33
#define SERVO_POT_PIN 12  // New servo controlled by potentiometer

// Servo GPIO pin assignments (change if needed)
#define SERVO_1_PIN  14   // Forward/Reverse
#define SERVO_2_PIN  27   // Up/Down
#define SERVO_3_PIN  26   // Gripper
#define SERVO_4_PIN  25   // Left/Right

void setup() {
  Serial.begin(9600);

  Wire.begin();  // Default: SDA = 21, SCL = 22

  // Wake up MPU6050
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  // Attach servos
  servo_1.attach(SERVO_1_PIN, 500, 2400);
  servo_2.attach(SERVO_2_PIN, 500, 2400);
  servo_3.attach(SERVO_3_PIN, 500, 2400);
  servo_4.attach(SERVO_4_PIN, 500, 2400);
  servo_pot.attach(SERVO_POT_PIN, 500, 2400);  // Attach new potentiometer servo
}

void loop() {
  // Read MPU6050
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);

  axis_X = Wire.read() << 8 | Wire.read();
  axis_Y = Wire.read() << 8 | Wire.read();
  axis_Z = Wire.read() << 8 | Wire.read();

  int xAng = map(axis_X, minVal, maxVal, -90, 90);
  int yAng = map(axis_Y, minVal, maxVal, -90, 90);
  int zAng = map(axis_Z, minVal, maxVal, -90, 90);

  x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI);
  y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI);
  z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);

  // Flex sensor -> gripper
  int flex_sensorip = analogRead(FLEX_SENSOR_PIN);  // 0–4095 on ESP32
  int gripper = (flex_sensorip > 3000) ? 0 : 180;
  servo_3.write(gripper);

  // X-axis → servo_1 and servo_2
  if (x >= 0 && x <= 60) {
    int mov1 = map(x, 0, 60, 0, 90);
    Serial.print("Movement in F/R = ");
    Serial.print(mov1);
    Serial.println((char)176);
    servo_1.write(mov1);
  } else if (x >= 300 && x <= 360) {
    int mov2 = map(x, 360, 250, 0, 180);
    Serial.print("Movement in Up/Down = ");
    Serial.print(mov2);
    Serial.println((char)176);
    servo_2.write(mov2);
  }

  // Y-axis → servo_4
  if (y >= 0 && y <= 60) {
    int mov3 = map(y, 0, 60, 90, 180);
    Serial.print("Movement in Left = ");
    Serial.print(mov3);
    Serial.println((char)176);
    servo_4.write(mov3);
  } else if (y >= 300 && y <= 360) {
    int mov3 = map(y, 360, 300, 90, 0);
    Serial.print("Movement in Right = ");
    Serial.print(mov3);
    Serial.println((char)176);
    servo_4.write(mov3);
  }

  // Potentiometer to control new servo
  int potValue = analogRead(POTENTIOMETER_PIN);  // 0–4095
  int servoAngle = map(potValue, 0, 4095, 0, 90);
  Serial.print("Potentiometer Servo Angle: ");
  Serial.println(servoAngle);
  servo_pot.write(servoAngle);

  delay(50);  // Small delay to smooth movements
}