#include "LMotorController.h"
#include "Arduino.h"


LMotorController::LMotorController(int ena, int in1, int in2, int enb, int in3, int in4, double motorAConst, double motorBConst)
{
    _motorAConst = motorAConst;
    _motorBConst = motorBConst;
    
	_ena = ena;
	_in1 = in1;
	_in2 = in2;
	_enb = enb;
	_in3 = in3;
	_in4 = in4;
	
	pinMode(_ena, OUTPUT);
	pinMode(_in1, OUTPUT);
	pinMode(_in2, OUTPUT);
    
	pinMode(_enb, OUTPUT);
	pinMode(_in3, OUTPUT);
	pinMode(_in4, OUTPUT);
}


void LMotorController::move(int leftSpeed, int rightSpeed, int minAbsSpeed)
{
    if (rightSpeed < 0)
    {
        rightSpeed = min(rightSpeed, -1*minAbsSpeed);
        rightSpeed = max(rightSpeed, -255);
    }
    else if (rightSpeed > 0)
    {
        rightSpeed = max(rightSpeed, minAbsSpeed);
        rightSpeed = min(rightSpeed, 255);
    }
    
    int realRightSpeed = map(abs(rightSpeed), 0, 255, minAbsSpeed, 255);

    if (leftSpeed < 0)
    {
        leftSpeed = min(leftSpeed, -1*minAbsSpeed);
        leftSpeed = max(leftSpeed, -255);
    }
    else if (leftSpeed > 0)
    {
        leftSpeed = max(leftSpeed, minAbsSpeed);
        leftSpeed = min(leftSpeed, 255);
    }
    
    int realLeftSpeed = map(abs(leftSpeed), 0, 255, minAbsSpeed, 255);
    
    digitalWrite(_in3, rightSpeed > 0 ? HIGH : LOW);
    digitalWrite(_in4, rightSpeed > 0 ? LOW : HIGH);
    digitalWrite(_in1, leftSpeed > 0 ? HIGH : LOW);
    digitalWrite(_in2, leftSpeed > 0 ? LOW : HIGH);
    analogWrite(_ena, realRightSpeed * _motorAConst);
    analogWrite(_enb, realLeftSpeed * _motorBConst);
}


void LMotorController::move(int speed, int minAbsSpeed)
{
    int direction = 1;
    
    if (speed < 0)
    {
        direction = -1;
        
        speed = min(speed, -1*minAbsSpeed);
        speed = max(speed, -255);
    }
    else
    {
        speed = max(speed, minAbsSpeed);
        speed = min(speed, 255);
    }
    
    if (speed == _currentSpeed) return;
    
    int realSpeed = max(minAbsSpeed, abs(speed));
    
    digitalWrite(_in1, speed > 0 ? HIGH : LOW);
    digitalWrite(_in2, speed > 0 ? LOW : HIGH);
    digitalWrite(_in3, speed > 0 ? HIGH : LOW);
    digitalWrite(_in4, speed > 0 ? LOW : HIGH);
    analogWrite(_ena, realSpeed * _motorAConst);
    analogWrite(_enb, realSpeed * _motorBConst);
    
    _currentSpeed = direction * realSpeed;
}


void LMotorController::move(int speed)
{
    if (speed == _currentSpeed) return;
    
    if (speed > 255) speed = 255;
    else if (speed < -255) speed = -255;
    
    digitalWrite(_in1, speed > 0 ? HIGH : LOW);
    digitalWrite(_in2, speed > 0 ? LOW : HIGH);
    digitalWrite(_in3, speed > 0 ? HIGH : LOW);
    digitalWrite(_in4, speed > 0 ? LOW : HIGH);
    analogWrite(_ena, abs(speed) * _motorAConst);
    analogWrite(_enb, abs(speed) * _motorBConst);
    
    _currentSpeed = speed;
}


void LMotorController::turnLeft(int speed, int minAbsSpeed)
{
    digitalWrite(_in1, HIGH);
    digitalWrite(_in2, LOW);
    digitalWrite(_in3, LOW);
    digitalWrite(_in4, HIGH);
    int minSpeed;
    if (speed < 0)
    {
    speed = min(speed, -1*minAbsSpeed);
    speed = max(speed, -255);
    minSpeed = min(minAbsSpeed,255-abs(speed));
    analogWrite(_ena, minSpeed * _motorAConst);
    analogWrite(_enb, -1*speed * _motorBConst);
    }
    else
    {
    speed = max(speed, minAbsSpeed);
    speed = min(speed, 255);        
    minSpeed = min(minAbsSpeed,255-abs(speed));
    analogWrite(_ena, speed * _motorAConst);
    analogWrite(_enb, minSpeed * _motorBConst);
    }
}
void LMotorController::turnRight(int speed, int minAbsSpeed)
{
    digitalWrite(_in1, LOW);
    digitalWrite(_in2, HIGH);
    digitalWrite(_in3, HIGH);
    digitalWrite(_in4, LOW);   
    int minSpeed;
    
    if (speed < 0)
    {
    speed = min(speed, -1*minAbsSpeed);
    speed = max(speed, -255);    
    minSpeed = min(minAbsSpeed,255-abs(speed));
    analogWrite(_ena, -1*speed * _motorAConst);
    analogWrite(_enb, minSpeed * _motorBConst);
    }
    else
    {
    speed = max(speed, minAbsSpeed);
    speed = min(speed, 255);
    minSpeed = min(minAbsSpeed,255-abs(speed));
    analogWrite(_ena, minSpeed * _motorAConst);
    analogWrite(_enb, speed * _motorBConst);
    }
}


void LMotorController::stopMoving()
{
    digitalWrite(_in1, LOW);
    digitalWrite(_in2, LOW);
    digitalWrite(_in3, LOW);
    digitalWrite(_in4, LOW);
    digitalWrite(_ena, HIGH);
    digitalWrite(_enb, HIGH);
    
    _currentSpeed = 0;
}
