#include "App.hpp"
#include "BoardI2C.hpp"
//#include "MPU6050_6Axis_MotionApps20.h"
#define IGNORE_CHARGE true

BoardI2C i2c;
/*
BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial());
Sensors *sensors = nullptr; */

static Application *app = nullptr;

void setup() {
    Serial.begin(115200);
    app = new Application(new FlightDispatcher(
      new BluetoothDispatcher(new BluetoothSerial()),
      new FlightController(
        new Sensors(new Barometer(new GyverBME280(i2c)), new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
                    new Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()),
        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 1), 1),
        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 2), 2),
        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 3), 3), IGNORE_CHARGE)));
    /*sensors = new Sensors(
    new Barometer(new GyverBME280(i2c)), new MPU(new
  MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), new Kalman2DFilter(10.f, 1.f, 1.8f),
  new BatteryController()); sensors->measureBaseAltitudeSync();
  bluetoothDispatcher.initialize();
  Serial.println("System initialized");*/
}

void loop() {
    app->tick();
    /*
  sensors->tick();
  bluetoothDispatcher.tick();
  delay(1);*/
}