bool rider1,rider2;

unsigned long rider1_ready_time = 0;
unsigned long rider2_ready_time = 0;
unsigned long to_start_time = 0;

bool rider1_ready = false;
bool rider2_ready = false;
bool green = false;

void setup() {
  Serial.begin(9600);
  for(int i = 2; i<=13; i++)
  {
    pinMode(i, OUTPUT);
    digitalWrite(i, HIGH);
  }

  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  pinMode(A3, OUTPUT);
  pinMode(A4, OUTPUT);
  pinMode(A5, OUTPUT);
  pinMode(A2,OUTPUT);
  digitalWrite(A2,HIGH);
  digitalWrite(A4,HIGH);
  digitalWrite(A3,HIGH);
  digitalWrite(A5,HIGH);
}

void loop() {
  stage1();
}

void stage1()
{
  rider1 = digitalRead(A0);
  rider2 = digitalRead(A1);
  Serial.println("Rider1: " + String(rider1_ready_time) + " " + rider1_ready +" | Rider2: " + String(rider2_ready_time) + " " + rider2_ready + " | To Start Time: " + to_start_time );
  
  if(!rider1 || !rider2)
  {
    if(!rider1)
    {

      if(rider1_ready_time == 0)
      {
        rider1_ready_time = millis();
      }
      digitalWrite(13,LOW);
      if(millis()>= rider1_ready_time + 3000)
      {
        digitalWrite(12,LOW);
        rider1_ready = true;
      }
    }else{
      rider1_ready_time = 0;
      rider1_ready = false;
      digitalWrite(13,HIGH);
      digitalWrite(12,HIGH);
      to_start_time = 0;
    }

    if(!rider2)
    {
      if(rider2_ready_time == 0)
      {
        rider2_ready_time = millis();
      }
      digitalWrite(6,LOW);
      if(millis()>= rider2_ready_time + 3000)
      {
        digitalWrite(5,LOW);
        rider2_ready = true;
      }
    }else{
      rider2_ready_time = 0;
      rider2_ready = false;
      digitalWrite(6,HIGH);
      digitalWrite(5,HIGH);
      to_start_time = 0;
    }


  }else{
    digitalWrite(6,HIGH);
    digitalWrite(5,HIGH);
    digitalWrite(13,HIGH);
    digitalWrite(12,HIGH);
    rider1_ready_time = 0;
    rider2_ready_time = 0;
    rider1_ready = false;
    rider2_ready = false;
    to_start_time = 0;
  }

  if(rider1_ready && rider2_ready)
  {
    if(to_start_time == 0)
    {
      to_start_time = millis();
    }

    if(millis() >= to_start_time + 5000)
    {
      stage3(); // SELECT THE STAGE HERE
    }
  }else{
    to_start_time = 0;
  }


}

void stage2()
{
  int sequence = 0;
  while(1){
    rider1 = digitalRead(A0);
    rider2 = digitalRead(A1);
    

    if(rider1 && !green)
    {
      digitalWrite(7,LOW);
    }
    if(!rider1 && green && sequence == 4)
    {
      digitalWrite(A3, LOW);
    }


    if(rider2 && !green)
    {
      digitalWrite(A4,LOW);
    }

    if(!rider2 && green && sequence == 4)
    {
      digitalWrite(A2, LOW);
    }

    if(millis() >= to_start_time + 3230 && sequence == 0)
    {
      digitalWrite(6,HIGH);
      digitalWrite(5,HIGH);
      digitalWrite(13,HIGH);
      digitalWrite(12,HIGH);
      digitalWrite(11,LOW);
      digitalWrite(4,LOW);
      sequence = 1;
    }
    if(millis() >= to_start_time + 3230 && sequence == 1)
    { 
      digitalWrite(11,HIGH);
      digitalWrite(4,HIGH);
      digitalWrite(10,LOW);
      digitalWrite(3,LOW);
      sequence = 2;
    }
    if(millis() >= to_start_time + 3450 && sequence == 2)
    { 
      digitalWrite(10,HIGH);
      digitalWrite(3,HIGH);
      digitalWrite(9,LOW);
      digitalWrite(2,LOW);
      sequence = 3;
    }
    if(millis() >= to_start_time + 3630 && sequence == 3)
    { 
      digitalWrite(9,HIGH);
      digitalWrite(2,HIGH);
      digitalWrite(8,LOW);
      digitalWrite(A5,LOW);
      sequence = 4;
      green = true;
    }

  }
  
}

void stage3()
{
  int sequence = 0;
  while(1){
    rider1 = digitalRead(A0);
    rider2 = digitalRead(A1);
    

    if(rider1 && !green)
    {
      digitalWrite(7,LOW);
    }
    if(!rider1 && green && sequence == 1)
    {
      digitalWrite(A3, LOW);
    }


    if(rider2 && !green)
    {
      digitalWrite(A4,LOW);
    }

    if(!rider2 && green && sequence == 1)
    {
      digitalWrite(A2, LOW);
    }

    if(millis() >= to_start_time + 3230 && sequence == 0)
    {
      digitalWrite(6,HIGH);
      digitalWrite(5,HIGH);
      digitalWrite(13,HIGH);
      digitalWrite(12,HIGH);
      digitalWrite(11,LOW);
      digitalWrite(4,LOW);
      digitalWrite(10,LOW);
      digitalWrite(3,LOW);
      digitalWrite(9,LOW);
      digitalWrite(2,LOW);
      sequence = 1;
    }
    
    if(millis() >= to_start_time + 3630 && sequence == 1)
    { 
      digitalWrite(11,HIGH);
      digitalWrite(4,HIGH);
      digitalWrite(10,HIGH);
      digitalWrite(3,HIGH);
      digitalWrite(9,HIGH);
      digitalWrite(2,HIGH);
      digitalWrite(8,LOW);
      digitalWrite(A5,LOW);
      sequence = 1;
      green = true;
    }

  }
  
}