/**
 * @file driveManager.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains a class to switch the DriveModi
 * @version 0.1
 * @date 2021-12-14
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#ifndef DRIVE_MANAGER_H
#define DRIVE_MANAGER_H

//GPS Serial Pins
#define GPS_RX 17
#define GPS_TX 16
#define GPS_BAUD 9600

#include <Arduino.h>

#include "moveControl.h"
#include "driveModi/driveModi.h"
#include "navigation.h"

// All Drive Modi
#include "driveModi/Modi/ManualControl/manualControl.h"
#include "driveModi/Modi/CaptureRoute/captureRoute.h"
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "driveModi/Modi/ConsolControl/consolControl.h"
#include "driveModi/Modi/TestMode/testMode.h"

/**
 * @brief An enum to choose the DriveMode
 * 
 */
enum class Modi {   
    Off,
    ManualControl,
    CaptureRoute,
    Autopilot,
    ConsolControl,
    TestMode
};

class DriveManager {
    public:
        /**
         * @brief Construct a new Drive Manager object
         * 
         * Creates a Navigation object
         * 
         * @param moveControl 
         */
        DriveManager(MoveControl *moveControl);

        /**
         * @brief Destroy the Drive Manager object
         * 
         */
        ~DriveManager();

        /**
         * @brief Calls all loop functions
         * 
         * Calls the MoveControl, Navigation loop function. The
         * loop function from the DriveMode is called if it is set.
         * 
         * This function should be called every main loop.
         * 
         */
        void loop();

        /**
         * @brief Increment the DriveModi enum
         * Than calls changeModus
         * 
         * @see Modi
         */
        void nextModus();

        /**
         * @brief Change the DriveModi to a specific value
         * 
         * Sets MoveControl to a safe state, delete the last
         * DriveModi and than set the new DriveModi
         * 
         * @param modus 
         */
        void changeModus(Modi modus);

        /**
         * @brief Get the Navigation object
         * 
         * @return Navigation* 
         */
        Navigation* getNavigation() {return this->navigation;}

        /**
         * @brief Get the DriveModi Ptr object
         * 
         * This is a pointer to the Object of the current DriveMode.
         * If you know which DriveMode is active, you can cast this
         * pointer to it.
         * 
         * @see getDriveModi
         * @return DriveModi* 
         */
        DriveModi* getDriveModiPtr() { return this->currentModusPtr; }

        /**
         * @brief Get the DriveModi enum
         * 
         * @return Modi 
         */
        Modi getDriveModi() { return this->currentModus; }

    private:
        Modi currentModus = Modi::Off;
        MoveControl *moveControl;
        DriveModi *currentModusPtr = nullptr;
        Navigation* navigation;
};


#endif // DRIVE_MANAGER_H