#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include "DHT.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <RTClib.h>  // RTC DS1307 library

#define CS_PIN 5          // Chip select pin for SD card
#define DHT1_PIN 4        // GPIO connected to the first DHT22 (Battery Temp)
#define DHT2_PIN 15       // GPIO connected to the second DHT22 (Motor Temp)
#define DHT_TYPE DHT22    // DHT22 sensor

#define RED_LED_PIN 2     // GPIO pin for Red LED
#define GREEN_LED_PIN 13  // GPIO pin for Green LED
#define LOGGING_LED_PIN 12 // GPIO pin for SD Card Logging LED

DHT dht1(DHT1_PIN, DHT_TYPE);  // First DHT22 (Battery Temp)
DHT dht2(DHT2_PIN, DHT_TYPE);  // Second DHT22 (Motor Temp)
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Set LCD address to 0x27 for a 16 chars and 2 line display
Adafruit_MPU6050 mpu;
RTC_DS1307 rtc;

// Low-pass filter coefficients
const float alpha = 0.5; // Adjust this value for smoothing (0 < alpha < 1)
float angleX = 0.0;  // Filtered angleX
float angleY = 0.0;  // Filtered angleY

void setup() {
  Serial.begin(115200);
  
  // Initialize the DHT22 sensors
  dht1.begin();
  dht2.begin();

  // Initialize the LCD
  lcd.init();
  lcd.backlight();  // Turn on the backlight

  // Initialize the SD card
  if (!SD.begin(CS_PIN)) {
    Serial.println("Card Mount Failed");
    lcd.setCursor(0, 0);
    lcd.print("SD Mount Failed");
    return;
  }
  
  uint64_t cardSize = SD.cardSize() / (1024 * 1024);
  Serial.printf("SD Card Size: %llu MB\n", cardSize);
  lcd.setCursor(0, 0);
  lcd.print("SD initialized");

  // Create a CSV file and add headers if file does not exist
  File file = SD.open("/datalog.csv", FILE_WRITE);
  if (!file) {
    Serial.println("Failed to open file for writing");
    lcd.setCursor(0, 1);
    lcd.print("File Open Fail");
    return;
  }

  // Write headers if the file is empty
  if (file.size() == 0) {
    file.println("Time,BatteryTemp (C),MotorTemp (C),AngleX,AngleY,AccelX,AccelY,AccelZ,GyroX,GyroY,GyroZ");
  }
  file.close();

  // Initialize MPU6050 sensor
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    lcd.setCursor(0, 1);
    lcd.print("MPU6050 Fail");
    return;
  }
  
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Initialize RTC
  if (!rtc.begin()) {
    lcd.setCursor(0, 1);
    lcd.print("RTC Fail");
    return;
  }
  if (!rtc.isrunning()) {
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  }

  // Initialize LED pins
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(LOGGING_LED_PIN, OUTPUT); // Set SD Card Logging LED pin as output
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
  digitalWrite(LOGGING_LED_PIN, LOW); // Initially turn off logging LED

  lcd.setCursor(0, 1);
  lcd.print("Sensors Ready");
  delay(2000);  // Delay to display the info before starting logging
}

void loop() {
  // Read temperature values from the DHT22 sensors
  float batteryTemp = dht1.readTemperature();
  float motorTemp = dht2.readTemperature();

  // Read acceleration data from the MPU6050
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate tilt angle using accelerometer data
  float newAngleX = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
  float newAngleY = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;

  // Apply low-pass filter
  angleX = alpha * newAngleX + (1 - alpha) * angleX;
  angleY = alpha * newAngleY + (1 - alpha) * angleY;

  // Check if any reads failed
  if (isnan(batteryTemp) || isnan(motorTemp)) {
    Serial.println("Failed to read from DHT sensors!");
    lcd.setCursor(0, 0);
    lcd.print("DHT Error");
    return;
  }

  // Get the current date and time from the RTC
  DateTime now = rtc.now();

  // Print battery and motor temperature values to Serial Monitor
  Serial.print("Battery Temperature: ");
  Serial.print(batteryTemp);
  Serial.println("°C");

  Serial.print("Motor Temperature: ");
  Serial.print(motorTemp);
  Serial.println("°C");

  // Print the MPU6050 X and Y angles to Serial Monitor
  Serial.print("MPU6050 Angle X: ");
  Serial.print(angleX);
  Serial.println("°");

  Serial.print("MPU6050 Angle Y: ");
  Serial.print(angleY);
  Serial.println("°");

  // Accident detection threshold (tilt beyond 45 degrees)
  if (abs(angleX) > 45 || abs(angleY) > 45) {
    Serial.println("Accident Detected! Tilt beyond 45°.");
    lcd.setCursor(0, 0);
    lcd.print("Accident Detected!");
    
    // Turn on the red LED (continuous)
    digitalWrite(RED_LED_PIN, HIGH);  
    
    // Check which axis triggered the accident
    if (abs(angleX) > 45) {
      lcd.setCursor(0, 1);
      lcd.print("Angle X: ");
      lcd.print(angleX);
      lcd.print("  ");  // Clear any extra characters
    } else if (abs(angleY) > 45) {
      lcd.setCursor(0, 1);
      lcd.print("Angle Y: ");
      lcd.print(angleY);
      lcd.print("  ");  // Clear any extra characters
    }

    delay(1000);  // Hold accident message for 1 second

    // Simulate SMS sent (blink green LED)
    digitalWrite(GREEN_LED_PIN, HIGH);  // Turn on green LED
    delay(1000);  // Green LED stays on for 1 second
    digitalWrite(GREEN_LED_PIN, LOW);   // Turn off green LED
    delay(1000);  // Wait for another second (1-second off)

    // Print SMS sent message to Serial Monitor
    Serial.println("SMS Sent.");

    lcd.clear();  // Clear the display for the next readings
  } else {
    // No accident detected
    Serial.println("No accident detected.");
    
    // Turn off the red LED if no accident detected
    digitalWrite(RED_LED_PIN, LOW);
    
    lcd.setCursor(0, 0);
    lcd.print("Battery Temp: ");
    lcd.print(batteryTemp);
    lcd.setCursor(0, 1);
    lcd.print("Motor Temp: ");
    lcd.print(motorTemp);
  }

  // Log data to SD card
  File file = SD.open("/datalog.csv", FILE_APPEND);
  if (!file) {
    Serial.println("Failed to open file for appending");
    lcd.setCursor(0, 1);
    lcd.print("File Append Fail");
    return;
  }

  // Turn on the logging LED to indicate logging is in progress
  digitalWrite(LOGGING_LED_PIN, HIGH);

  // Write data to the CSV file
  file.printf("%02d/%02d/%02d %02d:%02d:%02d,%0.2f,%0.2f,%0.2f,%0.2f,%0.2f,%d,%d,%d\n",
              now.year(), now.month(), now.day(), now.hour(), now.minute(), now.second(),
              batteryTemp, motorTemp, angleX, angleY, a.acceleration.x, a.acceleration.y, a.acceleration.z,
              g.gyro.x, g.gyro.y, g.gyro.z);
  file.close();

  // Turn off the logging LED after writing
  digitalWrite(LOGGING_LED_PIN, LOW);

  Serial.println("---------------------"); // Print a straight line

  // Wait for a second before reading again
  delay(2000);
}