#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>  // LCD I2C library

Adafruit_MPU6050 mpu;

// init. LCD (I2C address 0x27, 20 columns, 4 rows)
LiquidCrystal_I2C lcd(0x27, 20, 4);

// ESP32 pin connected to potentiometer (current sensor simulation)
const int currentSensorPin = 36;  // VP pin in Wokwi, GPIO 36
const int LED_PIN_YELLOW = 25;
const int LED_PIN_ORANGE = 33;
const int LED_PIN_RED = 32;

void setup(void) {
  // init. Serial Communication
  Serial.begin(115200);
  while (!Serial)
    delay(10); // waiting for serial monitor

  // init. LCD
  lcd.init();
  lcd.backlight();

  // welcome message
  lcd.setCursor(0, 0);
  lcd.print("Initializing Sensors...");

  Serial.println("Adafruit MPU6050 & Power Monitor Test!");

  // try to init. MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("MPU6050 Error!");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Wheelchair Ready...");

  // config. MPU6050 settings
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }

  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+-250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+-500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+-1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+-2000 deg/s");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }

  // Initialize LED pins as OUTPUT
  pinMode(LED_PIN_YELLOW, OUTPUT);
  pinMode(LED_PIN_ORANGE, OUTPUT);
  pinMode(LED_PIN_RED, OUTPUT);
  
  // Make sure all LEDs are off initially
  digitalWrite(LED_PIN_YELLOW, LOW);
  digitalWrite(LED_PIN_ORANGE, LOW);
  digitalWrite(LED_PIN_RED, LOW);

  Serial.println("");
  delay(1000); // wait before starting loop

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Power Monitor Ready");
}

void loop() {
  // --------------------- battery monitoring ---------------------

  // read sensor (analogRead on ESP32 ranges from 0 to 4095)
  int sensorValue = analogRead(currentSensorPin);

  // convert to amps 
  float current = map(sensorValue, 0, 4095, 0, 5000) / 1000.0;  

  // convert to battery percentage (5A = 100%)
  float battery = (current / 5.0) * 100.0;

  // output to Serial
  Serial.print("Current: ");
  Serial.print(current, 2);
  Serial.println(" Amps");
  Serial.print("Battery: ");
  Serial.print(battery, 1);
  Serial.println(" %");

  // --------------------- accelerometer reading ---------------------

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

  // Output to Serial
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x, 2);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y, 2);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z, 2);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x, 2);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y, 2);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z, 2);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature, 2);
  Serial.println(" degC");

  Serial.println("");

  // --------------------- calc. Tilt Angle ---------------------

  // calc. tilt angle in degrees
  float pitch = atan2(a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180 / M_PI;
  float roll = atan2(a.acceleration.y, sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180 / M_PI;
 
  // print the values to the serial monitor
  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print(" Pitch: ");
  Serial.println(pitch);
 
  // determine tilt direction (fixed logic)
  String tiltDirectionV = "Flat";  //  pitch (Forward/Backward)
  String tiltDirectionH = "Flat";  //  roll (Left/Right)
 
  if (pitch < -5) {
    tiltDirectionV = "Forward Tilt: " + String(abs(pitch), 1) + "D";
  } else if (pitch > 5) {
    tiltDirectionV = "Backward Tilt: " + String(pitch, 1) + "D";
  }

  if (roll < -5) {
    tiltDirectionH = "Left Tilt: " + String(abs(roll), 1) + "D";
  } else if (roll > 5) {
    tiltDirectionH = "Right Tilt: " + String(roll, 1) + "D";
  }

  // --------------------- display info. ---------------------

  // power info.
  lcd.setCursor(0, 0);
  lcd.print("Power: ");
  lcd.print(battery, 1);
  lcd.print(" %      "); // extra spaces to clear previous characters

  // tilt angle
  lcd.setCursor(0, 1);
  lcd.print(tiltDirectionV);  // Display the vertical (pitch) tilt
  lcd.setCursor(0, 2);
  lcd.print(tiltDirectionH);  // Display the horizontal (roll) tilt

  // speed
  String speed = "__";
  lcd.setCursor(0,3);
  lcd.print("Speed: ");
  lcd.print(speed);
  lcd.print(" mph");


  // low battery check
  static unsigned long lastFlashTime = 0;
  static bool flashOn = false;

  if (battery < 20.0) {
    unsigned long currentTime = millis();
  
  // Flash every 500ms
  if (currentTime - lastFlashTime >= 500) {
    lastFlashTime = currentTime;
    flashOn = !flashOn; // Toggle flash state
  }

  lcd.setCursor(0, 0);
  if (flashOn) {
    lcd.print("Battery Low!        ");  // Flash warning message
  } else {
    lcd.print("                    ");  // Clear line for flash effect
  }
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Power: ");
    lcd.print(battery, 1);
    lcd.print(" %      ");      // extra spaces to clear previous characters
  }

  // ------------------- LEDS -------------------------
    // Set default warning levels
  String warning = "Safe";

  // Determine tilt warnings based on pitch and roll
  if (abs(pitch) > 30 || abs(roll) > 30) {
    warning = "RED - Unsafe!";
    activateLED(LED_PIN_RED);
  } 
  else if (abs(pitch) > 20 || abs(roll) > 20) {
    warning = "ORANGE - Danger!";
    activateLED(LED_PIN_ORANGE);
  } 
  else if (abs(pitch) > 10 || abs(roll) > 10) {
    warning = "YELLOW - Steep";
    activateLED(LED_PIN_YELLOW);
  }
  else {
    // Turn off all LEDs if safe
    deactivateAllLEDs();
  }

  // print warning messages on lcd 
  //lcd.print(warning)

  // temp. info.
  // lcd.setCursor(0, 3);
  // lcd.print("Temp: ");
  // lcd.print(temp.temperature, 1);
  // lcd.print(" C    ");

  // --------------------- Delay ---------------------
  delay(500); // wait .5s before updating
}


// Function to turn on a specific LED
void activateLED(int pin) {
  // Turn off all LEDs first
  deactivateAllLEDs();
  
  // Turn on the specified LED
  digitalWrite(pin, HIGH);
}

// Function to turn off all LEDs
void deactivateAllLEDs() {
  digitalWrite(LED_PIN_YELLOW, LOW);
  digitalWrite(LED_PIN_ORANGE, LOW);
  digitalWrite(LED_PIN_RED, LOW);
}