#include "control_module.h"

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

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

ControlModule::~ControlModule()
{
}

void ControlModule::adjustSpeed(){
    double factor = 0.0;
    if(maxSpeed > 1.0)
    {
        factor = 1.0;
    }
    else if(maxSpeed < -1.0)
    {
        factor = -1.0;
    }
    else
    {
        factor = maxSpeed;
    }

    for(int i = 0; i < 4; i++)
    {
        motors[i] *= factor;
    }
};

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

void ControlModule::drive(double ratio)
{
    motors[0] += ratio;
    motors[1] += ratio;
    motors[2] += ratio;
    motors[3] += ratio;
}

void ControlModule::rotate(double ratio)
{
    motors[0] += ratio;
    motors[1] -= ratio;
    motors[2] += ratio;
    motors[3] -= ratio;
}

void ControlModule::unit(){
    double 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, double angle){
    std::unique_lock<std::mutex> lock(mtx);
    double rotationRatio = ratio(angle);
    rotate(rotationRatio);
    drive(1.0-std::fabs(rotationRatio));
    unit();
    adjustSpeed();
}

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

double ControlModule::ratio(double angle)
{
    double minAngularSpeed = -1.0;
    double maxAngularSpeed = 1.0;
    double speedRange = maxAngularSpeed - minAngularSpeed;

    double minAngle = -20.0;
    double maxAngle = 20.0;
    double angularRange = maxAngle - minAngle;

    if(angle < minAngle)
    {
        angle = minAngle;
    }
    if(angle > maxAngle)
    {
        angle = maxAngle;
    }
    double progress = angle - minAngle;
    double progressPercent = progress/angularRange;

    double speed = minAngularSpeed + progressPercent*speedRange;
    return speed;
}