#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

// MPU6050 sensor instance
MPU6050 accelgyro;

// Accelerometer and gyroscope data variables
int16_t ax, ay, az;
int16_t gx, gy, gz;

// Calibration Variables for Accelerometer and Gyroscope
long ax_offset = 0, ay_offset = 0, az_offset = 0;
long gx_offset = 0, gy_offset = 0, gz_offset = 0;

const int ledPin = 9;

// Neural network parameters (placeholders - replace with actual values)
const int inputNodes = 3;
const int hiddenNodes1 = 8;
const int hiddenNodes2 = 10;
const int outputNodes = 1;

// Weights and biases (placeholders - replace with actual trained values)
double weightsIH[3][8] = {
  {-1.7332929372787476, -0.030728066340088844, 0.53829026222229, 0.7977724075317383, 0.7918646335601807, -0.1429106444120407, 1.2371577024459839, 0.19737738370895386}, 
  {-0.8957563042640686, 0.05988804250955582, 0.414846807718277, -2.173250436782837, 1.5082167387008667, 1.3577358722686768, 0.35337963700294495, 1.0292646884918213}, 
  {0.8616539239883423, 0.3304803967475891, 1.2526369094848633, 0.8985792994499207, -0.2142328917980194, 1.2913665771484375, 0.22661778330802917, 1.4102686643600464}
};
double biasH1[8] = {-0.4279812276363373, 1.0470871925354004, 1.0945868492126465, 0.10092014819383621, -0.21153399348258972, -0.05674135684967041, -0.3410114347934723, -0.4191977083683014};
double weightsHH[8][10] = {{ 0.3178192675113678, 0.19624866545200348, -0.5934187769889832, -0.15736271440982819, -0.8161898851394653, -2.1838181018829346, -0.168051078915596, -1.5617356300354004, -3.3251638412475586, 1.0358718633651733},
{0.5164819359779358, 0.4999035596847534, 0.6351611018180847, 0.8489148616790771, 0.06574417650699615, -0.07216133922338486, 1.4452823400497437, -0.949745237827301, 1.2763844728469849, 0.08397503197193146}, 
{0.6403419375419617, 0.5179471969604492, -1.0608934164047241, -0.03340163081884384, 1.10591459274292, 1.0943435430526733, 0.6430270671844482, 0.6119453310966492, 0.5741710662841797, -0.16410189867019653}, 
{-0.5150542855262756, -0.16269627213478088, 0.054955314844846725, -0.5612840056419373, -0.6321089863777161, -0.7397919297218323, -3.978217601776123, -0.5486058592796326, -3.3771872520446777, 0.6071551442146301},
{-0.017643576487898827, -0.07011861354112625, -0.47582802176475525, -0.27513113617897034, -0.4402068257331848, -1.3787341117858887, 0.36472412943840027, -0.7622816562652588, 0.4926058351993561, 0.12027505785226822},
{0.4748125970363617, 0.24685722589492798, 0.7043462991714478, 1.2536100149154663, -1.4474002122879028, -0.677786648273468, 0.5319338440895081, -0.21389080584049225, 0.25951313972473145, -0.5079286098480225}, 
{-1.0941194295883179, -1.5741463899612427, -3.257852077484131, -0.9003089666366577, 0.4900616407394409, -0.2991902828216553, -0.055223528295755386, 0.3819582462310791, -0.04429405927658081, -0.6773982644081116}, 
{-1.6500792503356934, -1.5120511054992676, 0.8199033737182617, -1.0092185735702515, -0.7204727530479431, -0.42476436495780945, 0.44715550541877747, 0.1975637972354889, 0.4323095679283142, -0.797813355922699}};

double biasH2[10] = {-0.1010177955031395,0.25223785638809204, 0.29574695229530334, 0.7421938180923462, 0.38986897468566895, 0.6200947761535645, 0.8144934177398682, 0.992377519607544, 0.42223060131073, 0.11852127313613892};
double weightsHO[10][1] = {{0.8774768114089966}, {1.0202502012252808}, {3.1046414375305176}, {0.914866030216217}, {-1.262221336364746}, {-2.2898406982421875}, {-1.2658586502075195}, {-0.4295941889286041}, {1.6991374492645264}, {0.2477605789899826}};
double biasO[1] = {0.04932986572384834};

// Function to perform ReLU activation
double relu(double x) {
    return max(0.0, x);
}

double sigmoid(double x) {
    return 1.0 / (1.0 + exp(-x));
}

double runNeuralNetwork(double input[]) {
    double hidden1[hiddenNodes1] = {0};
    double hidden2[hiddenNodes2] = {0};
    double output[outputNodes] = {0};

    // Input to Hidden Layer 1
    for (int i = 0; i < hiddenNodes1; i++) {
        hidden1[i] = 0;
        for (int j = 0; j < inputNodes; j++) {
            hidden1[i] += input[j] * weightsIH[j][i];
        }
        hidden1[i] += biasH1[i];
        hidden1[i] = relu(hidden1[i]); // Activation function
    }

    // Hidden Layer 1 to Hidden Layer 2
    for (int i = 0; i < hiddenNodes2; i++) {
        hidden2[i] = 0;
        for (int j = 0; j < hiddenNodes1; j++) {
            hidden2[i] += hidden1[j] * weightsHH[j][i];
        }
        hidden2[i] += biasH2[i];
        hidden2[i] = relu(hidden2[i]); // Activation function
    }

    // Hidden Layer 2 to Output
    for (int i = 0; i < outputNodes; i++) {
        output[i] = 0;
        for (int j = 0; j < hiddenNodes2; j++) {
            output[i] += hidden2[j] * weightsHO[j][i];
        }
        output[i] += biasO[i];
        output[i] = sigmoid(output[i]); // Activation function
    }

    return output[0]; // Assuming single output for simplicity
}

void setup() {
    Serial.begin(9600);
    Wire.begin();
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    pinMode(ledPin, OUTPUT);
    // Calibrate sensors
    calibrateSensor();
}

void loop() {// Read sensor data
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // Convert raw data to usable form
    double inputs[3] = {
        (ax - ax_offset) / 16384.0,
        (ay - ay_offset) / 16384.0,
        (az - az_offset) / 16384.0
    };

    double ax_g = (ax - ax_offset) / 16384.0;
    double ay_g = (ay - ay_offset) / 16384.0;
    double az_g = (az - az_offset) / 16384.0;

    // // Apply offsets for gyroscope
    // double gx_dps = (gx - gx_offset) / 131.0; // DPS (degrees per second) for gyroscope
    // double gy_dps = (gy - gy_offset) / 131.0;
    // double gz_dps = (gz - gz_offset) / 131.0;

    // Display corrected values
    Serial.print("Adjusted aX: "); Serial.print(ax_g); Serial.print(" g\t");
    Serial.print("Adjusted aY: "); Serial.print(ay_g); Serial.print(" g\t");
    Serial.print("Adjusted aZ: "); Serial.print(az_g); Serial.println(" g");
    // Serial.print("Adjusted gX: "); Serial.print(gx_dps); Serial.print(" °/s\t");
    // Serial.print("Adjusted gY: "); Serial.print(gy_dps); Serial.print(" °/s\t");
    // Serial.print("Adjusted gZ: "); Serial.print(gz_dps); Serial.println(" °/s");

    // Compute neural network output
    double fall_score = runNeuralNetwork(inputs);
    Serial.print("Fall Score: ");
    Serial.println(fall_score);
    bool is_falling = fall_score > 0.7; // Threshold needs adjustment based on training

    if (is_falling) {
    digitalWrite(ledPin, HIGH);
    Serial.println("Fall detected!");
    } else {
    digitalWrite(ledPin, LOW);
    }

    // Output results
    Serial.print("Fall Detected: ");
    Serial.println(is_falling);

    delay(100);
}

// Sensor calibration function
void calibrateSensor() {
    long ax_sum = 0, ay_sum = 0, az_sum = 0;
    long gx_sum = 0, gy_sum = 0, gz_sum = 0;

    for (int i = 0; i < 1000; i++) {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        ax_sum += ax;
        ay_sum += ay;
        az_sum += az;
        gx_sum += gx;
        gy_sum += gy;
        gz_sum += gz;
        delay(5); // Small delay to not overload the I2C bus
    }

    ax_offset = ax_sum / 1000;
    ay_offset = ay_sum / 1000;
    az_offset = (az_sum / 1000) + 16384; // Adjust for gravity (assuming sensor is flat)
    gx_offset = gx_sum / 1000;
    gy_offset = gy_sum / 1000;
    gz_offset = gz_sum / 1000;

    Serial.println("Calibration complete");
}