#include <Wire.h>
#include <Servo.h>
// #include <MPU6050.h> // Include the library for the MPU6050 IMU

Servo servo1; // Servo for yaw (servo 1)
Servo servo2; // Servo for pitch (servo 2)
Servo servo3; // Servo for yaw (servo 3)
Servo servo4; // Servo for pitch (servo 4)

// MPU6050 imu; // Create an MPU6050 object

// Function to calculate the angle from gyro data
void calculateAngle(float gyroX, float gyroY, float gyroZ, float& angleX, float& angleY, float& angleZ, float dt) {
  // Integrate angular velocities to get angles
  angleX += gyroX * dt;
  angleY += gyroY * dt;
  angleZ += gyroZ * dt;
}

// Global variables to store the angles
float angleX = 0;
float angleY = 0;
float angleZ = 0;


// Function to map angles from th IMU
void mapIMUangle(int yawValue, int pitchValue, int rollValue, int* servoValues) {
  servoValues[0] = 90 + yawValue;       // PosServoYawValue
  servoValues[1] = 90 + pitchValue;     // PosServoPitchValue
  servoValues[2] = 90 - yawValue;       // NegServoYawValue
  servoValues[3] = 90 - pitchValue;     // NegServoPitchValue

  servoValues[4] = 90 + rollValue;     // PosServoRollValue
  
}


void setup() {
  Wire.begin(); // Initialize I2C communication
  Serial.begin(9600); // Start serial communication at 9600 baud rate
//   imu.initialize(); // Initialize the IMU
  
  // if (!imu.testConnection()) {
//     Serial.println("IMU connection failed");
//   }

  // Attach the servos to their respective pins
  servo1.attach(7);
  servo2.attach(6);
  servo3.attach(5);
  servo4.attach(4);
}

void loop() {
//   int16_t ax, ay, az, gx, gy, gz;
  
  // Read raw accelerometer and gyroscope values
//   imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convert accelerometer data to angles
  // Assuming ax is roll, ay is pitch, and az is yaw

  // Check if data is available in the serial buffer
  // Prompt the user for the yaw value
  Serial.println("Enter yaw value: ");
  while (Serial.available() == 0) {} // Wait for user input
  int yawValue = Serial.parseInt(); // Read the yaw value
  Serial.println(yawValue);

// Clear the Serial buffer
while (Serial.available() > 0) {
  Serial.read(); // Read and discard any remaining characters
}

  // Prompt the user for the pitch value
  Serial.println("Enter pitch value: ");
  while (Serial.available() == 0) {} // Wait for user input
  int pitchValue = Serial.parseInt(); // Read the pitch value
  Serial.println(pitchValue);


// Clear the Serial buffer
while (Serial.available() > 0) {
  Serial.read(); // Read and discard any remaining characters
}

  // Prompt the user for the roll value
  Serial.println("Enter roll value: ");
  while (Serial.available() == 0) {} // Wait for user input
  int rollValue = Serial.parseInt(); // Read the roll value
  Serial.println(rollValue);


// Clear the Serial buffer
while (Serial.available() > 0) {
  Serial.read(); // Read and discard any remaining characters
}

  int servoValues[5]; // Array to hold the returned values
  mapIMUangle(yawValue, pitchValue, rollValue, servoValues);

  Serial.println((servoValues[1] + servoValues[2] + servoValues[4] -90)/2);
  Serial.println((servoValues[3] + servoValues[2] + servoValues[4] -90)/2);
  Serial.println((servoValues[3] + servoValues[0] + servoValues[4] -90)/2);
  Serial.println((servoValues[1] + servoValues[0] + servoValues[4] -90)/2);

  // Update servos with the new values
  servo1.write((servoValues[1] + servoValues[2] + servoValues[4] -90)/2); // servo 1, positive pitch value and negative yaw value
  servo2.write((servoValues[3] + servoValues[2] + servoValues[4] -90)/2); // servo 2, negative pitch value and negative yaw value
  servo3.write((servoValues[3] + servoValues[0] + servoValues[4] -90)/2); // servo 3, negative pitch value and positive yaw value
  servo4.write((servoValues[1] + servoValues[0] + servoValues[4] -90)/2); // servo 4, positive pitch value and positive yaw value

  delay(100); // Delay to allow the servos to reach the position
}