// 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 "Sensors.hpp"

Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
                 BatteryController *battery, volatile bool loop_on_fail) {
    assert(barometer != nullptr);
    assert(mpu != nullptr);
    assert(filter != nullptr);
    assert(battery != nullptr);

    _barometer = barometer;
    _mpu = mpu;
    _2d_filter = filter;
    _battery = battery;

    ESP_LOGI(_tag, "Initialize BMP280...");
    if (_barometer->initialize()) {
        ESP_LOGI(_tag, "BMP280 initialized");
    } else {
        ESP_LOGI(_tag, "BMP280 initialization failed!");
        while (loop_on_fail)
            ;
    }

    ESP_LOGI(_tag, "Initialize MPU6050...");
    if (_mpu->initialize()) {
        ESP_LOGI(_tag, "MPU6050 initialized");
    } else {
        ESP_LOGI(_tag, "MPU6050 initialization failed!");
        while (loop_on_fail)
            ;
    }

    _battery->initialize();
    ESP_LOGI(_tag, "BatteryController initialized");
    ESP_LOGI(_tag, "Sensors initialized");
}

Sensors::~Sensors() {
    delete _barometer;
    delete _mpu;
    delete _2d_filter;
    delete _battery;
}

void Sensors::measureBaseAltitudeSync(void) {
    _barometer->measureBaseAltitudeSync();
}

void Sensors::measureBaseAltitudeAsync(void) {
    _barometer->measureBaseAltitudeAsync();
}

float Sensors::rawFlightHeight(void) const {
    /* Returns flight height from barometer immediatly */
    return _barometer->flightHeight();
}

float Sensors::flightHeight(void) const {
    /* Returns flight height from cache immediatly */
    if (_flightHeightFromBarometer == 0.f) {
        ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
        return rawFlightHeight();
    } else {
        return _flightHeightFromBarometer;
    }
}

MpuData Sensors::mpuData() const {
    if (_mpuData.ax == 0.f and _mpuData.gravity == 0.f) {
        ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
    }
    return _mpuData;
}

bool Sensors::tick(void) {
    /* Super loop iteraion */
    /* Return true on update */
    _barometer->tick();
    bool err;
    bool isMpuDataReady = _mpu->tick(err);
    if (isMpuDataReady and !err) {
        _2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(),
                           _flightHeightFromBarometer, _zVelocityAltitude);
        _mpuData = { .ax = _mpu->ax(),
                     .ay = _mpu->ay(),
                     .az = _mpu->az(),
                     .gx = _mpu->gx(),
                     .gy = _mpu->gy(),
                     .gz = _mpu->gz(),
                     .yaw = _mpu->yaw(),
                     .pitch = _mpu->pitch(),
                     .roll = _mpu->roll(),
                     .gravity = _mpu->gravityZ(),
                     .zInertial = _mpu->accZInertial() };
        return true;
    } else if (err) {
        ESP_LOGE(_tag, "Failed to get DMP data!");
    }
    return false;
}

int Sensors::batteryCharge(void) const {
    return _battery->percent();
}

void Sensors::onMpuCalibrationFinished(OnCalibrationFinishedCb callback) {
    _mpu->onCalibrationFinished(callback);
}

void Sensors::startMpuCalibration() {
    _mpu->startCalibration();
}

void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) {
    _barometer->onMeasuaringHeightFinished(callback);
}