#include <ESP32Servo.h>
#include <Wire.h>
#define ECHO_PIN 34  // GPIO 34 (not an analog pin, used for input)
#define TRIG_PIN 32  // GPIO 32 (used for output)
Servo servo_gyro; // blue HORN
Servo servo_sens; // Black HORN
int pos1 = 0; // gyroscope servo position
int pos2 = 0; // sensor servo position

const int buzzer = 25; // GPIO 25 for the buzzer

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

// MPU-6050 registers
const int MPU_ADDR = 0x68; // I2C address of the MPU-6050

int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  // Initialize MPU-6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // Power management 1
  Wire.write(0); // Wake up the MPU-6050
  Wire.endTransmission(true);
  
  servo_gyro.attach(13); // Attach gyroscope servo to pin 13
  servo_sens.attach(14); // Attach sensor servo to pin 14
  pinMode(buzzer, OUTPUT); // Set buzzer - pin 25 as an output
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

void loop() {
  // Read accelerometer and gyroscope values
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting with the first accelerometer register
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true); // Request 14 bytes

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
  gx = Wire.read() << 8 | Wire.read();
  gy = Wire.read() << 8 | Wire.read();
  gz = Wire.read() << 8 | Wire.read();

  // Calculate the angle
  float angle = atan2(ay, az) * 180 / PI; // Get tilt angle in degrees
  
  // Map the angle to servo range (0-180)
  int servoPos = map(angle, -90, 90, 0, 180);
  servoPos = constrain(servoPos, 0, 180); // Ensure within servo range
  // Write to servo
  servo_gyro.write(servoPos);
  
  // Print for debugging
  Serial.print("Angle: ");
  Serial.println(angle);
  delay(100);
  
  float distance = readDistanceCM();
  Serial.print("Distance: ");
  Serial.println(distance);
  
  for (pos2 = 0; pos2 <= 180; pos2 += 1) { // 0-180 (1 degree steps)
    servo_sens.write(pos2);  // Tell servo to go to position in variable 'pos'
    delay(15);           
  }
  
  for (pos2 = 180; pos2 >= 0; pos2 -= 1) { // 180-0 (1 degree steps)
    servo_sens.write(pos2);  // Tell servo to go to position in variable 'pos'
    delay(15);           
  }
  
  if (distance < 100) {
    tone(buzzer, 440, 809); // Generate a tone at 440 Hz for 809 ms
  } else {
    digitalWrite(buzzer, LOW);
  }
}