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

#define TRIGGER_PIN 5
#define ECHO_PIN 18
#define BUZZER_PIN 26
#define BUTTON_PIN 14

int step = 0;

Adafruit_MPU6050 mpu;

// Thresholds 
const float FALL_THRESHOLD = 10.0; // Distance in cm
const float MIN_ACCEL_THRESHOLD = 4.0;
const float MAX_ACCEL_THRESHOLD = 20.0;
const float MIN_GYRO_THRESHOLD = 1.57;
const float MAX_GYRO_THRESHOLD = 15.7;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  delay(100);

  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  Serial.println("AccelX,AccelY,AccelZ,GyroX,GyroY,GyroZ,Distance,FallStatus");

}

void loop() {
  
  float accelX, accelY = 0, accelZ, gyroX, gyroY = 0, gyroZ = 0, distance;
  String fallType = "No Fall";

switch (step % 5) {
    case 0:
      accelX = 5.0; accelZ = 6.0; gyroX = 1.7; distance = 9.0;
      break;
    case 1:
      accelX = 6.2; accelZ = 7.0; gyroX = 2.5; distance = 8.5;
      break;
    case 2:
      accelX = 4.8; accelZ = 5.5; gyroX = 3.2; distance = 6.0;
      break;
    case 3:
      accelX = 5.5; accelZ = 8.0; gyroX = 2.0; distance = 4.9;
      break;
    case 4:
      accelX = 6.8; accelZ = 6.2; gyroX = 1.8; distance = 2.2;
      break;
  }

//fall detection logic

  bool forwardFall = (
    distance <= FALL_THRESHOLD &&
    accelX >= MIN_ACCEL_THRESHOLD && accelX <= MAX_ACCEL_THRESHOLD &&
    accelZ >= MIN_ACCEL_THRESHOLD && accelZ <= MAX_ACCEL_THRESHOLD &&
    gyroX >= MIN_GYRO_THRESHOLD && gyroX <= MAX_GYRO_THRESHOLD
  );

  bool backwardFall = (
    distance <= FALL_THRESHOLD &&
    accelX <= -MIN_ACCEL_THRESHOLD && accelX >= -MAX_ACCEL_THRESHOLD &&
    accelZ >= MIN_ACCEL_THRESHOLD && accelZ <= MAX_ACCEL_THRESHOLD &&
    gyroX >= MIN_GYRO_THRESHOLD && gyroX <= MAX_GYRO_THRESHOLD
  );

  if (forwardFall) {
    fallType = "Forward Fall";
  } else if (backwardFall) {
    fallType = "Backward Fall";
  } else if (distance <= FALL_THRESHOLD) {
    fallType = "Unclassified Fall";
  }

  // Output data for CSV
  Serial.print(accelX); Serial.print(",");
  Serial.print(accelY); Serial.print(",");
  Serial.print(accelZ); Serial.print(",");
  Serial.print(gyroX); Serial.print(",");
  Serial.print(gyroY); Serial.print(",");
  Serial.print(gyroZ); Serial.print(",");
  Serial.print(distance); Serial.print(",");
  Serial.println(fallType);

  // Activate buzzer if fall detected
  if (fallType != "No Fall") {
    digitalWrite(BUZZER_PIN, HIGH);
  } else {
    digitalWrite(BUZZER_PIN, LOW);
  }

  // Buzzer off
  if (digitalRead(BUTTON_PIN) == LOW) {
    digitalWrite(BUZZER_PIN, LOW);
  }

  delay(1000);
}

float measureDistance() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  float duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}