#ifndef MOVE_CONTROL_H
#define MOVE_CONTROL_H

#include <cstdint>

#include "motorControl.h"
#include "speedometer.h"
#include "config.h"

enum drivingStatus {stop,
                    straightForward,
                    straightBackward,
                    arcForwardLeft,
                    arcForwardRight,
                    arcBackwardLeft,
                    arcBackwardRight,
                    rotateLeft,
                    rotateRight};

enum regulateStatus {stop,
                    drive,
                    rotateLeft,
                    rotateRight};

class MoveControl {
    public:
        MoveControl();
        void init(MotorControl *left_motor, MotorControl *right_motor,
                    Speedometer *left_encoder, Speedometer *right_encoder);
        void runMoveControl();
        void setDrivingStatus(drivingStatus status);
        void setSpeed(double speed);
        void setRotationspeed(double speed);

    private:
        void calcWheelSpeed();
        void regulateMotors();

        MotorControl *left_motor;
        MotorControl *right_motor;
        Speedometer *left_speedometer;
        Speedometer *right_speedometer;

        drivingStatus driving_status = drivingStatus::stop;

        double x_speed = 0;
        double rotation_speed = 0;

        double wheelspeed_left_target = 0;
        double wheelspeed_right_target = 0;
};

#endif // MOVE_CONTROL_H