#include "FlightDispatcher.hpp"

FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
                                   FlightController *flightController, bool loop_on_fail) {
    assert(bluetoothDispatcher != nullptr);
    assert(flightController != nullptr);

    _bluetoothDispatcher = bluetoothDispatcher;
    _flightController = flightController;

    bluetoothDispatcher->onNewDeviceConnected([this](BTAddress device) {
        this->_onNewDeviceConnected(device);
    });
    bluetoothDispatcher->onNewPackageReceived([this](char *package) {
        this->_onNewMessageReceived(package);
    });

    if (_bluetoothDispatcher->initialize()) {
        ESP_LOGI(_tag, "Bluetooth initialized successfully.");
    } else {
        ESP_LOGE(_tag, "Failed to initialize Bluetooth dispatcher!");
        while (loop_on_fail)
            ;
    }
    _telemetryTimer = millis();
}

FlightDispatcher::~FlightDispatcher() {
    delete _bluetoothDispatcher;
    delete _flightController;
}

void FlightDispatcher::tick() {
    _flightController->tick();
    _bluetoothDispatcher->tick();
    if (millis() - _telemetryTimer >= _telemetryTimeIntervalMS) {
        _sendTelemetry();
        _telemetryTimer = millis();
    }
}

void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
    auto currentState = _flightController->currentDeviceState();
    gson::string package;
    package.beginObj();
    package["batteryCharge"] = currentState.batteryCharge;
    package["flightHeight"] = currentState.flightHeight;
    package["status"] = currentState.status;
    package.endObj();
    package.end();
    package += "\r\n";
    auto pkg = package.s.c_str();
    _bluetoothDispatcher->sendPackage(pkg, strlen(pkg));
}

void FlightDispatcher::_onNewMessageReceived(char *package) {
    ESP_LOGI(_tag, "Received new package: %s", package);
    gson::Doc pkg;
    pkg.parse(package, _jsonMaxDepth);
    // TODO: process package
}

void FlightDispatcher::_sendTelemetry() {
    /* Notify mobile client about device state */
    auto state = _flightController->currentDeviceState();
    gson::string package;
    package.beginObj();
    package["batteryCharge"] = state.batteryCharge;
    package["flightHeight"] = state.flightHeight;
    package["status"] = state.status;
    package["y"] = state.mpuState.yaw;
    package["p"] = state.mpuState.pitch;
    package["r"] = state.mpuState.roll;
    package["zIn"] = state.mpuState.zInertial;
    package.endObj();
    package.end();
    package += "\r\n";

    _bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str())));
}