/**
 * @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;
}

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);
    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::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();
    }
}

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);
}