#ifndef ROUTE_H
#define ROUTE_H

#include <cstdint>
#include <list>
#include <TinyGPS++.h>
#include <Arduino.h>

#include "hardware/motorControl.h"
#include "hardware/speedometer.h"
#include "debugMqtt.h"
#include "config.h"

struct Point{
    double lat = 0;
    double lon = 0;
    bool operator==(const Point& rhs) const {
        return this->lat == rhs.lat && this->lon == rhs.lon;
    }
};

class Route {
    public:
        Route();

        void runRoute();

        void addPointToRoute(Point point);
        void addCurrentLocationToRoute();
        void delRoute();
        void resetRoute();

        Point startRoute();
        Point getNextPoint();
        uint16_t getNumberOfPoints();

        static double getDis(Point point_1, Point point_2);


    private:
        bool nearlySameLocation(Point p1, Point p2);

        std::list<Point> points;
        std::list<Point>::iterator it;
        Point currentLocation;

        uint16_t count_points = 0;

        bool route_started = false;
        bool route_finished = false;

        TinyGPSPlus gps;

        DebugMqtt *debug;

};

#endif // ROUTE_H