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

#include "navigation.h"

Navigation::Navigation(uint8_t rx, uint8_t tx) {
    this->gps = new TinyGPSPlus;
    Serial1.begin(9600, SERIAL_8N1, rx, tx);
}

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

void Navigation::loop() {
    while (Serial1.available())
        gps->encode(Serial1.read());
}

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::distanceBetwenn(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
        }
    }

    double currentCourse = 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 = currentLocation();
    if (!p.isValid()) { return false; }
    if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetwenn(p, this->lastPoint)) {
        this->route->addPointToRoute(p);
        this->lastPoint = p;
        return true;
    }
    return false;
}

Point Navigation::currentLocation() {
    Point p;
    if (this->gps->location.isValid()) {
        p.lat = this->gps->location.lat();
        p.lon = this->gps->location.lng();
    }
    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::distanceBetwenn(Point p1, Point p2) {
    if (p1.isValid() && p2.isValid())
        return TinyGPSPlus::distanceBetween(p1.lat, p1.lon, p2.lat, p2.lon);
    return -1;
}

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