/**
 * @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:
        enum State {
            NoNtrip = -2,
            NoRoute = -1,
            None = 0,
            NavigationStarted,
            GetToStartPoint,
            SelfDrivingAvailable,
            SelfDriving,
            TargetReached
        };

        /**
         * @brief Construct a new Autopilot object
         * 
         * @param moveControl for ManualControl
         * @param navigation for route instructions
         */
        Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);

        /**
         * @brief Destroy the Autopilot object
         * 
         * Disconnect the NTRIP-Client
         */
        ~Autopilot();

        /**
         * @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 loopDelayMillis 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 autopilot
         * 
         * 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();

        void restart();

        /**
         * @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 State object
         * 
         * @return State 
         */
        State getState() const { return this->state; }
        
        /**
         * @brief Tells if there are new informations to display
         * 
         * @return true 
         * @return false 
         */
        bool shouldUpdate();

    private:
        void init();
        void checkNtrip();
        void setSpeedInRelToDistance(double distance);
        void setRotInRelToDistance(int16_t course);

        Navigation* navigation;
        CourseCorrection courseCorrection;
        RouteInfo routeInfo;
        State state = State::None;
        NTRIPClientStates lastState;

        bool updateDisplay = false;

        uint8_t loopDelayMillis = 40;
        uint8_t ntripReconnectAttemps = 0;
        uint8_t ntripReconnectMaxAttemps = 10;
        uint16_t displayUpdateDelayMillis = 1000;
        uint16_t reconnectNtripDelayMillis = 1000;
        uint16_t autopilotChangeDelayMillis = 500;
        uint32_t displayUpdateLastMillis = 0;
        uint32_t loopLastMillis = 0;
        uint32_t reconnectNtripLastMillis = 0;
        uint32_t lastAutopilotChangeMillis = 0;
};

#endif // AUTOPILOT_H