#pragma once
#include <mutex>
#include <vector>
#include <iostream>
#include <cmath>

class ControlModule
{
private:
    mutable std::mutex mtx;
    double motors[4]; //LeftFront; RightFront; LeftBack; RightBack
    double maxSpeed;
    double rotateSpeed;
    double moveSideSpeed;
public:
    ControlModule(/* args */);
    ControlModule(double maxSpeed, double rotateSpeed, double moveSideSpeed);
    ~ControlModule();
    void adjustSpeed();
    void moveSide(int imageColumsMiddle, int contourColumsMiddle);
    void rotate(double ratio);
    void drive(double ratio);
    void unit();            //Brings the max Value to 1.0
    double ratio(double angle);

    void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle); //Funktion to be called
    std::vector<double> readMotors();
};