/**
 * @file navigation.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains a class which navigate an object by the given route
 * @version 0.1
 * @date 2022-01-10
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef NAVIGATION_H
#define NAVIGATION_H

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

#include "route.h"

/**
 * @brief The minimal distance between Points
 * 
 * This is a very critical option. It have to be
 * around the half accuracy of the positioning system
 * 
 */
#define MIN_DISTANCE_BETWEEN_POINTS 1

/**
 * @brief This struct inherits the result of the navigation
 * 
 * The drive get objects of this struct and should
 * correct the direction in dependency on this.
 * 
 */
struct CourseCorrection {
    int16_t correction;
    double distance;
};

/**
 * @brief This class navigate an object
 * 
 * The class use the given Route and the gps device
 * to tell the driver in which direction he have to
 * be drive and the distance to the next checkpoint.
 * 
 */
class Navigation {
    public:
        /**
         * @brief Construct a new Navigation object
         * 
         * @param rx pin to the gps device
         * @param tx pin to the gps device
         * @param route with which to navigate
         */
        Navigation(uint8_t rx, uint8_t tx, Route* route = nullptr);

        /**
         * @brief Destroy the Navigation object
         * 
         */
        ~Navigation();

        /**
         * @brief Decodes new GPS information
         * 
         * Should be called ever main loop.
         */
        void loop();

        /**
         * @brief creates a new empty route
         * 
         */
        void newRoute();

        /**
         * @brief Tries to start the route
         * 
         * For example the route can not be started 
         * if there are no Points or wrong Points.
         * 
         * @return true route is started
         * @return false route can not be started
         */
        bool startNavigation();

        /**
         * @brief Get the Course Correction object
         * 
         * This should be called by the driver to get new instructions.
         * 
         * @return CourseCorrection 
         */
        CourseCorrection getCourseCorrection();

        /**
         * @brief Tries to add the current Position to the route
         * 
         * This can be go wrong if there is no valid GPS signal
         * 
         * @return true successful added point
         * @return false no point added to route
         */
        bool addCurrentPosToRoute();

        /**
         * @brief Returns the GPS object
         * 
         * @return TinyGPSPlus* 
         */
        TinyGPSPlus* getGPS() { return this->gps; }

        /**
         * @brief Get the Route Info object
         * 
         * This object contains information about the route.
         * For example the stored points.
         * 
         * @return RouteInfo 
         */
        RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }

    private:
        /**
         * @brief Returns the current Location
         * 
         * @return Point 
         */
        Point currentLocation();

        /**
         * @brief Set the next point as target
         * 
         * @return true
         * @return false 
         */
        bool nextPoint();

        /**
         * @brief Set the target point
         * 
         * @param target 
         * @return true 
         * @return false 
         */
        bool setTargetPoint(Point target);

        /**
         * @brief Calculate the distance between to points
         * 
         * @param p1 point 1
         * @param p2 point 2
         * @return double distance in meter
         */
        static double distanceBetween(Point p1, Point p2);

        /**
         * @brief Calculates the course from point1 to point2
         * 
         * @param p1 point 1
         * @param p2 point 2
         * @return double degree north = 0° west = 270°
         */
        static double courseTo(Point p1, Point p2);

        TinyGPSPlus* gps;
        Route* route = nullptr;

        Point lastPoint;
        Point targetPoint;

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

#endif // NAVIGATION_H