#include <Wire.h>

#define ACCLX
#define ACCLY
#define ACCLZ 
#define GYROX
#define GYROY
#define GYROZ
#define ROLL
#define PITCH
#define TEMP 
#define YAW


double initialYaw = 0.0; // Initial yaw angle
double currentYaw = initialYaw;
unsigned long previousTime = 0; // Store the previous time

const float g = 9.8;
const float gyroSensitivity = 0.74;
const int MPU6050_addr = 0x68; // I2C address of the MPU-6050

int16_t accelerometer_x, accelerometer_Y, accelerometer_Z;
int16_t gyro_x, gyro_y, gyro_z;


float xlConv(int16_t accl){
  float acceleration;
  acceleration = (accl / 1000) * g;
  return acceleration;
}

void setup() {
  Serial.begin(9600);
  Wire.begin(); // Initialize I2C
  Wire.beginTransmission(MPU6050_addr); // Start communication with MPU6050
  Wire.setClock(400000);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // Set to 0 to wake up MPU-6050
  Wire.endTransmission(true);
}

void loop() {
/* 
	6 bytes for accelerometer
		2 bytes x
		2 bytes y
		2 bytes z
	2 bytes for temperature
	6 bytes for gyroscope
		2 bytes x
		2 bytes y 
		2 bytes z
*/
#ifdef ACCLX
  Wire.endTransmission(false);
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x3B); // starting with register (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 2, true); // request 2 registers(16 bits in total)
  accelerometer_x = Wire.read() << 8 | Wire.read();
  Serial.print("Acceleration X (m/s^2) :");
  Serial.print(xlConv(accelerometer_x)); Serial.print(", ");
#endif

#ifdef ACCLY
  Wire.endTransmission(false);
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x3D); // starting with register (ACCEL_XOUT_Y)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 2, true);
  accelerometer_Y = Wire.read() << 8 | Wire.read();
  Serial.print("Acceleration Y (m/s^2) :");
  Serial.print(xlConv(accelerometer_Y)); Serial.print(", ");
#endif

#ifdef ACCLZ
  Wire.endTransmission(false);
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x3F); // starting with register (ACCEL_XOUT_Z)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 2, true);
  accelerometer_Z = Wire.read() << 8 | Wire.read();
  Serial.print("Acceleration Z (m/s^2) :");
  Serial.print(xlConv(accelerometer_Z)); Serial.print(", ");
#endif

#ifdef TEMP
  Wire.endTransmission(false);
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x41); // starting with register (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 2, true);
  int16_t temperature = Wire.read() << 8 | Wire.read(); // This value is rarely used
  Serial.print("Temp (°C): ");
  Serial.print(temperature / 340.00 + 36.53); Serial.print(", ");
#endif

#ifdef GYROX
  Wire.endTransmission(false);
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x43); // starting with register (GYRO_XOUT)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 2, true);
  gyro_x = Wire.read() << 8 | Wire.read();
  Serial.print("Gyro X(deg/s): ");
  Serial.print(gyro_x); Serial.print(", ");
#endif

#ifdef GYROY
  Wire.endTransmission(false);
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x45); // starting with register (GYRO_YOUT)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 2, true);
  gyro_y = Wire.read() << 8 | Wire.read();
  Serial.print("Gyro Y(deg/s): ");
  Serial.print(gyro_y); Serial.print(", ");

#endif

#ifdef GYROZ
  Wire.endTransmission(false);
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x47); // starting with register (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 2, true);
  gyro_z = Wire.read() << 8 | Wire.read();
  Serial.print("Gyro Z(deg/s): ");
  Serial.print(gyro_z); Serial.print(", ");
#endif

#ifdef ROLL
  double roll = atan2(accelerometer_Y, accelerometer_Z);
  roll = roll * 180.0 / PI; 
  Serial.print("Roll (degrees): "); 
  Serial.println(roll); 
#endif

#ifdef PITCH
  double pitch = atan2(-accelerometer_x, sqrt(accelerometer_Y * accelerometer_Y + accelerometer_Z * accelerometer_Z));
  pitch = pitch * 180.0 / PI;
  Serial.print("Pitch (degrees): "); 
  Serial.println(pitch);
#endif

#ifdef YAW
  unsigned long currentTime = millis();
  double deltaTime = (currentTime - previousTime) / 1000.0; // Convert to seconds
  double gyroZ = gyro_z / gyroSensitivity;

  double deltaAngle = gyroZ * deltaTime; // Change in yaw angle
  currentYaw += deltaAngle;

  if (currentYaw > 180.0) {
    currentYaw -= 360.0;
  } else if (currentYaw < -180.0) {
    currentYaw += 360.0;
  }

  Serial.print("Yaw (degrees): ");
  Serial.println(currentYaw);

  previousTime = currentTime;


#endif

  delay(1000);
}