/**
 * @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"

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

    uint8_t versionHigh = this->gps->getProtocolVersionHigh();
    uint8_t versionLow = this->gps->getProtocolVersionLow();
    std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;

    this->gps->setI2COutput(COM_TYPE_UBX);
    this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);

    this->gps->setPortInput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);

    this->gps->getNavigationFrequency(1);

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

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

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

        delete this->host;
        delete this->mountPoint;
        delete this->user;
        delete this->password;
    }

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

void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
    this->host = new char[host.length() + 1];
    this->mountPoint = new char[mountPoint.length() + 1];
    this->user = new char[user.length() + 1];
    this->password = new char[password.length() + 1];

    strcpy(this->host, host.c_str());
    strcpy(this->mountPoint, mountPoint.c_str());
    strcpy(this->user, user.c_str());
    strcpy(this->password, password.c_str());
    this->port = port;

    this->ntripClient = new NTRIPClient;
    this->isNtripInit = true;

    std::cout << "Requesting SourceTable." << std::endl;
    if(this->ntripClient->reqSrcTbl(this->host, (int) this->port)){
        char buffer[512];
        delay(5);
        while(this->ntripClient->available()) {
            this->ntripClient->readLine(buffer,sizeof(buffer));
            std::cout << buffer << std::endl;
        }   
    } else {
        std::cout << "SourceTable request error" << std::endl;
    }
    std::cout << "Requesting SourceTable is OK" << std::endl;
    this->ntripClient->stop(); //Need to call "stop" function for next request.
  
    std::cout << "Requesting MountPoint's Raw data" << std::endl;
    if(!this->ntripClient->reqRaw(this->host, this->port, this->mountPoint,
                                  this->user, this->password)){
        delay(15000);
        ESP.restart();
    }
    std::cout << "Requesting MountPoint is OK" << std::endl;
}

void Navigation::loop() {
    //FIXME: Hier sollte funktion zum aktualisieren des GPS Moduls stehen

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

    uint8_t rtcmData[512 * 4];
    uint16_t rtcmCount = this->ntripClient->available();
    if (rtcmCount) {
        this->ntripClient->readBytes(rtcmData, rtcmCount);
        this->gps->pushRawData(rtcmData, rtcmCount);
    }
}

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

    Point currentPos = this->currentLocation();
    double targetCourse = Navigation::courseTo(currentPos, this->targetPoint);
    double distance = Navigation::distanceBetween(currentPos, 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() {
    Point p = this->currentLocation();
    if (!p.isValid()) { return false; }

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

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

Point Navigation::currentLocation() {
    Point p;
    //TODO: check if the position is possible the true location
    if (this->gps) {
        p.lat = this->gps->getLatitude();
        p.lon = this->gps->getLongitude();
    }
    return p;
}

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

double Navigation::distanceBetween(Point p1, Point p2) {
    if (p1.isValid() && p2.isValid())
        return NavigationUtils::distanceBetween(p1, p2);
    return -1;
}

double Navigation::courseTo(Point p1, Point p2) {
    if (p1.isValid() && p2.isValid())
        return NavigationUtils::courseTo(p1, p2);
    return -1;
}