#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 = 6;
const int hiddenNodes1 = 8;
const int hiddenNodes2 = 10;
const int outputNodes = 1;

// Weights and biases (placeholders - replace with actual trained values)
double weightsIH[6][8] = {
  {-1.6666831970214844, -1.982496976852417, 2.305609941482544, 10.40841007232666, 0.6642792224884033, -2.2217612266540527, 7.048738479614258, -2.222945213317871}, 
  {-8.625215530395508, 8.178571701049805, -0.8873629570007324, 0.009885075502097607, -5.9111199378967285, 5.046274185180664, 0.25031688809394836, 9.973685264587402}, 
  {2.6748464107513428, -0.7955495715141296, -0.40777719020843506, 0.4385840892791748, -6.972821235656738, 3.80814266204834, -1.290450096130371, -2.081714391708374}, 
  {-0.012974081560969353, 0.008458582684397697, 0.01329050213098526, -0.007923971861600876, -0.01061349269002676, -0.03150360658764839, -0.012809110805392265, -0.016528744250535965}, 
  {-0.00627892604097724, 0.018411483615636826, 0.11086048185825348, 0.0005318758776411414, -0.019660737365484238, 0.027200328186154366, -0.09902000427246094, -0.010255307890474796}, 
  {-0.038234081119298935, -0.27854248881340027, -0.19022709131240845, 0.049788083881139755, 0.004336487967520952, 0.1944037824869156, -0.08052022755146027, -0.010592487640678883}
};
double biasH1[8] = {-2.1129140853881836, -3.2491250038146973, -1.8860037326812744, -0.7087794542312622, -1.637790560722351, -1.679781436920166, -1.5999687910079956, -0.590392529964447};
double weightsHH[8][10] = {
  {-0.2600303292274475, -0.08955512195825577, 0.24186621606349945, -0.5574265122413635, -0.2133403867483139, -1.9746298789978027, -0.3069925904273987, -1.293097972869873, -1.6836482286453247, -0.6809917688369751},
  {0.2834261655807495, -0.29355594515800476, -0.03023320995271206, 0.0007925248355604708, -0.13590846955776215, -0.1663261502981186, -0.07207901030778885, -0.12232939898967743, 0.4240383505821228, -0.3093823790550232}, 
  {-0.1772463321685791, 0.041221145540475845, 0.011736176908016205, -0.16919241845607758, -0.26203933358192444, 0.22015726566314697, -0.071701779961586, -0.17149178683757782, 0.003509757574647665, 0.11479897797107697}, 
  {-0.32035282254219055, -1.1200673580169678, -0.6377600431442261, 0.08841549605131149, -0.5261958837509155, -0.08532295376062393, 0.5005834102630615, -0.29807424545288086, 0.49427732825279236, 0.5635676980018616}, 
  {0.49917301535606384, 0.7205110192298889, 0.17159371078014374, -0.5565217733383179, -0.5026853680610657, 0.20160624384880066, -0.35470399260520935, -0.6737722158432007, -2.924612522125244, -0.8187964558601379}, 
  {0.2862154543399811, 0.09122097492218018, 0.054715149104595184, -0.1966077983379364, -0.23437073826789856, 0.006074185017496347, -0.00766748609021306, -0.24229510128498077, -0.44860005378723145, -0.33083561062812805},
  {0.07687021791934967, 0.06972159445285797, -0.045988406985998154, -0.10723959654569626, -0.4650948941707611, 0.10180417448282242, 0.11322056502103806, -0.19073475897312164, -0.9030448794364929, 0.204911470413208},
  {0.26840659976005554, 0.5484216213226318, -3.363384246826172, -0.4857252836227417, 0.12391776591539383, -1.4583289623260498, -0.20364071428775787, -0.07516636699438095, -0.13447588682174683, -0.261120468378067}
};
double biasH2[10] = {2.6277077198028564, -5.700150012969971, 2.715191602706909, 3.5393295288085938, 3.5148284435272217, -0.6013083457946777, -4.933608055114746, 2.1235153675079346, -0.9696763157844543, 3.8187592029571533};
double weightsHO[10][1] = {0.15733002126216888, -0.17739206552505493, 0.3195098042488098, -1.1960334777832031, 1.3986133337020874, -0.13479840755462646, 0.22795462608337402, -2.3304572105407715, 0.11784365773200989, -0.24133506417274475};
double biasO[1] = {0.38713976740837097};

// 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[6] = {
        (ax - ax_offset) / 16384.0,
        (ay - ay_offset) / 16384.0,
        (az - az_offset) / 16384.0,
        (gx - gx_offset) / 131.0,
        (gy - gy_offset) / 131.0,
        (gz - gz_offset) / 131.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.5; // 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");
}