// MCDL14 Drive
// none
#include "SoftwareSerial.h"
// 6.25 Inch Rotation
// 5.76 Rotations Per Yard
// 1.25 Seconds Per Rotation
// 7.2 Seconds Per Yard
SoftwareSerial btsend(11, 10);
const int alive = 13;
//Forward
const int posfwd = 4;
const int negfwd = 6;
//Backward
const int posbak = 5;
const int negbak = 7;
// Last Position
int pos = 0;
int rot = 0;
char btrec;
// Run Time (Ref.)
int zero = 0;
int forty = 5555;
int fifty = 6944;
int sixty = 8333;
int seventy = 9722;
int eigthy = 11111;
int ninety = 12500;
void setup() {
  Serial.begin(9600);
  btsend.begin(9600);
  pinMode(alive, OUTPUT);
  pinMode(posfwd, OUTPUT);
  pinMode(negfwd, OUTPUT);
  pinMode(posbak, OUTPUT);
  pinMode(negbak, OUTPUT);
}
void loop() {
  // Bluetooth Coms
  while (btsend.available()) {
    btrec = btsend.read();
   
    Serial.println(btrec);
    Serial.println(pos);
    // 40 Yards
    if (btrec == '1') {
      if (pos > forty) {
        rot = pos - forty;
        Serial.println(rot);
        backward();
        delay(rot);
        halt();
        pos = forty;
      }
      else if (pos < forty) {
        rot = forty - pos;
        Serial.println(rot);
        forward();
        delay(rot);
        halt();
        pos = forty;
      }
      else if (pos == forty) {
        halt();
        pos = forty;
      }
    }
    // 50 Yards
    if (btrec == '2') {
      if (pos > fifty) {
        rot = pos - fifty;
        Serial.println(rot);
        backward();
        delay(rot);
        halt();
        pos = fifty;
      }
      else if (pos < fifty) {
        rot = fifty - pos;
        Serial.println(rot);
        forward();
        delay(rot);
        halt();
        pos = fifty;
      }
      else if (pos == fifty) {
        halt();
        pos = fifty;
      }
    }
    // 60 Yards
    if (btrec == '3') {
      if (pos > sixty) {
        rot = pos - sixty;
        Serial.println(rot);
        backward();
        delay(rot);
        halt();
        pos = sixty;
      }
      else if (pos < sixty) {
        rot = sixty - pos;
        Serial.println(rot);
        forward();
        delay(rot);
        halt();
        pos = sixty;
      }
      else if (pos == sixty) {
        halt();
        pos = sixty;
      }
    }
    // 70 Yards
    if (btrec == '4') {
      if (pos > seventy) {
        rot = pos - seventy;
        Serial.println(rot);
        backward();
        delay(rot);
        halt();
        pos = seventy;
      }
      else if (pos < seventy) {
        rot = seventy - pos;
        Serial.println(rot);
        forward();
        delay(rot);
        halt();
        pos = seventy;
      }
      else if (pos == seventy) {
        halt();
        pos = seventy;
      }
    }
    // 80 Yards
    if (btrec == '5') {
      if (pos > eigthy) {
        rot = pos - eigthy;
        Serial.println(rot);
        backward();
        delay(rot);
        halt();
        pos = eigthy;
      }
      else if (pos < eigthy) {
        rot = eigthy - pos;
        Serial.println(rot);
        forward();
        delay(rot);
        halt();
        pos = eigthy;
      }
      else if (pos == eigthy) {
        halt();
        pos = eigthy;
      }
    }
    // 90 Yards
    if (btrec == '6') {
      if (pos > ninety) {
        rot = pos - ninety;
        Serial.println(rot);
        backward();
        delay(rot);
        halt();
        pos = ninety;
      }
      else if (pos < ninety) {
        rot = ninety - pos;
        Serial.println(rot);
        forward();
        delay(rot);
        halt();
        pos = ninety;
      }
      else if (pos == ninety) {
        halt();
        pos = ninety;
      }
    }
    // Home
    if (btrec == '7') {
      if (pos > zero) {
        rot = pos - zero;
        Serial.println(rot);
        backward();
        delay(rot);
        halt();
        pos = zero;
      }
      else if (pos < zero) {
        rot = zero - pos;
        Serial.println(rot);
        forward();
        delay(rot);
        halt();
        pos = zero;
      }
      else if (pos == zero) {
        halt();
        pos = zero;
      }
    }
  }
}
void backward() {
  Serial.println("backward");
  digitalWrite(posbak, HIGH);
  digitalWrite(negbak, HIGH);
  digitalWrite(alive, HIGH);
}
void forward() {
  Serial.println("forward");
  digitalWrite(posfwd, HIGH);
  digitalWrite(negfwd, HIGH);
  digitalWrite(alive, HIGH);
}
void halt() {
  Serial.println("stop");
  digitalWrite(posfwd, LOW);
  digitalWrite(negfwd, LOW);
  digitalWrite(posbak, LOW);
  digitalWrite(negbak, LOW);
  digitalWrite(alive, LOW);
}