/**
 * @file senors.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() {
    this->loopDelay = 50;
}

SensorData::~SensorData() {
    if (this->ntripClient)
        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->gpsConfiguration();
    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 (1);
    }
    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 (1);
    }
    this->initGnss();
}

void SensorData::enableRealCompass() {
    this->realCompass = new QMC5883LCompass();
    // Init Compass
    Wire.beginTransmission(0x0d);
    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() {

}

void SensorData::enableGyroskop() {
    this->gyroskop = new MPU6050();
    this->gyroskop->initialize();
    if (!this->gyroskop->testConnection()) {
        std::cout << "SensorData::enableGyroskop: Gyroskop is not conntected. Freeze!" << std::endl;
        while (true);
    }

    uint8_t deviceStatus = this->gyroskop->dmpInitialize();

    this->gyroskop->setXGyroOffset(220);
    this->gyroskop->setYGyroOffset(76);
    this->gyroskop->setZGyroOffset(-85);
    this->gyroskop->setZAccelOffset(1788);
    
    if (deviceStatus == 0) {
        this->gyroskop->CalibrateAccel(6);
        this->gyroskop->CalibrateGyro(6);
        this->gyroskop->PrintActiveOffsets();
        this->gyroskop->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::enableGyroskop: DMP Initialization failed (code" << (int) deviceStatus <<"). Freeze!" << std::endl;
        while (true);
    }
}

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

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

void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
    if (!SensorData::outputStatusPrintPVTdata)
        return;

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

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

    uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
    char carrSolnString[16];
    if (carrSoln == 0)
        strcpy(carrSolnString, "None");
    else if (carrSoln == 1)
        strcpy(carrSolnString, "Floating");
    else if (carrSoln == 2)
        strcpy(carrSolnString, "Fixed");
    else
        strcpy(carrSolnString, "UNKNOWN");

    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 (this->realCompass) {
        this->realCompass->read();
        this->realAzimuth = this->realCompass->getAzimuth();
    }

    if (this->gyroskop
        && this->gyroskop->dmpGetCurrentFIFOPacket(this->gyroBuffer)) 
    {
        this->gyroskop->dmpGetQuaternion(&this->quaternion, this->gyroBuffer);
        this->gyroskop->dmpGetGravity(&this->gravity, &this->quaternion);
        this->gyroskop->dmpGetYawPitchRoll(this->yawPitchRoll, &this->quaternion, &this->gravity);
    }
}

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

void SensorData::initGnss() {
    uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
    uint8_t versionLow = this->gnss->getProtocolVersionLow();
    std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(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;
    coords.lat = this->gnssData->lat / 10000000.0;
    coords.lon = this->gnssData->lon / 10000000.0;

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