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

#include "sensors.h"

Sensors::Sensors() {
    this->sensorData = new SensorData(this);
}

void Sensors::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 Sensors::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 Sensors::enableCompass() {
    this->compass = new QMC5883LCompass();
    // Init Compass
    Wire.beginTransmission(0x0d);
    Wire.write(0x0b);
    Wire.write(0x01);
    Wire.endTransmission();
    this->compass->setMode(0x01,0x0C,0x10,0X00);
    CalibrateCompass caliCompass(this->compass);
    caliCompass.loadData();
    caliCompass.useData();
}

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

void Sensors::run() {
    this->compass->read();
    this->realAzimuth = this->compass->getAzimuth();
}

void Sensors::runAsChild() {
    this->gnss->checkUblox();
    this->gnss->checkCallbacks();

    if (Sensors::newData)
        Sensors::newData = false;
}

void Sensors::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(&(Sensors::savePVTdata));
    // Sensors::setOutputStatusPrintPVTdata(true);
    this->gnss->setNavigationFrequency(1);
    this->gnss->setAutoPVT(true);
}

void Sensors::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
    if (!Sensors::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 Sensors::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
    Sensors::printPVTdata(ubxDataStruct);

    Sensors::newData = true;
    Sensors::ubxDataStatic = ubxDataStruct;
    Sensors::ubxUpdateTimeStatic = millis();
}