// MCDL14 Drive
// Vije Miller

#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);
}