#include <Wire.h>
#include <MPU6050.h>
#include<math.h>

/*Agenda of Code:
Just get the angle, acceleration and roc value store this data */

MPU6050 mpu;

const float a = 60.00;   // Threshold Angle
const float b = 1.50;    // Net Acceleration Threshold
const float c = 2.00;    // Rate of change probability
float acta=150.00;//
float acta1;
unsigned long previoustime;
float proba = 0.92;
int fallsigns[1000];
int i = 0, count = 0;
float a1 = 0.00; // Angle in current iteration
float b1 = 0.0;  // Net Acceleration in current iteration
float c1 = 0.0;  // Rate of Change in current iteration
const float sa = 0.4;
const float sn = 0.5;
const float ss = 0.1;

float wa = 0.2;  // Angle Weight
float wn = 0.4;  // Net Accel Weight
float ws = 0.4;  // Rate of Change weight
float fallProbability = 0.0;
float a2 = a;

int buzzerPin = 2;
void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();

  pinMode(buzzerPin, OUTPUT);  // Initialize the buzzer pin as an output

  // Check MPU6050 connection
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }
  Serial.println("MPU6050 connection successful");
}

void loop() {
  // Get angle data from MPU6050
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Convert raw accelerometer data to 'g' (assuming 16-bit data with range ±2g)
  float ax_g = ax / 16384.0;
  float ay_g = ay / 16384.0;
  float az_g = az / 16384.0;

  // Calculate pitch (angle in degrees)
  acta1 = atan2(ax_g, sqrt(ay_g * ay_g + az_g * az_g)) * 180.0 / PI;
  a1=acta1-90.00;
  if(a1<0){
   a1=abs(a1);
}
  // Calculate net acceleration (in g's)
  b1 = sqrt(ax_g * ax_g + ay_g * ay_g + az_g * az_g);

  // Calculate the time difference
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - previoustime) / 1000.0; // Time difference in seconds

  // Calculate the rate of change of angle
  c1 = (a1 - a2) / deltaTime;

  // Calculate fall probability using the dynamically updated weights
  float fallProbability = calculateFallProbability(a1, b1, c1);

  // Print the angle, net acceleration, rate of change, and fall probability
  Serial.println("Angle: " + String(a1) + " degrees");
  Serial.println("Actual Angle: " + String(acta1) + " degrees");
  Serial.println("Net Acceleration: " + String(b1) + " g");
  Serial.println("Rate of Change: " + String(c1) + " deg/s");
  Serial.println("Fall Probability: " + String(fallProbability));

  // Trigger an alert if fall signs are detected
  if (fallProbability > 0.6 || b1 > b) {
    Serial.println("Fall signs are detected!");
    fallsigns[count] = 1;
  } else {
    fallsigns[count] = 0;
  }
  // Check if there are three consecutive fall signs
  if (count >= 2 && fallsigns[count - 2] == 1 && fallsigns[count - 1] == 1 && fallsigns[count] == 1) {
    Serial.println("Fall is expected, triggering the alert...");
    triggerBuzzer();
  } else {
    digitalWrite(buzzerPin, LOW); // Turn off the buzzer
  }
  // Update variables for the next iteration
  previoustime = currentTime;
  a2 = a1;
  count = (count + 1) % 1000; // Wrap count to avoid overflow
  delay(2000); // Adjust delay for sensor readings
}

// Function to calculate fall probability
float calculateFallProbability(float a1, float b1, float c1) {
  float preterm = (sa * wa * (a1 - a) + sn * wn * (b1 - b) + ss * ws * (c1 - c));
  float expterm = exp(-preterm);
  fallProbability = 1.992 * proba / (1.0 + expterm);
  if(fallProbability>=1.00){
    fallProbability=1.00;
  }
  return fallProbability; 
}

// Function to trigger the buzzer
void triggerBuzzer() {
  digitalWrite(buzzerPin, HIGH); // Turn on the buzzer
  delay(5000);                   // Keep the buzzer on for 5 seconds
  digitalWrite(buzzerPin, LOW);  // Turn off the buzzer
  delay(500);
}