/**
 * @file testMode.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains a class to test different functions from the rover.
 * @version 0.1
 * @date 2022-09-08
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#ifndef TEST_MODE_H 
#define TEST_MODE_H

#include <iostream>

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


/**
 * @brief Test different functions from the rover.
 * 
 * You can test engine, engine controller, light and speed meter.
 */
class TestMode : public DriveModi {
    public:
        /**
         * @brief Different states to test the engine
         * 
         */
        enum Maneuver {
            None,
            LeftEngine,
            RightEngine,
            BothEngine,
            Turn,
            Drive
        };

        bool drive(int16_t cm = 0, int16_t degree = 0);

        /**
         * @brief Sets the left Engine to a specific power value.
         * 
         * This start a maneuver with the given time.
         * 
         * @param powerPercentage from 0% to 100%
         * @param seconds time to run the engine
         * @return true success
         * @return false failure
         */
        bool leftEngine(int16_t powerPercentage, int16_t seconds);

        /**
         * @brief See leftEngine
         * 
         * @param powerPercentage 
         * @param seconds 
         * @return true 
         * @return false 
         */
        bool rightEngine(int16_t powerPercentage, int16_t seconds);

        /**
         * @brief See leftEngine
         * 
         * @param powerPercentage 
         * @param seconds 
         * @return true 
         * @return false 
         */
        bool bothEngine(int16_t powerPercentage, int16_t seconds);

        /**
         * @brief Abort the running maneuver.
         * 
         * Stops all movement. This is the only possibility to cancel a maneuver
         * before the time is up. 
         */
        void abortManeuver();

        uint8_t getRemainingManeuverTime() const;

        /**
         * @brief Returns true if a maneuver is running
         * 
         * @return true 
         * @return false 
         */
        bool getBusy() const { return this->busy; }
        Maneuver getManeuver() const { return this->maneuver; }
        Speedometer* getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); }
        Speedometer* getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); }

    private:
        void run() override;
        bool engineInit(int16_t powerPercentage, int16_t seconds);

        Navigation* navigation;
        Maneuver maneuver = Maneuver::None;
        int16_t maneuverValueOne = 0;
        int16_t maneuverValueTwo = 0;

        bool busy = false;
        bool abort = false;
        
        uint16_t azimuth;
        int16_t degree;
        uint32_t maneuverTime = 0;
        uint32_t actionStart = 0;
};

#endif // TEST_MODE_H