/**
 * @file manualControl.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief A small class to drive the Rover by the controller joystick.
 * @version 0.1
 * @date 2021-12-13
 * 
 * @copyright Copyright (c) 2021
 * 
 */
#ifndef MANUAL_CONTROL_H
#define MANUAL_CONTROL_H

#include "moveControl.h"
#include "driveModi/driveModi.h"
#include "controlPadInput.h"
#include "calibrateCompass.h"

class DirectionChangeWrapper {
    public:
        virtual void action() = 0;
};

/**
 * @brief Drive the Rover with a Joystick
 * 
 * This class gets the x and y value from the PS3 controller
 * and map the values to moveControl
 * 
 * @see MoveControl
 */
class ManualControl : public DriveModi {
    public:
        enum class InputMode : uint8_t {
            Analog,
            Digital
        };

        ManualControl(MoveControl *moveControl, const ControlPadInput *input);

        /**
         * @brief Destroy the Manual Control object
         * Set in moveControl speed and rotation to 0 and set DrivingStatus::Stop
         */
        ~ManualControl();

        /**
         * @brief Calls analogControl() to update all values.
         * 
         * This function should be called every mainloop. If the delay is not reached, than the
         * functions returns immediately.
         * @see runSpeedometer()
         * @see setDelay()
         */
        void loop() override;

        /**
         * @brief Set the min delay between each loop
         * 
         * @param delay_ time in Milliseconds
         */
        void setDelay(uint8_t delay_) { delay = delay_; }

        /**
         * @brief Set the max speed
         * 
         * @param maxSpeed in m/s 
         */
        void setMaxSpeed(double maxSpeed) { max_speed = maxSpeed; }
        void increaseMaxSpeed(double increase = 0.1) { max_speed += increase; }
        void decreaseMaxSpeed(double increase = 0.1) { max_speed -= increase; }
        double getMaxSpeed() { return this->max_speed; }
        
        /**
         * @brief Set the max rotation
         * 
         * @param maxRotation in rad/s (maybe) 
         */
        void setMaxRotation(double maxRotation) { max_rotation = maxRotation; }
        void increaseMaxRotation(double increase = 0.1) { max_rotation += increase; }
        void decreaseMaxRotation(double increase = 0.1) { max_rotation -= increase; }
        double getMaxRotation() { return this->max_rotation; }

        void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; }

        void switchInputMode();
        void setInputMode(InputMode mode) { this->inputMode = mode; }
        InputMode getInputMode() const { return this->inputMode; }
        void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; }

        uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); }
        uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }

    protected:
        MoveControl *moveControl;
        const ControlPadInput* input;

    private:
        /**
         * @brief Noramly called repeatedly by loop() to calcluate new values.
         * Set new values for speed and rotation in moveControl
         */
        void analogControl();
        void digitalControl();

        uint32_t lastMillis = 0;
        uint8_t delay = 10;
        double max_speed = 1;
        double max_rotation = 7;
        CalibrateCompass* caliCompass = nullptr;
        DirectionChangeWrapper* directionChangeWrapper = nullptr;
        InputMode inputMode = InputMode::Analog;
};

#endif // MANUAL_CONTROL_H