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

typedef int16_t fixed_point_t;

#define FRACTIONAL_BITS 8
#define FIXED_POINT_MULTIPLIER (1 << FRACTIONAL_BITS)

// Convert float to fixed-point
#define TO_FIXED_POINT(x) ((fixed_point_t)((x) * FIXED_POINT_MULTIPLIER))

// Convert fixed-point to float (for debugging)
#define TO_FLOAT(x) (((float)(x)) / FIXED_POINT_MULTIPLIER)

// 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;

// Neural network parameters (placeholders - replace with actual values)
const int input_nodes = 6;
const int hidden1_nodes = 6;
const int hidden2_nodes = 10;
const int output_nodes = 1;

// Fixed-point multiplication
fixed_point_t fixed_point_mul(fixed_point_t a, fixed_point_t b) {
    return (a * b) >> FRACTIONAL_BITS;
}

// Fixed-point addition
fixed_point_t fixed_point_add(fixed_point_t a, fixed_point_t b) {
    return a + b;
}

// Fixed-point ReLU
fixed_point_t fixed_point_relu(fixed_point_t x) {
    return (x > 0) ? x : 0;
}

// Define fixed-point weights and biases (converted beforehand using Python)
fixed_point_t hidden1_weights[6][6] = {{23649, -6373, 8158, 1884, 29021, 8617},
  {-8920, 43387, 23183, 11909, 16550, -16695}, 
  {-13255, -7455, 3334, 16695, -4998, -35484},
  {25, 5, 0, -98, -14, -102},
  {237, -13, -88, 58, -15, -98},
  {69, -19, -1166, 956, 126, 91}};

fixed_point_t hidden1_biases[6] = {9097, 12258, -12166, -13068, 13464, -10879};

fixed_point_t hidden2_weights[6][10] = {{329, -8225, -714, -471, -380, 1149, -114, 725, -524, -4950},
 {1670, 2202, 113, 1223, -1909, -1693, -5026, -4475, -4871, -2501},
 {1026, -2107, 710, -960, -724, -195, -431, -1527, 59, -276},
 {-3203, -7644, 1960, -551, -564, -1025, -365, -489, 7, -1161},
 {633, -1318, -495, -1635, 751, 3019, -1177, 2587, 27, 780},
 {-990, 215, 88, -548, -3928, -3116, -344, -8920, 1235, -6312}};
fixed_point_t hidden2_biases[10] = {-6912, 2054, 1863, 13892, 17942, 6989, 13939, -14864, 2818, 10104};

fixed_point_t output_weights[10] = {713, -1707, 308, 2191, -5440, -797, 5764, 1400, 483, 4588};
fixed_point_t output_bias = 2911;


// Function to perform ReLU activation
float relu(float x) {
    return x > 0 ? x : 0;
}

// Function to compute the output of the neural network
fixed_point_t computeNeuralNetwork(fixed_point_t inputs[6]) {
    fixed_point_t hidden1[6] = {0};
    fixed_point_t hidden2[10] = {0};
    // Remove the incorrect declaration of 'output' here
    // ...

    // First hidden layer
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
            hidden1[i] = fixed_point_add(hidden1[i], fixed_point_mul(inputs[j], hidden1_weights[j][i]));
        }
        hidden1[i] = fixed_point_add(hidden1[i], hidden1_biases[i]);
        hidden1[i] = fixed_point_relu(hidden1[i]);
    }

    // Second hidden layer
    for (int i = 0; i < 10; i++) {
        for (int j = 0; j < 6; j++) {
            hidden2[i] += hidden1[j] * hidden2_weights[j][i];
        }
        hidden2[i] += hidden2_biases[i];
        hidden2[i] = fixed_point_relu(hidden2[i]);
    }

    // Correct the redeclaration of 'output' here
    fixed_point_t output = hidden2_biases[0]; // Start with bias
    for (int i = 0; i < 10; i++) { // Correct the loop limit according to hidden2 nodes
        output = fixed_point_add(output, fixed_point_mul(hidden2[i], output_weights[i]));
    }
    return output;
}

// Setup function
void setup() {
    Wire.begin();
    Serial.begin(38400);
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    // Calibrate sensors
    calibrateSensor();
}

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

    // Convert raw data to fixed-point and usable form
    fixed_point_t inputs[6] = {
        TO_FIXED_POINT((ax - ax_offset) / 16384.0),
        TO_FIXED_POINT((ay - ay_offset) / 16384.0),
        TO_FIXED_POINT((az - az_offset) / 16384.0),
        TO_FIXED_POINT((gx - gx_offset) / 131.0),
        TO_FIXED_POINT((gy - gy_offset) / 131.0),
        TO_FIXED_POINT((gz - gz_offset) / 131.0)
    };

    // Compute neural network output
    fixed_point_t fixed_point_fall_score = computeNeuralNetwork(inputs);
    fixed_point_t FIXED_POINT_MULTIPLIER = -27.37;
    float fall_score = TO_FLOAT(fixed_point_fall_score); // Convert back to float for comparison
    Serial.print("Fall Score: ");
    Serial.println(fall_score);
    bool is_falling = fall_score > (0.8 * FIXED_POINT_MULTIPLIER); // Threshold needs adjustment based on training

    // 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");
}