/**
 * @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
        };

        /**
         * @brief Construct a new Test Mode object
         * 
         * @param moveControl 
         * @param navigation 
         */
        TestMode(MoveControl *moveControl, Navigation* navigation);
        ~TestMode();

        /**
         * @brief Controls the maneuver.
         * 
         * Needed for actions which take a longer time.
         * Should be called ever main loop.
         */
        void loop() override;

        /**
         * @brief Set the speed for the Maneuver Drive
         * 
         * @param speed m/s / 100
         */
        void setSpeed(int16_t speed);

        /**
         * @brief Set the rotation speed for the Maneuver Drive
         * 
         * @param speed rad/s / 100
         */
        void setRotationSpeed(int16_t speed);

        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();

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

    private:
        bool engineInit(int16_t powerPercentage, int16_t seconds);

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

        double speed = 0;
        double rotationSpeed = 0;

        bool busy = false;
        bool abort = false;
        
        uint8_t delay = 10;
        uint16_t azimuth;
        int16_t degree;
        uint32_t maneuverTime = 0;
        uint32_t lastMillis = 0;
        uint32_t actionStart = 0;


};

#endif // TEST_MODE_H