#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

// Indicator pins
const int leftIndicator = 2;
const int rightIndicator = 3;

// Button pins
const int recalibrateButton = 4;
const int leftSwitch = 5;
const int rightSwitch = 6;

// Thresholds
float pitchThreshold = 15.0;
float rollThreshold = 15.0;

// Base values
float basePitch = 0.0;
float baseRoll = 0.0;

bool leftOn = false;
bool rightOn = false;

void setup() {
  Serial.begin(115200);
  
  pinMode(leftIndicator, OUTPUT);
  pinMode(rightIndicator, OUTPUT);

  pinMode(recalibrateButton, INPUT_PULLUP);
  pinMode(leftSwitch, INPUT_PULLUP);
  pinMode(rightSwitch, INPUT_PULLUP);

  if (!mpu.begin()) {
    Serial.println("MPU6050 not found");
    while (1);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);

  delay(1000);
  calibrate();
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  float pitch = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
  float roll = atan2(-a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180 / PI;

  float pitchOffset = pitch - basePitch;
  float rollOffset = roll - baseRoll;

  if (digitalRead(recalibrateButton) == LOW) {
    calibrate();
    delay(500);
  }

  if (digitalRead(leftSwitch) == LOW) {
    leftOn = true;
    digitalWrite(leftIndicator, HIGH);
    delay(200);  // Debounce
  }

  if (digitalRead(rightSwitch) == LOW) {
    rightOn = true;
    digitalWrite(rightIndicator, HIGH);
    delay(200);  // Debounce
  }

  if (leftOn && abs(rollOffset) > rollThreshold) {
    leftOn = false;
    digitalWrite(leftIndicator, LOW);
  }

  if (rightOn && abs(rollOffset) > rollThreshold) {
    rightOn = false;
    digitalWrite(rightIndicator, LOW);
  }

  delay(100);
}

void calibrate() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  basePitch = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
  baseRoll = atan2(-a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180 / PI;

  Serial.println("Calibration done!");
  Serial.print("Base Pitch: "); Serial.println(basePitch);
  Serial.print("Base Roll: "); Serial.println(baseRoll);
}