// Modified version of (c) Michael Schoeffler 2017, http://www.mschoeffler.de

#include <Wire.h> // This library allows you to communicate with I2C devices.
#include<math.h>
#include <Servo.h>
#define g 16000.0
#define PI 3.14

const int MPU_ADDR = 0x68; // I2C address of the MPU-6050. If AD0 pin is set to HIGH, the I2C address will be 0x69.

int16_t accelerometer_x, accelerometer_y, accelerometer_z; // variables for accelerometer raw data
int16_t gyro_x, gyro_y, gyro_z; // variables for gyro raw data
int16_t temperature; // variables for temperature data

float calculateMagnitude(int16_t x, int16_t y, int16_t z) {
  z = z - g; // Subtract bias (g)
  return sqrt(x * x + y * y + z * z);
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR); // Begins a transmission to the I2C slave (GY-521 board)
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
}
void loop() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2, p.40]
  Wire.endTransmission(false); // the parameter indicates that the Arduino will send a restart. As a result, the connection is kept active.
  Wire.requestFrom(MPU_ADDR, 7*2, true); // request a total of 7*2=14 registers
  
  // "Wire.read()<<8 | Wire.read();" means two registers are read and stored in the same variable
  accelerometer_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H) and 0x3C (ACCEL_XOUT_L)
  accelerometer_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x3D (ACCEL_YOUT_H) and 0x3E (ACCEL_YOUT_L)
  accelerometer_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) and 0x40 (ACCEL_ZOUT_L)
  temperature = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H) and 0x42 (TEMP_OUT_L)
  gyro_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x43 (GYRO_XOUT_H) and 0x44 (GYRO_XOUT_L)
  gyro_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x45 (GYRO_YOUT_H) and 0x46 (GYRO_YOUT_L)
  gyro_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x47 (GYRO_ZOUT_H) and 0x48 (GYRO_ZOUT_L)
  
  //TODO: Your code goes here

  float magnitude = calculateMagnitude(accelerometer_x, accelerometer_y, accelerometer_z);

  Serial.println(magnitude);
  // Normalize accelerometer vector
  float x_norm = accelerometer_x / magnitude;
  float y_norm = accelerometer_z / magnitude;
  accelerometer_z -= g;
  float z_norm = accelerometer_z / magnitude;

  // Calculate angles between accelerometer vector and main axes
  float angle_x = acos(x_norm) * 180.0 / PI;  // Angle with X-axis
  float angle_y = acos(y_norm) * 180.0 / PI;  // Angle with Y-axis
  float angle_z = acos(z_norm) * 180.0 / PI;  // Angle with Z-axis

  // Convert angles to range from 0 to 360
  angle_x = fmod(angle_x, 360.0);
  angle_y = fmod(angle_y, 360.0);
  angle_z = fmod(angle_z, 360.0);

  // Print angles
  Serial.print("Angle with X-axis: ");
  Serial.print(angle_x);
  Serial.println(" degrees");

  Serial.print("Angle with Y-axis: ");
  Serial.print(angle_y);
  Serial.println(" degrees");

  Serial.print("Angle with Z-axis: ");
  Serial.print(angle_z);
  Serial.println(" degrees");

  // delay
  delay(1000);
}