#include "qt_pi.h"

void initMotorController() {
  
  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
  digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
  
}
  
void setMotorSpeed(int i, int spd) {
    
  unsigned char reverse = 0;
  
  if (spd < 0)
  {
    spd = -spd;
    reverse = 1;
  }
  
  if (spd > 255)
  {
    spd = 255;
  }
  
  if(spd == 0)           
  { 
    switch(i){
      case LEFT_WHEEL:
        digitalWrite(LEFT_MOTOR_FORWARD, 1);
        digitalWrite(LEFT_MOTOR_BACKWARD, 1);   
        break;
      case RIGHT_WHEEL:
        digitalWrite(RIGHT_MOTOR_FORWARD, 1);
        digitalWrite(RIGHT_MOTOR_BACKWARD, 1);
        break;
    }
  }
  
  else 
  { 
    switch(i){
      
      case LEFT_WHEEL:
        
        switch(reverse)
        {
            case 0:  
              analogWrite(LEFT_MOTOR_FORWARD, spd);
              analogWrite(LEFT_MOTOR_BACKWARD, 0);
              break;
            case 1:
              analogWrite(LEFT_MOTOR_FORWARD, 0);
              analogWrite(LEFT_MOTOR_BACKWARD, spd);
              break;
        }
        break;
        
      case RIGHT_WHEEL:
        switch(reverse)
        {
            case 0:  
              analogWrite(RIGHT_MOTOR_FORWARD, spd);
              analogWrite(RIGHT_MOTOR_BACKWARD, 0);
              break;
            case 1:
              analogWrite(RIGHT_MOTOR_FORWARD, 0);
              analogWrite(RIGHT_MOTOR_BACKWARD, spd);
              break;
        }
        break;
      }
  } 
}
  
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
  
  setMotorSpeed(LEFT_WHEEL, leftSpeed);
  setMotorSpeed(RIGHT_WHEEL, rightSpeed);
  
}
