// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com

#include "App.hpp"
#include "BoardI2C.hpp"

BoardI2C i2c;

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 RangingSensor(i2c),
                                new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
                                new Kalman2DFilter(10.f, 1.f, 1.8f),
                                new BatteryController(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN),
                                HeightSensor::RangeMeterOnly),
                    new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightControl"),  // height
                    new SavedPidRegulator(2.f, 12.f, 0.f, "YawControl"),  // yaw
                    new BrushedMotor(1, 2, 3, 4, 1),
                    new BrushedMotor(1, 2, 3, 4, 1),
                    new PIDController(0.6f, 3.5f, 0.03f,
                                      new BrushedMotor(1, 2, 3, 4, 3),
                                      "RollControl"),  // pitch
                    true)));
}

void loop() {
    app->tick();
}