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

#ifndef NAVIGATION_H
#define NAVIGATION_H

#include <TinyGPS++.h>
#include <Arduino.h>
#include <iostream>

#include "route.h"

#define MIN_DISTANCE_BETWEEN_POINTS 1

struct CourseCorrection {
    int16_t correction;
    double distance;
};
class Navigation {
    public:
        Navigation(uint8_t rx, uint8_t tx);
        ~Navigation();
        void loop();

        void newRoute();

        bool startNavigation();
        CourseCorrection getCourseCorrection();

        bool addCurrentPosToRoute();

        TinyGPSPlus* getGPS() { return this->gps; }

    private:
        Point currentLocation();
        bool nextPoint();
        bool setTargetPoint(Point target);
        static double distanceBetwenn(Point p1, Point p2);
        static double courseTo(Point p1, Point p2);

        TinyGPSPlus* gps;
        Route* route;

        Point lastPoint;
        Point targetPoint;

        bool navigationStarted = false;
        bool navigationFinished = false;
};

#endif // NAVIGATION_H