#include "control_module.h"

ControlModule::ControlModule(): ControlModule(1.0, 1.0, 1.0)
{    
}

ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed)
{
    motors[0] = 0.0;
    motors[1] = 0.0;
    motors[2] = 0.0;
    motors[3] = 0.0;
    this->forwardSpeed = forwardSpeed;
    this->rotateSpeed = rotateSpeed;
    this->moveSideSpeed = moveSideSpeed;
}

ControlModule::~ControlModule()
{
}

void ControlModule::moveForward(){
    for(int i = 0; i < 4; i++){
        motors[i] += forwardSpeed;
    }
};

void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
    float speed = moveSideSpeed * static_cast<float>(contourColumsMiddle - imageColumsMiddle)/static_cast<float>(imageColumsMiddle);
    motors[0] += speed;
    motors[1] -= speed;
    motors[2] -= speed;
    motors[3] += speed;
}

void ControlModule::rotate(int angle){
    float speed = rotateSpeed * (static_cast<float>(angle) + 90.0f)/90.0f;
    motors[0] += speed;
    motors[1] -= speed;
    motors[2] += speed;
    motors[3] -= speed;
}

void ControlModule::unit(){
    float max = 1.0E-6;
    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
        if(motors[i] > max)
        max = motors[i];
    }

    //Avoid dividing by zero
    if (max > 0.001)
    {
        for(int i = 0; i < 4; i++){
            motors[i] /= max;
        }
    }

}

void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
    std::unique_lock<std::mutex> lock(mtx);
    moveForward();
    moveSide(imageColumsMiddle, contourColumsMiddle);
    rotate(angle);
    unit();
}

std::vector<float> ControlModule::readMotors()
{
    std::unique_lock<std::mutex> lock(mtx);
    return std::vector<float> {motors[0], motors[1], motors[2], motors[3]};
}