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

MPU6050 mpu;

// Variables to store the calibrated initial orientation
float initialRoll = 0.0;
float initialPitch = 0.0;

// Example GPS coordinates of the other device
float otherLat = 34.0522; // Latitude of the other device
float otherLon = -118.2437; // Longitude of the other device

void setup() {
  Serial.begin(9600);
  
  // Initialize the MPU6050 sensor
  Wire.begin();
  mpu.initialize();
  
  // Wake up the sensor
  mpu.setSleepEnabled(false);
  
  // Manually calibrate the initial orientation
  calibrateInitialOrientation();
}

void loop() {
  // Read raw accelerometer values
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  
  // Read raw gyroscope values
  int16_t gx, gy, gz;
  mpu.getRotation(&gx, &gy, &gz);

  // Convert raw accelerometer values to roll and pitch angles
  float roll = atan2(ay, az) * RAD_TO_DEG;
  float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;

  // Convert raw gyroscope values to radians per second
  float dt = 0.1; // Time interval in seconds, adjust as needed
  float gyroX = gx / 131.0 * DEG_TO_RAD; // Convert to radians per second
  float gyroY = gy / 131.0 * DEG_TO_RAD; // Convert to radians per second

  // Integrate gyroscope data to obtain current orientation
  roll += gyroX * dt;
  pitch += gyroY * dt;

  // Ensure the roll and pitch angles are within -180 to 180 degrees
  roll = fmod(roll + 180, 360) - 180;
  pitch = fmod(pitch + 180, 360) - 180;

  // Calculate heading (yaw) angle from roll and pitch (tilt-compensated)
  float heading = atan2(-ay, ax) * RAD_TO_DEG;
  if (heading < 0) heading += 360; // Ensure heading is in the range [0, 360)

  // Calculate the angle difference between the initial orientation and the current orientation
  float angleFromNorth = heading - initialRoll;

  // Adjust the angle to be within [0, 360)
  if (angleFromNorth < 0) angleFromNorth += 360;

  // Print the angle from north
  Serial.print("Angle from North: ");
  Serial.println(angleFromNorth);

  // Calculate bearing angle between current and other GPS coordinates (example values)
  float bearing = calculateBearing(40.7128, -74.0060, otherLat, otherLon); // Example current device's location

  // Calculate relative angle between device direction and direction to other device
  float relativeAngle = angleFromNorth - bearing;

  // Adjust the relative angle to be within [-180, 180)
  if (relativeAngle >= 180) relativeAngle -= 360;
  else if (relativeAngle < -180) relativeAngle += 360;

  // Print the relative angle with positive/negative signs
  Serial.print("Relative Angle to Other Device: ");
  Serial.println(relativeAngle);

  delay(100); // Adjust delay as needed
}

// Function to manually calibrate the initial orientation (set direction to north)
void calibrateInitialOrientation() {
  Serial.println("Please hold the sensor steady with its initial orientation pointing north.");
  delay(5000); // Wait for 5 seconds to allow time to stabilize
  
  // Read raw accelerometer values
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  
  // Calculate roll and pitch angles
  initialRoll = atan2(ay, az) * RAD_TO_DEG;
  initialPitch = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;

  Serial.print("Initial Roll: ");
  Serial.println(initialRoll);
  Serial.print("Initial Pitch: ");
  Serial.println(initialPitch);
}

// Function to calculate bearing angle between two GPS coordinates
float calculateBearing(float lat1, float lon1, float lat2, float lon2) {
  float dLon = lon2 - lon1;
  float y = sin(dLon) * cos(lat2);
  float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
  float bearing = atan2(y, x) * RAD_TO_DEG;

  // Adjust the bearing angle to be within [0, 360)
  if (bearing < 0) bearing += 360;

  return bearing;
}