#include "FlightController.hpp"

FlightController::FlightController(Sensors *sensors, PIDController *pid1,
                                   PIDController *pid2, PIDController *pid3, bool unlimitedBattery) {
    assert(sensors != nullptr);
    assert(pid1 != nullptr);
    assert(pid2 != nullptr);
    assert(pid3 != nullptr);
    _sensors = sensors;
    _unlimitedBattery = unlimitedBattery;

    _status = DeviceStatus::Idle;
    _updateBatteryCharge();
    ESP_LOGI(_tag, "Flight controller initialized");
}

FlightController::~FlightController() {
    delete _sensors;
    delete _pid1;
    delete _pid2;
    delete _pid3;
}

void FlightController::tick() {
    bool hasUpdates = _sensors->tick();
    if (hasUpdates) {
        _updateBatteryCharge();
    }
    switch (_status) {
    case DeviceStatus::Idle: break;
    case DeviceStatus::ChargeRequired: break;
    case DeviceStatus::IsPreparingForTakeoff: break;
    case DeviceStatus::IsBoarding:
        /* TODO: implement boarding */
        break;
    case DeviceStatus::IsFlying:
        /* TODO: implement flying */
        break;
    }
}

ReportedDeviceState FlightController::currentDeviceState() const {
    return { .batteryCharge = _sensors->batteryCharge(),
             .flightHeight = _sensors->flightHeight(),
             .status = _status,
             .mpuState = _sensors->mpuData() };
}

void FlightController::startTakeoff(OnMeasuringFinishedCb onTakeoffStarted) {
    /* Start preparation for takeoff */
    _updateBatteryCharge();
    if (_status == DeviceStatus::ChargeRequired)
        return;

    _status = DeviceStatus::IsPreparingForTakeoff;
    _sensors->onMeasuaringAltitudeFinished([this]() {
        if (_status == DeviceStatus::IsPreparingForTakeoff) {
            // Preparation for takeoff is done
            _takeoff();
        }
    });
    _sensors->measureBaseAltitudeAsync();
}

void FlightController::startBoarding() {
    if (_status == DeviceStatus::IsFlying) {
        _status = DeviceStatus::IsBoarding;
    }
}

void FlightController::_updateBatteryCharge() {
    if(_unlimitedBattery) return;
    if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
        if (_status == DeviceStatus::IsFlying or _status == DeviceStatus::IsBoarding) {
            // TODO: board the device
            _status = DeviceStatus::IsBoarding;
        } else {
            _status = DeviceStatus::ChargeRequired;
        }
    }
}

void FlightController::_takeoff() {
    // TODO: implement takeoff
    _status = DeviceStatus::IsFlying;
    if(_onTakeoffStarted) {
        _onTakeoffStarted();
    }
}

void FlightController::_boarding() {
    if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
        // TODO: stop motors and setpoints
        _status = DeviceStatus::Idle;
    } else {
        // TODO: implement boarding
    }
}

void FlightController::startImuCalibration() {
    _sensors->onMpuCalibrationFinished([this]() {
        this->_status = DeviceStatus::Idle;
    });
}