/**
 * @file autopilot.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains a class which use the navigate class to drive automaticaly
 * @version 0.1
 * @date 2022-02-02
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef AUTOPILOT_H
#define AUTOPILOT_H


#include "navigation.h"
#include "moveControl.h"
#include "driveModi/Modi/Autopilot/autopilotConfig.h"
#include "driveModi/Modi/ManualControl/manualControl.h"

/**
 * @brief This class use the navigate class to drive automaticaly
 * 
 * This class get the information from the navigate class. When an
 * object of this class is constructed the rover can be driven manually.
 * When the Rover is near to the first position of the Route, you can
 * switch to automatic drive.
 * 
 */
class Autopilot : public ManualControl {
    public:
        /**
         * @brief Construct a new Autopilot object
         * 
         * @param moveControl for ManualControl
         * @param navigation for route instructions
         */
        Autopilot(MoveControl* moveControl, Navigation* navigation);

        /**
         * @brief Calls runAutopilot or ManualControl loop
         * 
         * This function should be called every main loop. If self
         * driving is activated the function call run Autopilot. If
         * the delay is not reached the function returns immediately.
         * 
         * If self driving is not activated this functions calls the 
         * ManualControl loop additionally.
         */
        void loop();

        /**
         * @brief Manges the autoipilot
         * 
         * If the first Point is near to current location you can turn
         * the autopilot on.
         * Gets the course correction and decide what to do.
         */
        void runAutopilot();

        /**
         * @brief Get the Route Info object
         * 
         * @return RouteInfo 
         */
        RouteInfo getRouteInfo() const { return this->routeInfo; }

        /**
         * @brief Get the Course Correction object
         * 
         * @return CourseCorrection 
         */
        CourseCorrection getCourseCorrection() const { return this->courseCorrection; }

        /**
         * @brief Get the Navigation Started status
         * 
         * @return true 
         * @return false 
         */
        bool getNavigationStarted() const { return this->navigationStarted; }

        /**
         * @brief Get the Navigation Ended status
         * 
         * @return true 
         * @return false 
         */
        bool getNavigationEnded() const { return this->navigationEnded; }

        /**
         * @brief Get the Self Driving status
         * 
         * @return true 
         * @return false 
         */
        bool getSelfDriving() const { return this->selfDriving; }

        /**
         * @brief Get the Self Driving Available status
         * 
         * @return true 
         * @return false 
         */
        bool getSelfDrivingAvailable() const { return this->selfDrivingAvailable; }
        
        /**
         * @brief Tells if there are new informations to display
         * 
         * @return true 
         * @return false 
         */
        bool shouldUpdate();

    private:
        void setSelfDriving(bool val);
        void setSpeedInRelToDistance(double distance);
        void setRotInRelToDistance(int16_t course);

        Navigation* navigation;
        CourseCorrection courseCorrection;
        RouteInfo routeInfo;

        bool navigationStarted = false;
        bool navigationEnded = false;
        bool selfDriving = false;
        bool selfDrivingAvailable = false;
        bool updateDisplay = false;

        uint16_t lastMillis = 0;
        uint8_t delay = 40;
};

#endif // AUTOPILOT_H