#include "Filters/Kalman2DFilter.hpp"
#include "GyverBME280.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "RF/BluetoothDispatcher.hpp"
#include "Sensors/Barometer.hpp"
#include "Sensors/BatteryController.hpp"
#include "Sensors/MPU.hpp"
#include "Wire.h"
#include "board_pins.h"
#include <Arduino.h>

TwoWire i2c = TwoWire(0);
Barometer barometer(new GyverBME280(i2c));
MPU mpu(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c));
Kalman2DFilter kalman2d(10.f, 1.f, 1.8f);
BatteryController battery;
BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial());

void setup() {
    Serial.begin(115200);

    Serial.print("Ininitialize I2C...");
    Serial.println(i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000));

    Serial.print("Ininitialize BMP280...");
    Serial.println(barometer.initialize());

    Serial.print("Ininitialize MPU6050...");
    Serial.println(mpu.initialize());

    Serial.print("Ininitialize Bluetooth...");
    Serial.println(bluetoothDispatcher.initialize());

    battery.initialize();
    barometer.measureBaseAltitudeSync();

    Serial.println("System initialized");
}

void loop() {
    bool err;
    barometer.tick();
    if (mpu.tick(err) && !err) {
        float kalmanAltitude, ZVelocityAltitude;
        float barometerAltitude = barometer.flightHeight();
        kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
        //Serial.print(barometerAltitude);
    }
    bluetoothDispatcher.tick();
    delay(1);
}