// 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 "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;
    _pid1 = pid1;
    _pid2 = pid2;
    _pid3 = pid3;

    _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::IsImuCalibration: break;
    case DeviceStatus::IsBoarding:
        /* TODO: implement boarding */
        break;
    case DeviceStatus::IsFlying:
        /* TODO: implement flying */
        if (hasUpdates) {
            _pid1->tick();
            _pid2->tick();
            _pid3->tick();
        }
        break;
    }
}

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

void FlightController::startTakeoff() {
    /* Start preparation for takeoff */
    _updateBatteryCharge();
    if (_status != DeviceStatus::Idle)
        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) {
            startBoarding();
        } 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() {
    if (_status != DeviceStatus::Idle)
        return;
    _sensors->onMpuCalibrationFinished([this]() {
        this->_status = DeviceStatus::Idle;
    });
}

void FlightController::moveToFlightHeight(float newFlightHeight) {
    // TODO: implement setting flight height
}

void FlightController::newFlightHeightStickPosition(int16_t newFlightHeightStickPostition) {
    // TODO: implement stick input
}

void FlightController::newYawStickPosition(int16_t newYawStickPostition) {
    // TODO: implement stick input
}

void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
    // TODO: implement stick input
}

void FlightController::newRotor1Duty(uint32_t rotor1Duty) {
    if (_status == DeviceStatus::Idle) {
        _pid1->setRotorDuty(rotor1Duty);
    }
}

void FlightController::newRotor2Duty(uint32_t rotor2Duty) {
    if (_status == DeviceStatus::Idle) {
        _pid2->setRotorDuty(rotor2Duty);
    }
}

void FlightController::newRotor3Duty(uint32_t rotor3Duty) {
    if (_status == DeviceStatus::Idle) {
        _pid3->setRotorDuty(rotor3Duty);
    }
}

void FlightController::stopAllRotors() {
    _pid1->setRotorDuty(0);
    _pid2->setRotorDuty(0);
    _pid3->setRotorDuty(0);
}

void FlightController::setPid1Params(float p, float i, float d) {
    if (_status == DeviceStatus::Idle) {
        _pid1->Kp = p;
        _pid1->Ki = i;
        _pid1->Kd = d;
        _pid1->save();
    }
}

void FlightController::setPid2Params(float p, float i, float d) {
    if (_status == DeviceStatus::Idle) {
        _pid2->Kp = p;
        _pid2->Ki = i;
        _pid2->Kd = d;
        _pid1->save();
    }
}

void FlightController::setPid3Params(float p, float i, float d) {
    if (_status == DeviceStatus::Idle) {
        _pid3->Kp = p;
        _pid3->Ki = i;
        _pid3->Kd = d;
        _pid1->save();
    }
}

PidSettings *FlightController::pidSettings() const {
    PidSettings pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
    PidSettings pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
    PidSettings pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
    static PidSettings settings[] = { pid1, pid2, pid3 };
    return settings;
}