#include <Wire.h>

// MPU6050 I2C address
const int MPU6050_addr = 0x68;

// Pin definitions
const int BUZZER_PIN = 34;
const int LED_PIN = 35;

// Sensor raw data
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

// Orientation angles
float pitch, roll, yaw;
float prev_pitch = 0, prev_roll = 0, prev_yaw = 0;

// Threshold for orientation change (in degrees)
const float ANGLE_THRESHOLD = 15.0;

// Timing variables
unsigned long lastTime = 0;
const unsigned long SAMPLE_RATE = 100; // ms

// Alert timing
unsigned long alertStartTime = 0;
const unsigned long ALERT_DURATION = 500; // ms
bool alertActive = false;

void setup() {
  Serial.begin(115200);
  
  // Initialize I2C
  Wire.begin(21, 22); // SDA = 21, SCL = 22
  
  // Initialize MPU6050
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // Set to zero (wakes up the MPU6050)
  Wire.endTransmission(true);
  
  // Configure pins
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  
  // Turn off alert components initially
  digitalWrite(BUZZER_PIN, LOW);
  digitalWrite(LED_PIN, LOW);
  
  Serial.println("Orientation Visualizer Started");
  Serial.println("Pitch, Roll, Yaw, Alert Status");
  
  delay(1000); // Allow sensor to stabilize
}

void loop() {
  unsigned long currentTime = millis();
  
  // Read sensor data at specified sample rate
  if (currentTime - lastTime >= SAMPLE_RATE) {
    lastTime = currentTime;
    
    // Read MPU6050 data
    readMPU6050();
    
    // Calculate orientation angles
    calculateOrientation();
    
    // Check for significant orientation changes
    bool orientationChanged = checkOrientationChange();
    
    // Handle alerts
    if (orientationChanged && !alertActive) {
      triggerAlert();
    }
    
    // Update alert status
    updateAlert();
    
    // Print data to Serial Monitor
    printData(orientationChanged);
    
    // Update previous values
    prev_pitch = pitch;
    prev_roll = roll;
    prev_yaw = yaw;
  }
}

void readMPU6050() {
  Wire.beginTransmission(MPU6050_addr);
  Wire.write(0x3B);  // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_addr, 14, true); // Request 14 registers
  
  AcX = Wire.read() << 8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read() << 8 | Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

void calculateOrientation() {
  // Convert accelerometer values to angles
  // Roll (rotation around X-axis)
  roll = atan2(AcY, AcZ) * 180.0 / PI;
  
  // Pitch (rotation around Y-axis)
  pitch = atan2(-AcX, sqrt(AcY * AcY + AcZ * AcZ)) * 180.0 / PI;
  
  // Yaw calculation using gyroscope (simplified integration)
  static float yaw_gyro = 0;
  float gyro_z_rate = GyZ / 131.0; // Convert to degrees per second (assuming ±250°/s range)
  yaw_gyro += gyro_z_rate * (SAMPLE_RATE / 1000.0); // Integrate over time
  yaw = yaw_gyro;
  
  // Keep yaw within -180 to 180 range
  if (yaw > 180) yaw -= 360;
  if (yaw < -180) yaw += 360;
}

bool checkOrientationChange() {
  float pitch_change = abs(pitch - prev_pitch);
  float roll_change = abs(roll - prev_roll);
  float yaw_change = abs(yaw - prev_yaw);
  
  // Handle yaw wraparound
  if (yaw_change > 180) {
    yaw_change = 360 - yaw_change;
  }
  
  return (pitch_change > ANGLE_THRESHOLD || 
          roll_change > ANGLE_THRESHOLD || 
          yaw_change > ANGLE_THRESHOLD);
}

void triggerAlert() {
  alertActive = true;
  alertStartTime = millis();
  
  // Turn on buzzer and LED
  digitalWrite(BUZZER_PIN, HIGH);
  digitalWrite(LED_PIN, HIGH);
}

void updateAlert() {
  if (alertActive && (millis() - alertStartTime >= ALERT_DURATION)) {
    alertActive = false;
    
    // Turn off buzzer and LED
    digitalWrite(BUZZER_PIN, LOW);
    digitalWrite(LED_PIN, LOW);
  }
}

void printData(bool orientationChanged) {
  Serial.print("Pitch: ");
  Serial.print(pitch, 2);
  Serial.print("°, Roll: ");
  Serial.print(roll, 2);
  Serial.print("°, Yaw: ");
  Serial.print(yaw, 2);
  Serial.print("°");
  
  if (orientationChanged) {
    Serial.print(" [ALERT: Orientation Changed!]");
  }
  
  Serial.println();
}
/*
// Function to change the threshold (can be called from Serial input)
void setThreshold(float newThreshold) {
  if (newThreshold > 0 && newThreshold <= 180) {
    ANGLE_THRESHOLD = newThreshold;
    Serial.print("Threshold updated to: ");
    Serial.print(ANGLE_THRESHOLD);
    Serial.println("°");
  }
}*/