#ifndef MOVE_CONTROL_H
#define MOVE_CONTROL_H

#include <cstdint>
#include <PID_v1.h>

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

enum DrivingStatus {stop,
                    drive};


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);
        void setPidTunings(uint8_t side, double p, double i, double d);

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

        DebugMqtt *debug;

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

        DrivingStatus driving_status = DrivingStatus::stop;

        PID *left_pid;
        PID *right_pid;

        double x_speed = 0;
        double rotation_speed = 0;

        double wheelspeed_left_target = 0;
        double wheelspeed_right_target = 0;
        double wheelspeed_left = 0;
        double wheelspeed_right = 0;
        double left_pid_out;
        double right_pid_out;

};

#endif // MOVE_CONTROL_H