/**
 * @file navigation.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the implementation of the class Navigation
 * @version 0.1
 * @date 2022-01-31
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "navigation.h"

bool Navigation::outputStatusPrintPVTdata = false;

Navigation::Navigation(Route* route) {
    this->gps = new SFE_UBLOX_GNSS();
    if (this->gps->begin() == false) {
        std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
        while (1);
    }
    this->init(route);
}

Navigation::Navigation(SPIClass* spiPort, uint8_t csPin, Route* route) {
    this->gps = new SFE_UBLOX_GNSS();
    if (this->gps->begin(*spiPort, csPin, 4000000) == false) {
        std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl;
        while (1);
    }
    this->init(route);
}

void Navigation::init(Route* route) {
    uint8_t versionHigh = this->gps->getProtocolVersionHigh();
    uint8_t versionLow = this->gps->getProtocolVersionLow();
    std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;

    // std::cout << "Set GNSS Module to factory settings... ";
    // this->gps->factoryReset();
    // delay(5000);
    // std::cout << "Complete" << std::endl;

    this->gps->setSPIOutput(COM_TYPE_UBX);
    this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
    this->gps->setAutoPVTcallbackPtr(&(Navigation::printPVTdata));
    // Navigation::setOutputStatusPrintPVTdata(true);
    this->gps->setNavigationFrequency(1);
    this->gps->setAutoPVT(true);

    if (route)
        this->route = route;
    else
        this->route = new Route();

    this->ntripClient = new NTRIPClient();
}

Navigation::~Navigation() {
    delete this->gps;

    if (this->isNtripInit)
        delete this->ntripClient;

    if (this->route)
        delete this->route;
}

void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
    this->ntripClient->init(this->gps, 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;
}

void Navigation::loop() {
    static uint32_t lastTime = 0;

    this->gps->checkCallbacks();
    if (this->gps->getPVT() && this->gps->getGnssFixOk()) {
        this->updateCurrentLocation();
        std::cout << "duration between: " << millis() - lastTime << std::endl;
        lastTime = millis();
    } else if (millis() - lastTime > 20000 && !this->gps->getGnssFixOk()) {
        std::cout << "No Data from ZED arrived since a long time. Navigation::loop" << std::endl;
        this->gps->flushPVT();
        this->gps->softwareResetGNSSOnly();
        lastTime = millis();

        //std::cout
        //     << "checkUblox: " << this->gps->checkUblox() << "  -|-  "
        //     << "getGnssFixOk: " << this->gps->getGnssFixOk() << "  -|-  "
        //     << "getHWstatus: " << this->gps->getHWstatus() << "  -|-  "
        //     << "isConnected: " << this->gps->isConnected() << "  -|-  "
        //     << std::endl;
    }

    if (this->isNtripInit)
        this->ntripClient->loop();

    if (millis() - this->lastMillis < this->timeToWait)
        return;
}

void Navigation::newRoute() {
    if (this->route)
        delete this->route;
    this->route = new Route();
}

bool Navigation::startNavigation() {
    Point newTargetPoint = this->route->startRoute();
    this->navigationStarted = this->setTargetPoint(newTargetPoint);
    if (this->navigationStarted)
        this->navigationFinished = false;
    return this->navigationStarted;
}

CourseCorrection Navigation::getCourseCorrection() {
    CourseCorrection courseCorrection = {0, 0};

    this->updateCurrentLocation();
    double targetCourse = this->currentPosition.courseTo(this->targetPoint);
    double distance = this->currentPosition.distanceTo(this->targetPoint);

    if (targetCourse < 0 || distance < 0 || this->navigationFinished)
        return courseCorrection;

    // Check if I need a new Point
    if (distance < MIN_DISTANCE_BETWEEN_POINTS) {
        bool goOn = this->nextPoint();
        if (!goOn) {
            this->navigationFinished = true;
            this->navigationStarted = false;
            return courseCorrection; // End of navigation
        }
    }

    //FIXME: next line
    double currentCourse = 0;
    //double currentCourse = this->gps->course.deg();
    int16_t correction = currentCourse - targetCourse;
    if (correction > 180)
        correction -= 360;
    else if (correction < -180)
        correction += 360;

    courseCorrection.correction = correction;
    courseCorrection.distance = distance;
    return courseCorrection;
}

bool Navigation::addCurrentPosToRoute() {
    // FIXME: Should I use updateCurrentLocation here? think overall
    this->updateCurrentLocation();
    if (!this->currentPosition.isValid()) { return false; }

    // First Point
    if (this->route->getRouteInfo().totalPoints == 0) {
        this->route->addPointToRoute(this->currentPosition);
        this->lastPoint = this->currentPosition;
        return true;
    }

    // Ever Point after the first
    if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPoint)) {
        this->route->addPointToRoute(this->currentPosition);
        this->lastPoint = this->currentPosition;
        return true;
    }
    return false;
}

void Navigation::updateCurrentLocation() {
    //TODO: check if the position is possible the true location
    if (this->gps) {
        this->currentPosition.lat = this->gps->getLatitude();
        this->currentPosition.lon = this->gps->getLongitude();
    }
}

bool Navigation::nextPoint() {
    if (!this->navigationStarted)
        return false;
    Point newTargetPoint = this->route->getNextPoint();
    return this->setTargetPoint(newTargetPoint);
}

bool Navigation::setTargetPoint(Point target) {
    if (target.isValid()) {
        this->targetPoint = target;
        return true;
    }
    return false;
}

void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
    if (!Navigation::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 Navigation::setOutputStatusPrintPVTdata(bool status) {
    Navigation::outputStatusPrintPVTdata = status;
}