#include <SoftwareSerial.h>
//Track ID
const int id=5;
//Set Rx/Tx for SoftwareSerial.. need to be switched relative to computer
const int rx=0;
const int tx=1;
const int button = 7;
const int r_led = 8;
const int b_led = 9;
const int g_led = 10;
//Start the counter condition as -1 for connecting animation
int counter = -1;
int reading;
int buttonState = HIGH;
int lastButtonState = LOW;
unsigned long lastDebounceTime=0;
unsigned long debounceDelay = 50;

//Assign Software Serial pins
SoftwareSerial mySerial(rx,tx);

// the setup routine runs once when you press reset:
void setup() {                
  mySerial.begin(9600);
  // initialize the digital pin as an output.
  pinMode(b_led, OUTPUT); 
  pinMode(g_led, OUTPUT);
  pinMode(r_led, OUTPUT);
  pinMode(button, INPUT);
  //Set Tx as an INPUT to start so there won't be conflicts when connecting tracks
  pinMode(tx, INPUT);
}

// the loop routine runs over and over again forever:
void loop() {
  // check to see if the button state has changed
  reading = digitalRead(button);
  if ((reading != lastButtonState)){
    lastDebounceTime = millis();
  }
  if ((millis()-lastDebounceTime > debounceDelay)){
    //if it's changed and is currently pressed move the counter up
    if ((reading != buttonState)){
      buttonState = reading;
      if (buttonState == LOW){
        counter++;
      }
    }
  }

  //Start Connection Animation then switch LED to WHITE
  if (counter == -1){
    digitalWrite(r_led, LOW);
    digitalWrite(g_led, LOW);
    digitalWrite(b_led, LOW);
    delay(400);
    digitalWrite(r_led, HIGH);
    digitalWrite(g_led, HIGH);
    digitalWrite(b_led, HIGH);
    delay(400);
    digitalWrite(r_led, LOW);
    digitalWrite(g_led, LOW);
    digitalWrite(b_led, LOW);
    delay(400);
    delay(400);
    counter = 6;
  } 
  //RED
  if (counter%7==0){
    digitalWrite(r_led, LOW);
    digitalWrite(g_led, HIGH);
    digitalWrite(b_led, HIGH);
  }
  //YELLOW
  else if (counter%7==1){
    digitalWrite(r_led, LOW);
    digitalWrite(g_led, LOW);
    digitalWrite(b_led, HIGH);
  }
  //GREEN
  else if (counter%7==2){
    digitalWrite(r_led, HIGH);
    digitalWrite(g_led, LOW);
    digitalWrite(b_led, HIGH);
  }
  //CYAN
  else if (counter%7==3){
    digitalWrite(r_led, HIGH);
    digitalWrite(g_led, LOW);
    digitalWrite(b_led, LOW);
  }
  //BLUE
  else if (counter%7==4){
    digitalWrite(r_led, HIGH);
    digitalWrite(g_led, HIGH);
    digitalWrite(b_led, LOW);
  }
  //PURPLE
  else if (counter%7==5){
    digitalWrite(r_led, LOW);
    digitalWrite(g_led, HIGH);
    digitalWrite(b_led, LOW);
  }
  //WHITE
  else if (counter%7==6 && counter != -1){
    digitalWrite(r_led, LOW);
    digitalWrite(g_led, LOW);
    digitalWrite(b_led, LOW);
  }
  lastButtonState = reading;
  delay(10);
  //If the counter has reached the max value, go back to 0
  if (counter>=7){
    counter=0;
  }

  //Check to see if light sensor is engaged
  int sensorValue = analogRead(A3);
  if (sensorValue <= 300){
    //Need to write the data line high before switching the pinMode ot OUTPUT
    digitalWrite(tx, HIGH);
    //Set the pinMode to OUTPUT to send current counter value
    pinMode(tx, OUTPUT);
    mySerial.println(counter);
    //Set the pinMode back to INPUT, since it only needs to be an OUTPUT while sending a message
    pinMode(tx, INPUT);
    //This causes the LED to flicker quickly between the color value and white, which lightens the LED
    //giving visual confirmation that the current track piece sensor is responding to the light
    digitalWrite(r_led, LOW);
    digitalWrite(g_led, LOW);
    digitalWrite(b_led, LOW);
    delay(3);
  }
}