/**
 * @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());
}

Point Navigation::addCurrentPosToRoute() {
    Point p;



    if (this->gps->location.isUpdated() && this->gps->location.isValid()) {
        p.lat = this->gps->location.lat();
        p.lon = this->gps->location.lng();
        if (MIN_DISTANCE_BETWEEN_POINTS <= 
            TinyGPSPlus::distanceBetween(p.lat, p.lon, this->lastPoint.lat, this->lastPoint.lon))
            this->route->addPointToRoute(p);
    }
    return p;
}