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

Adafruit_MPU6050 mpu;

// ================== PARAMETER ==================
#define ANGLE_THRESHOLD 45.0
#define ABNORMAL_TIME 3000 // ms
#define ALPHA 0.96        // Complementary filter constant

// ================== VARIABEL ==================
float offAx = 0, offAy = 0, offAz = 0;
float offGx = 0, offGy = 0;

float pitch = 0, roll = 0;
unsigned long abnormalStart = 0;

unsigned long lastTime = 0;

// ================== SETUP ==================
void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  Serial.println("SMART HELMET - Complementary Filter Mode");

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

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

  delay(1000);

  calibrateMPU();
  lastTime = millis();
  Serial.println("System Ready");
}

// ================== LOOP ==================
void loop() {
  readAndProcess();
  delay(20); // ~50 Hz sampling
}

// ================== SENSOR PROCESS ==================
void readAndProcess() {
  sensors_event_t acc, gyro, temp;
  mpu.getEvent(&acc, &gyro, &temp);

  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;
  lastTime = now;

  // ---------- Accelerometer ----------
  float Ax = (acc.acceleration.x / 9.81) - offAx;
  float Ay = (acc.acceleration.y / 9.81) - offAy;
  float Az = (acc.acceleration.z / 9.81) - offAz;

  float pitchAcc = atan2(Ax, sqrt(Ay * Ay + Az * Az)) * 180 / PI;
  float rollAcc  = atan2(Ay, sqrt(Ax * Ax + Az * Az)) * 180 / PI;

  // ---------- Gyroscope ----------
  float Gx = (gyro.gyro.x * 180 / PI) - offGx; // rad/s → deg/s
  float Gy = (gyro.gyro.y * 180 / PI) - offGy;

  // Integrasi gyro
  float pitchGyro = pitch + Gx * dt;
  float rollGyro  = roll + Gy * dt;

  // ---------- Complementary Filter ----------
  pitch = ALPHA * pitchGyro + (1 - ALPHA) * pitchAcc;
  roll  = ALPHA * rollGyro  + (1 - ALPHA) * rollAcc;

  // ---------- OUTPUT ----------
  Serial.print("Pitch: ");
  Serial.print(pitch, 2);
  Serial.print(" | Roll: ");
  Serial.print(roll, 2);

  checkCondition();
}

// ================== LOGIKA THRESHOLD ==================
void checkCondition() {
  if (abs(pitch) > ANGLE_THRESHOLD || abs(roll) > ANGLE_THRESHOLD) {
    if (abnormalStart == 0) {
      abnormalStart = millis();
    }

    if (millis() - abnormalStart > ABNORMAL_TIME) {
      Serial.println("  --> STATUS: TIDAK NORMAL");
    } else {
      Serial.println("  --> STATUS: VALIDATING...");
    }
  } else {
    abnormalStart = 0;
    Serial.println("  --> STATUS: NORMAL");
  }
}

// ================== KALIBRASI ==================
void calibrateMPU() {
  Serial.println("Calibrating sensor...");
  Serial.println("Letakkan helm diam & datar");
  delay(3000);

  int samples = 300;
  float sumAx = 0, sumAy = 0, sumAz = 0;
  float sumGx = 0, sumGy = 0;

  for (int i = 0; i < samples; i++) {
    sensors_event_t acc, gyro, temp;
    mpu.getEvent(&acc, &gyro, &temp);

    sumAx += acc.acceleration.x / 9.81;
    sumAy += acc.acceleration.y / 9.81;
    sumAz += acc.acceleration.z / 9.81;

    sumGx += gyro.gyro.x * 180 / PI;
    sumGy += gyro.gyro.y * 180 / PI;

    delay(5);
  }

  offAx = sumAx / samples;
  offAy = sumAy / samples;
  offAz = (sumAz / samples) - 1.0;

  offGx = sumGx / samples;
  offGy = sumGy / samples;

  Serial.println("Calibration done");
}