#include <AFMotor.h>
AF_DCMotor motor4(4);
AF_DCMotor motor3(3);

int speed = 0;

#define ECHO_PIN_RAIL_ONE 52
#define TRIG_PIN_RAIL_ONE 53

#define ECHO_PIN_RAIL_TWO 50
#define TRIG_PIN_RAIL_TWO 51

unsigned long railOne = 0;
unsigned long railTwo = 0;

unsigned long railOneDistance = 5;
unsigned long railTwoDistance = 5;

unsigned long railOneHoldTime = 0;
unsigned long railTwoHoldTime = 0;

unsigned long stationTime = 20000;
unsigned long sampleTime = 5000;

unsigned long railOneLap = 0;
unsigned long railTwoLap = 0;

unsigned long sampleTimeOne = millis() + 5000;
unsigned long sampleTimeTwo = millis() + 5000;

boolean railOneRunning = 0;
boolean railTwoRunning = 0;

int g;
int r;

//int line1;
//int line2;

int rail;

void setup() {
  Serial.begin(9600); 
  pinMode(TRIG_PIN_RAIL_ONE, OUTPUT);
  pinMode(ECHO_PIN_RAIL_ONE, INPUT);
  pinMode(TRIG_PIN_RAIL_TWO, OUTPUT);
  pinMode(ECHO_PIN_RAIL_TWO, INPUT);

  pinMode(22, OUTPUT); //rail one green light -V
  pinMode(23, OUTPUT); //rail one red light -V
  pinMode(24, OUTPUT); //rail one + V

  pinMode(26, OUTPUT); //rail two green light -V
  pinMode(27, OUTPUT); //rail two red light -V
  pinMode(28, OUTPUT); //rail two +V

  pinMode(30, OUTPUT); //rail two turn out green light -v
  pinMode(31, OUTPUT); //rail two turn out red light -v
  pinMode(32, OUTPUT); //rail two turn out +v

  digitalWrite(24, HIGH); //rail one +V
  digitalWrite(28, HIGH); //rail two +V
  digitalWrite(32, HIGH); //rail two turn out +V
}

  float startup() {
  delay(500);
  lightGreen(22,23);
  lightRed(22,23);
  delay(500);
  lightGreen(26,27);
  lightRed(26,27);
  delay(500);
  lightGreen(30,31);
  lightRed(30,31);
  delay(1000);
  railOne = 1;
  railTwo = 1;
  accelerate(1);
  accelerate(2);
  }

  void lightGreen(int g, int r) {
    digitalWrite(g, LOW);
    delay(500);
    digitalWrite(r, HIGH);
  }

  void lightRed(int g, int r) {
    digitalWrite(g, HIGH);
    delay(500);
    digitalWrite(r, LOW);
  }

  float readDistanceINrailOne() {
  digitalWrite(TRIG_PIN_RAIL_ONE, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_RAIL_ONE, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_RAIL_ONE, LOW);
  //Serial.println(pulseIn(ECHO_PIN_RAIL_ONE, HIGH));
  int duration = pulseIn(ECHO_PIN_RAIL_ONE, HIGH);
  return duration / 148;
  }

  float readDistanceINrailTwo() {
  digitalWrite(TRIG_PIN_RAIL_TWO, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_RAIL_TWO, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_RAIL_TWO, LOW);
  int duration = pulseIn(ECHO_PIN_RAIL_TWO, HIGH);
  return duration / 148;
  }

  void accelerate(int rail) {
    uint8_t i;
    switch (rail) {
      case 1:
        lightGreen(22,23);
        delay(2000);
        motor4.run(RELEASE);
        motor4.run(FORWARD);
        for (i=80; i<255; i++) {
          motor4.setSpeed(i); 
          //++speed; 
          delay(20);
        }
      railOneRunning = 1;
      railOneLap = 0;
      break;
      case 2:
        lightGreen(26,27);
        delay(2000);
        motor3.run(RELEASE);
        motor3.run(FORWARD);
        for (i=80; i<255; i++) {
          motor3.setSpeed(i); 
          //++speed; 
          delay(20);
        }
      railTwoRunning = 1;
      railTwoLap = 0;
      break;
    }
  }

  void decellerate(int rail) {
    uint8_t i;
    switch (rail) {
      case 1:
      lightRed(22,23);
      //Serial.println("decellerate 1");
      for (i=255; i!=80; i--) {
      motor4.setSpeed(i); 
      //--speed; 
      delay(20);
      };
      motor4.setSpeed(0);
      railOneLap = 0;
      railOneRunning = 0;
      railOneHoldTime = millis() + stationTime;
      break;
      case 2:
      lightRed(26,27);
      //Serial.println("decellerate2");
      for (i=255; i!=80; i--) {
      motor3.setSpeed(i); 
      //--speed; 
      delay(20);
      };
      motor3.setSpeed(0);
      railTwoLap = 0;
      railTwoRunning = 0;
      railTwoHoldTime = millis() + stationTime;
      break;
    }
  }

void loop() { 

 if(!railOne && !railTwo) {
  startup(); 
 }

if(readDistanceINrailOne() <=2 && millis() >= sampleTimeOne  && railOneRunning) {
  railOneLap ++;
  sampleTimeOne = millis() + sampleTime;
}

if(readDistanceINrailTwo() <=2 && millis() >= sampleTimeTwo  && railTwoRunning) {
  railTwoLap ++;
  sampleTimeTwo = millis() + sampleTime;
}

if(readDistanceINrailOne() < 2 && railOneLap >= 5) {
railOneRunning = 0;
decellerate(1);
}

if(readDistanceINrailTwo() < 2 && railTwoLap >= 5) {
railTwoRunning = 0;
decellerate(2);
}

if (millis() >= railOneHoldTime && railOneLap == 0) {
  accelerate(1);
}

if (millis() >= railTwoHoldTime && railTwoLap == 0) {
  accelerate(2);
}
}