#define LIGHTS 6
#define STATES 8
#define MAX_ULONG 4294967295

const int stageBeam = A0;
const int faultBeam = A1;

// Define a structure to contain the Arduino pin and the defined control bit for each light
struct LightPinsBits
{
  const int pin;
  const int bit;
} const lightPinBit[LIGHTS] =
{
  { 2, 0x01},     // Left Stage
  { 3, 0x02},     // Left Yellow1
  { 4, 0x04},     // Left Yellow2
  { 6, 0x08},     // Left Yellow3
  { 5, 0x10},     // Left Green
  { 7, 0x20}      // Left Red
};

enum States { Waiting = 0, Staged, Yellow1, Yellow2, Yellow3, Start, Racing, Fault};
const unsigned long Timings[STATES]  = { MAX_ULONG, 2000, 500, 500, 500, 2000, 2000, 5000};
const int beamTrigger = 800;     // A value less than this trigger value will be consider LOW, otherwise it is HIGH
int lightMask[STATES];    // Define an array to hold the light pattern for each stage
States currentState = Waiting;  // Start in the waiting state
unsigned long startMillis;       // the millis counter when the race starts (Green, Go light)
unsigned long stateMillis = 0;   // the millis counter for the current state
unsigned long timerMillis;       // the millis counter when the second beam is broken 

void setup()
{
  Serial.begin(9600);

  // Set all of the light pins as outputs
  for( int i = 0; i < LIGHTS; i++)
    pinMode( lightPinBit[i].pin, OUTPUT);

  // Initialize all of the single (or no) light states
  lightMask[ Waiting] = 0;
  lightMask[ Start] = lightPinBit[4].bit;
  lightMask[ Racing] = lightPinBit[4].bit;
  lightMask[ Fault] = lightPinBit[5].bit;

  // Each of the states from staged through yellow 3 have all of the lights from the 
  //  previous state plus the light for the new state
  for( int i = Staged; i < Start; i++)
    lightMask[i] = lightMask[i-1] | lightPinBit[i-1].bit;
}

void loop()
{ 
  if( isStaged())
  {
    if( currentState == Waiting)
    {
      stateMillis = millis();
      setState( Staged);
    }
    else if( currentState >= Start)
    {
      stateMillis = millis();
    }
  }
  else
  {
    if( currentState == Staged)
      setState( Waiting);
  }
  
  // has the fault beam been interrupted?
  if( isFault())
  {
    switch( currentState)
    {
      case Staged:
      case Yellow1:
      case Yellow2:
      case Yellow3:
        stateMillis = millis();
        setState( Fault);      // Update the current state and the christmas tree
        break;

      case Start:
        stateMillis = millis();       // Capture the time the second beam is interrupted during a race
        timerMillis = stateMillis;
        setState( Racing);     // Update the current state and the christmas tree
        break;

      case Fault:
        stateMillis = millis();
        break;
    }
  }

  // Has the time for the current state elapsed?
  if( millis() - stateMillis >= Timings[currentState])
  {
    switch( currentState)
    {
      case Staged:
      case Yellow1:
      case Yellow2:
      case Yellow3:
        stateMillis += Timings[currentState];       // Update the stateMillis
        setState( currentState + 1);                // Advance to the next state
        break;

      case Start:
        startMillis = stateMillis + Timings[currentState];
      case Racing:
      case Fault:
        setState( Waiting);
        break;
    }
  }
}

bool isStaged()
{
  return( analogRead(stageBeam) < beamTrigger);
}

bool isFault()
{
  return( analogRead(faultBeam) < beamTrigger);
}

bool setState(States state)
{
  if( currentState != state)     // only change the lights if the state changed
  {
    currentState = state;        // store the curret state to the last state

    int mask = lightMask[state];    // Get the light bit mask for this state
 /*
    Serial.print("State Changing to: ");
    Serial.print( state);
    Serial.print(", Light mask: ");
    Serial.print(mask);
    Serial.print(", stateMillis: ");
    Serial.println(stateMillis);
*/
    // loop through all of the lights and set them accoring to their bit value in the mask
    for( int i = 0; i < LIGHTS; i++)
      digitalWrite( lightPinBit[i].pin, (lightPinBit[i].bit & mask) > 0);

    return true;
  }

  return false;
}