#include <Wire.h>
#include <Servo.h>
#include <MPU6050.h>

MPU6050 mpu1; // MPU on the shoulder
MPU6050 mpu2; // MPU on the elbow
MPU6050 mpu3; // MPU on the wrist

Servo shoulderServo1; // Servo for shoulder pitch
Servo shoulderServo2; // Servo for shoulder yaw
Servo shoulderServo3; // Servo for shoulder twist (rotation)
Servo elbowServo;      // Servo for elbow
Servo wristServo1;     // Servo for wrist pitch
Servo wristServo2;     // Servo for wrist roll

// Set up the pins for the servos
int shoulderPin1 = 3;
int shoulderPin2 = 5;
int shoulderPin3 = 7;  // New servo pin for shoulder twist
int elbowPin = 6;
int wristPin1 = 9;
int wristPin2 = 10;

void setup() {
  // Initialize communication with MPU6050
  Wire.begin();
  
  // Initialize the MPU6050s
  mpu1.initialize();
  mpu2.initialize();
  mpu3.initialize();
  
  // Check if MPU6050s are connected
  if (!mpu1.testConnection() || !mpu2.testConnection() || !mpu3.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  // Attach servos
  shoulderServo1.attach(shoulderPin1);
  shoulderServo2.attach(shoulderPin2);
  shoulderServo3.attach(shoulderPin3);  // Attach new shoulder twist servo
  elbowServo.attach(elbowPin);
  wristServo1.attach(wristPin1);
  wristServo2.attach(wristPin2);

  Serial.begin(115200); // Begin serial communication for debugging
}

void loop() {
  // Variables to store gyro data
  int16_t ax1, ay1, az1, gx1, gy1, gz1;  // Shoulder MPU
  int16_t ax2, ay2, az2, gx2, gy2, gz2;  // Elbow MPU
  int16_t ax3, ay3, az3, gx3, gy3, gz3;  // Wrist MPU

  // Read raw data from each MPU
  mpu1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
  mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);
  mpu3.getMotion6(&ax3, &ay3, &az3, &gx3, &gy3, &gz3);

  // Convert gyro values to degrees (you might need to adjust scaling)
  int shoulderPitch = map(gx1, -32768, 32767, 0, 180);  // Pitch axis of shoulder
  int shoulderYaw = map(gz1, -32768, 32767, 0, 180);    // Yaw axis of shoulder
  int shoulderTwist = map(gy1, -32768, 32767, 0, 180);  // Twist of shoulder (rotation)

  int elbowAngle = map(gx2, -32768, 32767, 0, 180);  // Elbow movement
  
  int wristPitch = map(gx3, -32768, 32767, 0, 180);  // Pitch of wrist
  int wristRoll = map(gz3, -32768, 32767, 0, 180);   // Roll of wrist

  // Move the servos based on the angles from the MPU6050s
  shoulderServo1.write(shoulderPitch);   // Shoulder pitch
  shoulderServo2.write(shoulderYaw);     // Shoulder yaw
  shoulderServo3.write(shoulderTwist);   // Shoulder twist (rotation)
  elbowServo.write(elbowAngle);          // Elbow
  wristServo1.write(wristPitch);         // Wrist pitch
  wristServo2.write(wristRoll);          // Wrist roll

  // Delay for a short while to make the movements smoother
  delay(50);
}