#include "control_module.h"

ControlModule::ControlModule(){    
}

ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed)
{
    this->forwardSpeed = forwardSpeed;
    this->rotateSpeed = rotateSpeed;
    this->moveSideSpeed = moveSideSpeed;
}

ControlModule::~ControlModule()
{
}

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

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

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

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

void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
    moveForward();
    moveSide(imageColumsMiddle, contourColumsMiddle);
    rotate(angle);
    unit();
}