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

#define HALL_PIN 5           // Hall Effect Sensor connected to pin 5
#define ACTUATOR_PIN 2       // LED simulating motor connected to pin 2

// Define LEDs for each condition
#define LED1 13              // LED for Hall sensor condition (5 meters)
#define LED2 14              // LED for Bank Angle condition (20° inclination)
#define LED3 12              // LED for Speed condition (40 km/h)
#define LED4 27              // LED for Handbrake disengaged indication

Adafruit_MPU6050 mpu;
volatile int hallCounter = 0;
const int tireCircumference = 200;  // Tire circumference in cm
int distanceTravelled = 0;
float angleThreshold = 20.0;        // Angle threshold for bank sensor
int speedThreshold = 40;            // Speed threshold in km/h

// Interrupt handler for Hall sensor
void IRAM_ATTR hallInterrupt() {
  hallCounter++;
}

void setup() {
  Serial.begin(115200);

  // Initialize Hall sensor
  pinMode(HALL_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(HALL_PIN), hallInterrupt, RISING);

  // Initialize the actuator control (LED simulating motor)
  pinMode(ACTUATOR_PIN, OUTPUT);

  // Initialize LEDs
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  pinMode(LED4, OUTPUT);

  // Initialize MPU6050 sensor for bank angle detection
  if (!mpu.begin()) {
    Serial.println("Failed to initialize MPU6050!");
    while (1);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
}

void loop() {
  // Get MPU6050 sensor readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate distance traveled based on Hall sensor
  distanceTravelled = (hallCounter * tireCircumference) / 100;  // in meters
  if (distanceTravelled >= 5) {  // If vehicle has traveled 5 meters
    disengageHandbrake();
    digitalWrite(LED1, HIGH);  // Turn on LED1 for Hall sensor condition
  } else {
    digitalWrite(LED1, LOW);   // Turn off LED1
  }

  // Calculate tilt angle using MPU6050
  float angle = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;
  if (abs(angle) >= angleThreshold) {  // If bank angle exceeds 20°
    disengageHandbrake();
    digitalWrite(LED2, HIGH);  // Turn on LED2 for Bank Angle condition
  } else {
    digitalWrite(LED2, LOW);   // Turn off LED2
  }

  // Check if speed exceeds 40 km/h (simulate with distance over 40 meters)
  if (distanceTravelled >= 40) {
    digitalWrite(LED3, HIGH);  // Turn on LED3 for Speed condition
    // Turn off ESP32 functions (simulated by no action)
  } else {
    digitalWrite(LED3, LOW);   // Turn off LED3
    // Handbrake and system operational below 40 km/h
  }

  // Example: Simulate actuator (LED4 for Handbrake disengaged)
  analogWrite(ACTUATOR_PIN, 255);  // Full brightness simulating full speed
  delay(1000);  // Delay for demonstration

  analogWrite(ACTUATOR_PIN, 0);    // Turn off the LED simulating no movement
  delay(1000);
}

void disengageHandbrake() {
  // Simulate disengaging the handbrake with LED4
  analogWrite(ACTUATOR_PIN, 255);  // Turn on the LED to represent motor action
  digitalWrite(LED4, HIGH);        // Turn on LED4 for Handbrake disengaged
  Serial.println("Handbrake disengaged!");
  delay(1000);                     // Simulate time for disengagement
  digitalWrite(LED4, LOW);         // Turn off LED4 after disengagement
  analogWrite(ACTUATOR_PIN, 0);    // Turn off actuator
}