/**
 * @file route.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains a class which hold multiple Points
 * @version 0.1
 * @date 2022-01-31
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef ROUTE_H
#define ROUTE_H

#include <Arduino.h>

#include <cstdint>
#include <list> 
#include <cmath>   

#define ROUTE_DEGREE_TO_RADIANT 0.01745
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
#define ROUTE_PI 3.14159265358979323846

/**
 * @brief A to handle points on the earth
 * 
 * The points inherits latidue and longitude as doubles
 * 
 */
class Point{
    public:
        struct Coordinates {
            double lon;
            double lat;

            bool operator==(const Coordinates rhs) const {
                return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
            }
        };

        enum PointAccuracy {
            none,
            fourDigOfCM,
            threeDigOfCM,
            twoDigOfCM,
            oneDigOfCM
        };


        /**
         * @brief Construct a new Point object
         * 
         * @param lat latitude
         * @param lon longitude
         */
        Point(double lat, double lon, uint32_t horizontalAccuracy = 0);
        Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0);
        Point(Coordinates coords, uint32_t horizontalAccuracy = 0);
        Point();

        /**
         * @brief checks if to points are equal
         * 
         * @param rhs 
         * @return true 
         * @return false 
         */
        bool operator==(const Point& rhs) const;

        /**
         * @brief Checks if the Point is initalized.
         * 
         * @return true 
         * @return false 
         */
        bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
        bool isValid() const { return (this->accuracy > 0) ? true : false; }

        double distanceTo(const Coordinates& point) const;
        double distanceTo(const Point& point) const;
        double courseTo(const Coordinates& point) const;
        double courseTo(const Point& point) const;

        double getLongitude() const { return this->coordinates.lon; }
        double getLatitude() const { return this->coordinates.lat; }
        Coordinates getCoordinates() const { return this->coordinates; }

        PointAccuracy getAccuracy() { return this->accuracy; }

    private:
        void init(uint32_t horizontalAccuracy);

        PointAccuracy accuracy = PointAccuracy::none;
        Coordinates coordinates;

        uint32_t creationTime = 0;
};

/**
 * @brief Holds some route information
 * 
 */
struct RouteInfo{
    /**
     * @brief Selected number of Points
     */
    uint16_t currentPoint;

    /**
     * @brief Total points stored in route
     */
    uint16_t totalPoints;
};

/**
 * @brief A class to manage multiple points
 * 
 * The list of point presents a route which can be driven
 */
class Route {
    public:
        /**
         * @brief Construct a new Route object
         */
        Route();

        /**
         * @brief Adds a point to the list
         * 
         * @param point 
         */
        void addPointToRoute(Point point);

        /**
         * @brief Select the first point as target
         * 
         * @return Point 
         */
        Point startRoute();

        /**
         * @brief Get the next point and set it as target
         * 
         * @return Point is zero if there are no more Points
         */
        Point getNextPoint();

        /**
         * @brief Get the Route Info object
         * 
         * @return RouteInfo 
         */
        RouteInfo getRouteInfo();

    private:
        uint16_t currentPoint = 0;

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

};

#endif // ROUTE_H