#include <Wire.h>
#include <SPI.h>
#include <Adafruit_BMP280.h>
#include <Adafruit_Sensor.h>

#define BMP_SCK  (13)
#define BMP_MISO (12)
#define BMP_MOSI (11)
#define BMP_CS   (10)

//Adafruit_BMP280 bmp; // I2C
Adafruit_BMP280 bmp(BMP_CS); // hardware SPI
//Adafruit_BMP280 bmp(BMP_CS, BMP_MOSI, BMP_MISO,  BMP_SCK);

// Mock MPU6050 class
class MPU6050 {
public:
    void initialize() {
        Serial.println("MPU6050 initialized.");
    }
    bool testConnection() {
        return true;
    }
    void update() {}
    double getGyroZ() {
        // Simulate 0.5 Hz roll about Z-axis (1 cycle every 2 seconds)
        static double angle = 0;
        double frequency = 0.5; // 0.5 Hz
        angle += 360 * frequency / 100; // Increment angle for each loop
        if (angle > 180) angle = -180; // Simulate angle wrap around
        return angle;
    }
};

MPU6050 mpu;
double gyroZ_bias = 0;

// Kalman filter variables for altitude estimation
double altitude_estimate = 0;
double velocity_estimate = 2.0; // Simulated vertical speed of 2 m/s
double accel_bias = 0;
double P[2][2] = { {1, 0}, {0, 1} };
double Q[2][2] = { {0.001, 0}, {0, 0.003} };
double R = 0.03;
double K[2];

double initial_altitude = 0; // Initial altitude offset

// Calibration function for MPU6050
void calibrateMPU6050() {
    long gyroZ_sum = 0;
    int samples = 1000;
    for (int i = 0; i < samples; i++) {
        mpu.update();
        gyroZ_sum += mpu.getGyroZ();
        delay(2);
    }
    gyroZ_bias = gyroZ_sum / samples;
}

// Kalman filter update function for altitude
void kalmanUpdate(double measured_altitude) {
    // Predict step
    altitude_estimate += velocity_estimate * 0.1; // dt = 0.1s
    P[0][0] += P[1][1] + Q[0][0];
    P[0][1] += P[1][1];
    P[1][0] += P[1][1];
    P[1][1] += Q[1][1];

    // Update step
    double y = measured_altitude - altitude_estimate;
    double S = P[0][0] + R;
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    altitude_estimate += K[0] * y;
    velocity_estimate += K[1] * y;

    double P00_temp = P[0][0];
    double P01_temp = P[0][1];

    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;
}

void setup() {
    Serial.begin(9600);
    while ( !Serial ) delay(100);   // wait for native usb
    Serial.println(F("BMP280 test"));
    unsigned status;
    //status = bmp.begin(BMP280_ADDRESS_ALT, BMP280_CHIPID);
    status = bmp.begin();
    if (!status) {
        Serial.println(F("Could not find a valid BMP280 sensor, check wiring or "
                         "try a different address!"));
        Serial.print("SensorID was: 0x"); Serial.println(bmp.sensorID(),16);
        Serial.print("        ID of 0xFF probably means a bad address, a BMP 180 or BMP 085\n");
        Serial.print("   ID of 0x56-0x58 represents a BMP 280,\n");
        Serial.print("        ID of 0x60 represents a BME 280.\n");
        Serial.print("        ID of 0x61 represents a BME 680.\n");
        while (1) delay(10);
    }

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

    Serial.println("Calibrating MPU6050...");
    calibrateMPU6050();
    Serial.print("Gyro Z Bias: ");
    Serial.println(gyroZ_bias);

    // BMP280 settings
    bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
                    Adafruit_BMP280::SAMPLING_X2,
                    Adafruit_BMP280::SAMPLING_X16,
                    Adafruit_BMP280::FILTER_X16,
                    Adafruit_BMP280::STANDBY_MS_1);
    
    // Set initial altitude based on current reading
    initial_altitude = bmp.readAltitude(1013.25);
}

void loop() {
    mpu.update();
    double gyroZ = mpu.getGyroZ() - gyroZ_bias;

    // Calculate roll angle based on gyroZ readings
    static double roll_angle = 0;
    double dt = 0.1; // Assume a fixed time step for simplicity
    roll_angle += gyroZ * dt;
    if (roll_angle > 180) roll_angle = -180;

    // Simulate altitude change with a constant velocity of 2 m/s
    double measured_altitude = bmp.readAltitude(1013.25) - initial_altitude + velocity_estimate * dt;

    // Update Kalman filter for altitude
    kalmanUpdate(measured_altitude);

    // Print both simulated altitude and Kalman estimated altitude
    Serial.print("Simulated Altitude: ");
    Serial.print(measured_altitude);
    Serial.print(", Estimated Altitude: ");
    Serial.println(altitude_estimate);

    delay(100); // Adjust delay as needed
}