#include <Wire.h>
#include <RTClib.h>           // For DS1307 RTC
#include <Adafruit_MPU6050.h> // For MPU6050 (or Adafruit_ADXL345_U for ADXL345)
#include <Adafruit_Sensor.h>
#include <DHT.h>              // For DHT22 sensor

#define DHTPIN 15            // DHT22 data pin connected to GPIO15
#define DHTTYPE DHT22        // DHT 22 (AM2302)

RTC_DS1307 rtc;
Adafruit_MPU6050 mpu; // Use Adafruit_ADXL345_Unified for ADXL345
DHT dht(DHTPIN, DHTTYPE);// Define DHT22 pin and type

// Define the second I2C bus instance (Wire1)
TwoWire I2CWire1 = TwoWire(1);

const float crashThreshold = 2.0;          // Simplified crash threshold in g's
const float stationaryThreshold = 0.1;    // Very narrow stationary threshold around 1g
const int stationaryTime = 3000;         // Time in milliseconds to consider stationar

unsigned long lastMovementTime = 0;

void setup() {
  Serial.begin(115200);
  
  // Initialize the default I2C bus (Wire) for the RTC DS1307
  Wire.begin(21, 22); // SDA = 21, SCL = 22
  if (!rtc.begin()) {
    Serial.println("Couldn't find RTC");
    while (1);
  }

  if (!rtc.isrunning()) {
    Serial.println("RTC is NOT running, setting the time...");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__))); // Set RTC to compile time
  }
  
  // Initialize the second I2C bus (Wire1) for the MPU6050 (or ADXL345)
  I2CWire1.begin(17, 16); // SDA = 17, SCL = 16
  if (!mpu.begin(0x68, &I2CWire1)) { // Use &I2CWire1 to pass the second bus
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    while (1);
  }
  // Initialize DHT22
  dht.begin();
  
  Serial.println("Setup successful!");
}

double CrashDet(double xAcc, double yAcc, double zAcc){
  float accMagnitude = sqrt(sq(xAcc) + sq(yAcc) + sq(zAcc)) / 9.81; 

  if (accMagnitude > crashThreshold) {
    Serial.print("Crash detected! Acceleration magnitude: ");
    Serial.print(accMagnitude);
    Serial.println(" g");

    lastMovementTime = millis();  // Reset the stationary timer
    return accMagnitude;
  }

  return 0;
}

void loop() {
  // Get RTC time
  DateTime now = rtc.now();
  double crashMag = 0.0;

  Serial.print("Current time: ");
  Serial.print(now.hour(), DEC);
  Serial.print(':');
  Serial.print(now.minute(), DEC);
  Serial.print(':');
  Serial.println(now.second(), DEC);
  
  // Get accelerometer data (MPU6050)
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  crashMag = CrashDet(a.acceleration.x, a.acceleration.y, a.acceleration.z);

  if(crashMag){
    Serial.println(crashMag);
  }
  
  Serial.print("Accelerometer X: "); Serial.print(a.acceleration.x);
  Serial.print(", Y: "); Serial.print(a.acceleration.y);
  Serial.print(", Z: "); Serial.println(a.acceleration.z);

  // Get DHT22 data (temperature and humidity)
  float temperature = dht.readTemperature(); // Celsius
  float humidity = dht.readHumidity();
  
  // Check if any reads failed
  if (isnan(temperature) || isnan(humidity)) {
    Serial.println("Failed to read from DHT sensor!");
  } else {
    Serial.print("Temperature: "); Serial.print(temperature); Serial.print("°C, ");
    Serial.print("Humidity: "); Serial.print(humidity); Serial.println("%");
  }
  delay(1000); // Wait 1 second before looping
}