/**
 * @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/ManualControl/manualControl.h"

class DirectionChangeSignal : public DirectionChangeWrapper {
    public:
        DirectionChangeSignal(Navigation* navigation);
        ~DirectionChangeSignal();
        void action() override;

    private:
        Navigation* navigation;
};

/**
 * @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 {
            InsufficientAccuracy = -2,
            NoRoute = -1,
            None = 0,
            NavigationStarted,
            GetToStartPoint,
            SelfDrivingAvailable,
            SelfDriving,
            SelfDrivingRotate,
            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();

        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();
        void testRotate(int16_t degree);
        void endRotate();
        void switchLoopMode() { this->loopMode = !this->loopMode; }
        bool getLoopMode() const { return this->loopMode; }

    private:
        void init();
        void drive();
        void beginRotate();
        void rotate();
        void run() override;
        void checkButtonInput();
        void askNavigationForOrder();
        void selfDriving();
        void restartLoop();

        Navigation* navigation;
        CourseCorrection courseCorrection;
        RouteInfo routeInfo;
        State state = State::None;
        State lastState = State::None;
        Navigation::Status lastOrderStatus;
        DirectionChangeSignal* directionChangeSignal;

        bool updateDisplay = false;
        bool loopMode = false;

        uint8_t maxCourseDeviationBeforeAct = 5;
        uint16_t autopilotChangeDelayMillis = 500;
        uint32_t lastAutopilotChangeMillis = 0;

        int16_t rotationAimAzimuth;

        double minRemainingDistance = 0.25;
};

#endif // AUTOPILOT_H