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

class DirectionChangeSignal;

/**
 * @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 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; }
    Navigation *getNavigation() const { return this->navigation; }

    /**
     * @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();
    void afterActivate() override;

    Navigation *navigation = nullptr;
    CourseCorrection courseCorrection{0, 0};
    RouteInfo routeInfo{0, 0};
    State state = State::None;
    State lastState = State::None;
    Navigation::Status lastOrderStatus;
    DirectionChangeSignal *directionChangeSignal = nullptr;

    bool updateDisplay = false;
    bool loopMode = false;

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

    int16_t rotationAimAzimuth = 0;

    double minRemainingDistance = 0.25;
};

class DirectionChangeSignal : public DirectionChangeWrapper
{
public:
    DirectionChangeSignal(Autopilot *pilot);
    ~DirectionChangeSignal();
    void action() override;

private:
    Autopilot *pilot;
};

#endif // AUTOPILOT_H