#ifndef MANUAL_CONTROL_H
#define MANUAL_CONTROL_H

#include <Ps3Controller.h>

#include "moveControl.h"
#include "motorControl.h"
#include "debugMqtt.h"
#include "debugTimes.h"

enum ControMode {halt, joystick, trigger};
class ManualControl {
    public:
        ManualControl();
        void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
        void runManualControl();
        void changeDriveMode(ControMode drive_mode);

    private:
        void driveWithControllerJoystick();
        void driveWithControllerShoulderTrigger();
        void reset();

        MoveControl *moveControl;
        MotorControl *motorControlLeft;
        MotorControl *motorControlRight;
        DebugMqtt *debug;

        ControMode controlMode = ControMode::halt;
        uint32_t last_millis = 0;

        // Is needed for driveWithControllerShoulderTrigger()
        uint8_t oldL = 0;
        uint8_t oldR = 0;
};

#endif // MANUAL_CONTROL_H