/*
  SoftwareSerial to ttyAMA0
*/

#include <SoftwareSerial.h>

#define PULSE_A 3
#define PULSE_B 11
#define DIR_A 12
#define DIR_B 13

SoftwareSerial mySerial(8, 9); // RX, TX
char serialIn;
int movementTime = 1000;
int basicPower = 50;

void setup()  
{
  //serial communication
  Serial.begin(4800);
  while (!Serial) {
    ;
  }
  Serial.println("Started");
  mySerial.begin(9600);
  
  //set the motor pins
  pinMode( PULSE_A, OUTPUT);
  pinMode( PULSE_B, OUTPUT);
  pinMode( DIR_A, OUTPUT);
  pinMode( DIR_B, OUTPUT);
  
  digitalWrite( PULSE_A, LOW);
  digitalWrite( PULSE_B, LOW);
  digitalWrite( DIR_A, LOW);
  digitalWrite( DIR_B, LOW);
  
  /*
  analogWrite( PULSE_A, 100);
  analogWrite( PULSE_B, 100);
  */
}

void loop()
{
  if (mySerial.available()){
    serialIn = mySerial.read();
    Serial.print( serialIn);
    if( serialIn == '+' ){
      goForward( movementTime, basicPower);
    }
    else if( serialIn == '-'){
      //goReverse( movementTime, basicPower);
    }
    else if( serialIn == '>'){
      turnRight(movementTime, basicPower);
    }
    else if( serialIn == '<'){
      turnLeft(movementTime, basicPower);
    } 
  }
  if (Serial.available())
    mySerial.write(Serial.read());
}

void goForward(int time, int power){
  digitalWrite( PULSE_A, HIGH);
  digitalWrite( PULSE_B, HIGH);
  delay(time);
  analogWrite( PULSE_A, power);
  analogWrite( PULSE_B, power);
  //delay(time);
  analogWrite( PULSE_A, 0);
  analogWrite( PULSE_B, 0);
  digitalWrite( PULSE_A, LOW);
  digitalWrite( PULSE_B, LOW);
}

/*
//as far as we know, this will never work...
void goReverse( int time, int power){
  digitalWrite( PULSE_A, HIGH);
  digitalWrite( PULSE_B, HIGH);
  delay(time);
  analogWrite( PULSE_A, power);
  analogWrite( PULSE_B, power);
  //delay(time);
  analogWrite( PULSE_A, 0);
  analogWrite( PULSE_B, 0);
  digitalWrite( PULSE_A, LOW);
  digitalWrite( PULSE_B, LOW);
}
*/

void turnLeft(int time, int power){
  digitalWrite( PULSE_A, HIGH);
  delay(time);
  analogWrite( PULSE_A, power);
  //delay(time);
  analogWrite( PULSE_A, 0);
  digitalWrite( PULSE_A, LOW);
}

void turnRight(int time, int power){
  digitalWrite( PULSE_B, HIGH);
  delay(time);
  analogWrite( PULSE_B, power);
  //delay(time);
  analogWrite( PULSE_B, 0);
  digitalWrite( PULSE_B, LOW);
}