/**
 * @file sensorData.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief
 * @version 0.1
 * @date 2023-09-02
 *
 * @copyright Copyright (c) 2023
 *
 */

#include "sensorData.h"

bool SensorData::outputStatusPrintPVTdata = false;
uint32_t SensorData::ubxUpdateTimeStatic = 0;
UBX_NAV_PVT_data_t *SensorData::ubxDataStatic = nullptr;

SensorData::SensorData()
{
    Component::loopDelay = SensorData::loopDelay;
}

SensorData::~SensorData()
{
    delete this->ntripClient;
}

void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password)
{
    this->ntripClient = new NTRIPClient(this->gnss, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
    this->ntripClient->gnssConfiguration();
    this->ntripClient->loop();
    this->ntripClient->setActivated(false);
    this->isNtripInit = true;
    this->addChildComponent(this->ntripClient);
}

void SensorData::enableGnss(SPIClass *spiPort, uint8_t csPin)
{
    this->gnss = new SFE_UBLOX_GNSS();
    if (this->gnss->begin(*spiPort, csPin, 4000000) == false)
    {
        std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl;
        while (true)
        {
        }
    }
    this->initGnss();
}

void SensorData::enableGnss()
{
    this->gnss = new SFE_UBLOX_GNSS();
    if (this->gnss->begin() == false)
    {
        std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
        while (true)
        {
        }
    }
    this->initGnss();
}

void SensorData::enableRealCompass()
{
    static constexpr byte address = 0x0d;

    this->realCompass = new QMC5883LCompass();
    // Init Compass
    Wire.beginTransmission(address);
    // TODO: describe Bytes !!!
    Wire.write(0x0b);
    Wire.write(0x01);
    Wire.endTransmission();
    this->realCompass->setMode(0x01, 0x0C, 0x10, 0X00);
    CalibrateCompass caliCompass(this->realCompass);
    caliCompass.loadData();
    caliCompass.useData();
}

void SensorData::enableCalcCompass()
{
    // TODO: !!! implementieren
}

void SensorData::enableGyroscope()
{
    this->gyroscope = new MPU6050();
    this->gyroscope->initialize();
    if (!this->gyroscope->testConnection())
    {
        std::cout << "SensorData::enableGyroscope: Gyroscope is not connected. Freeze!" << std::endl;
        while (true)
        {
        }
    }

    const uint8_t deviceStatus = this->gyroscope->dmpInitialize();

    // TODO: !!! MagicNumer 6x
    this->gyroscope->setXGyroOffset(220);
    this->gyroscope->setYGyroOffset(76);
    this->gyroscope->setZGyroOffset(-85);
    this->gyroscope->setZAccelOffset(1788);

    if (deviceStatus == 0)
    {
        this->gyroscope->CalibrateAccel(6);
        this->gyroscope->CalibrateGyro(6);
        // this->gyroscope->PrintActiveOffsets();
        this->gyroscope->setDMPEnabled(true);
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        std::cout << "SensorData::enableGyroscope: DMP Initialization failed (code" << static_cast<int>(deviceStatus) << "). Freeze!" << std::endl;
        while (true)
        {
        }
    }
}

CalcAzimuth::State SensorData::getCalcAzimuthState() const
{
    if (static_cast<bool>(this->calcCompass))
    {
        return this->calcCompass->getState();
    }
    return CalcAzimuth::State::Invalid;
}

NTRIPClientStates SensorData::getNtripState() const
{
    if (static_cast<bool>(this->ntripClient))
    {
        return this->ntripClient->getClientState();
    }
    return NTRIPClientStates::notAvailable;
}

void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
    static constexpr uint8_t stringSize = 32;

    if (!SensorData::outputStatusPrintPVTdata)
    {
        return;
    }

    const double latitude = ubxDataStruct->lat / 10000000.0;
    const double longitude = ubxDataStruct->lon / 10000000.0;
    const double altitude = ubxDataStruct->hMSL / 1000.0;

    const uint8_t fixType = ubxDataStruct->fixType;
    char fixTypeString[stringSize];
    if (fixType == 0)
    {
        strcpy(fixTypeString, static_cast<const char *>("None"));
    }
    else if (fixType == 1)
    {
        strcpy(fixTypeString, static_cast<const char *>("Dead Reckoning"));
    }
    else if (fixType == 2)
    {
        strcpy(fixTypeString, static_cast<const char *>("2D"));
    }
    else if (fixType == 3)
    {
        strcpy(fixTypeString, static_cast<const char *>("3D"));
    }
    else if (fixType == 3)
    {
        strcpy(fixTypeString, static_cast<const char *>("GNSS + Dead Reckoning"));
    }
    else if (fixType == 5)
    {
        strcpy(fixTypeString, static_cast<const char *>("Time Only"));
    }
    else
    {
        strcpy(fixTypeString, static_cast<const char *>("UNKNOWN"));
    }

    const uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
    char carrSolnString[stringSize];
    if (carrSoln == 0)
    {
        strcpy(carrSolnString, static_cast<const char *>("None"));
    }
    else if (carrSoln == 1)
    {
        strcpy(carrSolnString, static_cast<const char *>("Floating"));
    }
    else if (carrSoln == 2)
    {
        strcpy(carrSolnString, static_cast<const char *>("Fixed"));
    }
    else
    {
        strcpy(carrSolnString, static_cast<const char *>("UNKNOWN"));
    }

    const uint32_t hAcc = ubxDataStruct->hAcc;

    std::cout << "Lat: " << latitude
              << " Lng: " << longitude
              << " Alt: " << altitude << std::endl;

    std::cout << "Fix: " << fixTypeString
              << " Carrier Solution: " << carrSolnString
              << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
}

void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
    SensorData::printPVTdata(ubxDataStruct);

    SensorData::ubxDataStatic = ubxDataStruct;
    SensorData::ubxUpdateTimeStatic = millis();
}

void SensorData::setOutputStatusPrintPVTdata(bool status)
{
    SensorData::outputStatusPrintPVTdata = status;
}

void SensorData::run()
{
    if (static_cast<bool>(this->realCompass))
    {
        this->realCompass->read();
        this->realAzimuth = this->realCompass->getAzimuth();
    }

    if (static_cast<bool>(this->gyroscope) && this->gyroscope->dmpGetCurrentFIFOPacket(static_cast<uint8_t *>(this->gyroBuffer)))
    {
        this->gyroscope->dmpGetQuaternion(&this->quaternion, static_cast<uint8_t *>(this->gyroBuffer));
        this->gyroscope->dmpGetGravity(&this->gravity, &this->quaternion);
        this->gyroscope->dmpGetYawPitchRoll(static_cast<float *>(this->yawPitchRoll), &this->quaternion, &this->gravity);
    }
}

void SensorData::runAsChild()
{
    if (static_cast<bool>(this->gnss))
    {
        this->gnss->checkUblox();
        this->gnss->checkCallbacks();
        if (SensorData::ubxUpdateTimeStatic != this->lastUbxUpdate)
        {
            this->updateUbxData();
        }
    }
}

void SensorData::initGnss()
{
    const uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
    const uint8_t versionLow = this->gnss->getProtocolVersionLow();
    std::cout << "u-blox protocol version: " << static_cast<int>(versionHigh) << "." << static_cast<int>(versionLow) << std::endl;

    this->gnss->setSPIOutput(COM_TYPE_UBX);
    this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
    this->gnss->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
    this->gnss->setAutoPVTcallbackPtr(&(SensorData::savePVTdata));
    // SensorData::setOutputStatusPrintPVTdata(true);
    this->gnss->setNavigationFrequency(1);
    this->gnss->setAutoPVT(true);
}

void SensorData::updateUbxData()
{
    this->gnssData = SensorData::ubxDataStatic;
    this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic;

    Point::Coordinates coords{0, 0};
    coords.lat = this->gnssData->lat / 10000000.0;
    coords.lon = this->gnssData->lon / 10000000.0;

    this->currentPosition = Point(coords, this->gnssData->hAcc);
}