// Arduino Drag Tree
// Description: Timing system for 201 meters (1/8 miles)
// Author: Flavio Nakamura 
// Date: 27/11/2009 
// Version: 1.2
// Video: https://www.youtube.com/watch?v=f-6b7ibJDmA (beta)

#define BUTTON_START 50

#define LED_BLUE 21
#define LED_PRESTAGE_L 22
#define LED_PRESTAGE_R 23
#define LED_STAGE_L 24
#define LED_STAGE_R 25
#define LED_AMBER1_L 26
#define LED_AMBER1_R 27
#define LED_AMBER2_L 28
#define LED_AMBER2_R 29
#define LED_AMBER3_L 30
#define LED_AMBER3_R 31
#define LED_GREEN_L 32
#define LED_GREEN_R 33
#define LED_RED_L 34
#define LED_RED_R 35

#define LDR_PRESTAGE_L 36
#define LDR_PRESTAGE_R 37
#define LDR_STAGE_L 38
#define LDR_STAGE_R 39
#define LDR_GUARDBEAM_L 40
#define LDR_GUARDBEAM_R 41
#define LDR_P1_L 42
#define LDR_P1_R 43
#define LDR_P2_L 44
#define LDR_P2_R 45
#define LDR_SF_L 46
#define LDR_SF_R 47
#define LDR_ET_L 48
#define LDR_ET_R 49

#define COMMAND_RESET 'r'
#define COMMAND_TREE_PRO '0'
#define COMMAND_TREE_FULL '1'
#define COMMAND_LANE_CLOSE 'c'
#define COMMAND_LANE_OPEN 'C'
#define COMMAND_TEST_LAMPS 'l'
#define COMMAND_TEST_SENSOR_START 't'
#define COMMAND_TEST_SENSOR_END 'T'
#define COMMAND_STAGE_ONLY_L 'A'
#define COMMAND_STAGE_ONLY_R 'a'
#define COMMAND_STAGE_BOTH 'b'
#define COMMAND_START 's'
#define COMMAND_FINISH_L 'F'
#define COMMAND_FINISH_R 'f'

#define STATUS_LANE_CLOSED 1
#define STATUS_TEST_LAMPS 2
#define STATUS_TEST_SENSORS 3
#define STATUS_STAGING 4
#define STATUS_TIMING 5

#define INTERVAL_PROTREE 400
#define INTERVAL_SPORTSMAN 500
#define INTERVAL_BLINK 500
#define INTERVAL_LAMP_TEST 800

#define SENSOR_SPEED_DISTANCE 20

enum lane { undefined, left, right };

char command;
char status;
bool proTree;
bool onLaneL, onLaneR, finishedL, finishedR;
lane winner;

byte lampNumber;
unsigned long lampLastUpdate;

int timingLampInterval;
unsigned long timingStart, timingGreen;
unsigned long timingReactionL, timingP1L, timingP2L, timingSpeedFinalL, timingElapsedL;
unsigned long timingReactionR, timingP1R, timingP2R, timingSpeedFinalR, timingElapsedR;

bool sentRTL, sentP1L, sentP2L, sentSFL, sentETL;
bool sentRTR, sentP1R, sentP2R, sentSFR, sentETR;
bool sentWinner;

void setup() {
  pinMode(BUTTON_START, INPUT_PULLUP);

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  pinMode(LED_PRESTAGE_L, OUTPUT);
  pinMode(LED_PRESTAGE_R, OUTPUT);
  pinMode(LED_STAGE_L, OUTPUT);
  pinMode(LED_STAGE_R, OUTPUT);
  pinMode(LED_AMBER1_L, OUTPUT);
  pinMode(LED_AMBER1_R, OUTPUT);
  pinMode(LED_AMBER2_L, OUTPUT);
  pinMode(LED_AMBER2_R, OUTPUT);
  pinMode(LED_AMBER3_L, OUTPUT);
  pinMode(LED_AMBER3_R, OUTPUT);
  pinMode(LED_GREEN_L, OUTPUT);
  pinMode(LED_GREEN_R, OUTPUT);
  pinMode(LED_RED_L, OUTPUT);
  pinMode(LED_RED_R, OUTPUT);

  pinMode(LDR_PRESTAGE_L, INPUT);
  pinMode(LDR_PRESTAGE_R, INPUT);
  pinMode(LDR_STAGE_L, INPUT);
  pinMode(LDR_STAGE_R, INPUT);
  pinMode(LDR_GUARDBEAM_L, INPUT);
  pinMode(LDR_GUARDBEAM_R, INPUT);
  pinMode(LDR_P1_L, INPUT);
  pinMode(LDR_P1_R, INPUT);
  pinMode(LDR_P2_L, INPUT);
  pinMode(LDR_P2_R, INPUT);
  pinMode(LDR_SF_L, INPUT);
  pinMode(LDR_SF_R, INPUT);
  pinMode(LDR_ET_L, INPUT);
  pinMode(LDR_ET_R, INPUT);

  Serial.begin(115200);
  Serial.println("READY!");
}

void loop() {
  
  if (Serial.available() > 0) {
    command = Serial.read();
    commandProcess();
  }
  else if (!isHigh(BUTTON_START) && status == STATUS_STAGING) { 
    start();
  }

  switch (status) {
    case STATUS_LANE_CLOSED:
      laneClosed();
      break;
    case STATUS_TEST_LAMPS:
      lampTest();
      break;
    case STATUS_TEST_SENSORS:
      sensorTest();
      break;
    case STATUS_STAGING:
      staging();
      break;
    case STATUS_TIMING:
      timing();
      break;
    default:
      standBy();
      break;
  }
}

void commandProcess() {
  if (command == COMMAND_RESET) {
    status = 0;
    proTree = false;

    sensorCheckError();
    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_TREE_FULL) {
    proTree = false;

    sendConfirmation(command);
  }
  else if (command == COMMAND_TREE_PRO) {
    proTree = true;

    sendConfirmation(command);
  }
  else if (command == COMMAND_LANE_CLOSE && status != STATUS_LANE_CLOSED) {
    status = STATUS_LANE_CLOSED;
    
    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_LANE_OPEN && status == STATUS_LANE_CLOSED) {
    status = 0;
    
    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_TEST_LAMPS && status == 0) {
    status = STATUS_TEST_LAMPS;
    lampNumber = 0;

    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_TEST_SENSOR_START && status == 0) {
    status = STATUS_TEST_SENSORS;
    
    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_TEST_SENSOR_END && status == STATUS_TEST_SENSORS) {
    status = 0;
    
    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_STAGE_ONLY_L && status == 0) {
    status = STATUS_STAGING;
    onLaneL = true;
    onLaneR = false;
    timingStart = 0;

    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_STAGE_ONLY_R && status == 0) {
    status = STATUS_STAGING;
    onLaneL = false;
    onLaneR = true;
    timingStart = 0;

    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_STAGE_BOTH && status == 0) {
    status = STATUS_STAGING;
    onLaneL = true;
    onLaneR = true;
    timingStart = 0;

    setAllLampOff();
    sendConfirmation(command);
  }
  else if (command == COMMAND_START && status == STATUS_STAGING) start();
  else if (command == COMMAND_FINISH_L && status == STATUS_TIMING) {
    finishedL = true;

    sendConfirmation(command);
  } 
  else if (command == COMMAND_FINISH_R && status == STATUS_TIMING) {
    finishedR = true;

    sendConfirmation(command);
  }
}

void sendConfirmation(char command) {
  Serial.print("CC");
  Serial.print(command);
  Serial.println(";");
}

void setAllLampOff() {
  digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(LED_BLUE, LOW);
  digitalWrite(LED_PRESTAGE_L, LOW);
  digitalWrite(LED_PRESTAGE_R, LOW);
  digitalWrite(LED_STAGE_L, LOW);
  digitalWrite(LED_STAGE_R, LOW);
  digitalWrite(LED_AMBER1_L, LOW);
  digitalWrite(LED_AMBER1_R, LOW);
  digitalWrite(LED_AMBER2_L, LOW);
  digitalWrite(LED_AMBER2_R, LOW);
  digitalWrite(LED_AMBER3_L, LOW);
  digitalWrite(LED_AMBER3_R, LOW);
  digitalWrite(LED_GREEN_L, LOW);
  digitalWrite(LED_GREEN_R, LOW);
  digitalWrite(LED_RED_L, LOW);
  digitalWrite(LED_RED_R, LOW);
}

bool isHigh(int pin) {
  return digitalRead(pin) == HIGH;
}

void standBy() {
  unsigned long now = millis();

  if ((now - lampLastUpdate) >= INTERVAL_BLINK) {
    lampLastUpdate = now;

    digitalWrite(LED_BUILTIN, isHigh(LED_BUILTIN) ? LOW : HIGH);
  }
}

void laneClosed() {
  unsigned long now = millis();

  if ((now - lampLastUpdate) >= INTERVAL_BLINK) {
    lampLastUpdate = now;

    digitalWrite(LED_RED_L, isHigh(LED_RED_L) ? LOW : HIGH);
    digitalWrite(LED_RED_R, isHigh(LED_RED_R) ? LOW : HIGH);
  }
}

void lampTest() {
  unsigned long now = millis();

  if ((now - lampLastUpdate) >= INTERVAL_LAMP_TEST) {
    lampLastUpdate = now;

    if (lampNumber == 0) {
      digitalWrite(LED_BLUE, HIGH);
    }
    else if (lampNumber == 1) {
      digitalWrite(LED_BLUE, LOW);

      digitalWrite(LED_PRESTAGE_L, HIGH);
      digitalWrite(LED_PRESTAGE_R, HIGH);
    }
    else if (lampNumber == 2) {
      digitalWrite(LED_PRESTAGE_L, LOW);
      digitalWrite(LED_PRESTAGE_R, LOW);

      digitalWrite(LED_STAGE_L, HIGH);
      digitalWrite(LED_STAGE_R, HIGH);
    }
    else if (lampNumber == 3) {
      digitalWrite(LED_STAGE_L, LOW);
      digitalWrite(LED_STAGE_R, LOW);

      digitalWrite(LED_AMBER1_L, HIGH);
      digitalWrite(LED_AMBER1_R, HIGH);
    }
    else if (lampNumber == 4) {
      digitalWrite(LED_AMBER1_L, LOW);
      digitalWrite(LED_AMBER1_R, LOW);

      digitalWrite(LED_AMBER2_L, HIGH);
      digitalWrite(LED_AMBER2_R, HIGH);
    }
    else if (lampNumber == 5) {
      digitalWrite(LED_AMBER2_L, LOW);
      digitalWrite(LED_AMBER2_R, LOW);

      digitalWrite(LED_AMBER3_L, HIGH);
      digitalWrite(LED_AMBER3_R, HIGH);
    }
    else if (lampNumber == 6) {
      digitalWrite(LED_AMBER3_L, LOW);
      digitalWrite(LED_AMBER3_R, LOW);

      digitalWrite(LED_GREEN_L, HIGH);
      digitalWrite(LED_GREEN_R, HIGH);
    }
    else if (lampNumber == 7) {
      digitalWrite(LED_GREEN_L, LOW);
      digitalWrite(LED_GREEN_R, LOW);

      digitalWrite(LED_RED_L, HIGH);
      digitalWrite(LED_RED_R, HIGH);
    }
    else if (lampNumber == 8) {
      digitalWrite(LED_RED_L, LOW);
      digitalWrite(LED_RED_R, LOW);

      status = 0;
    }
    
    lampNumber++;
  }
}

void sensorCheckError() {
  if (isHigh(LDR_P1_L)) Serial.println("SE1L");
  if (isHigh(LDR_P2_L)) Serial.println("SE2L");
  if (isHigh(LDR_SF_L)) Serial.println("SE6L");
  if (isHigh(LDR_ET_L)) Serial.println("SE7L");

  if (isHigh(LDR_P1_R)) Serial.println("SE1R");
  if (isHigh(LDR_P2_R)) Serial.println("SE2R");
  if (isHigh(LDR_SF_R)) Serial.println("SE6R");
  if (isHigh(LDR_ET_R)) Serial.println("SE7R");
}

void sensorLampOn(int pinSensor, int pinLamp) {
  if (isHigh(pinSensor) && !isHigh(pinLamp)) digitalWrite(pinLamp, HIGH);
  else if (!isHigh(pinSensor) && isHigh(pinLamp)) digitalWrite(pinLamp, LOW);
}

void sensorTest() {
  unsigned long now = millis();

  if ((now - lampLastUpdate) >= INTERVAL_BLINK) {
    lampLastUpdate = now;

    digitalWrite(LED_BLUE, isHigh(LED_BLUE) ? LOW : HIGH);
  }

  sensorLampOn(LDR_PRESTAGE_L, LED_PRESTAGE_L);
  sensorLampOn(LDR_STAGE_L, LED_STAGE_L);
  sensorLampOn(LDR_GUARDBEAM_L, LED_AMBER1_L);
  sensorLampOn(LDR_P1_L, LED_AMBER2_L);
  sensorLampOn(LDR_P2_L, LED_AMBER3_L);
  sensorLampOn(LDR_SF_L, LED_GREEN_L);
  sensorLampOn(LDR_ET_L, LED_RED_L);

  sensorLampOn(LDR_PRESTAGE_R, LED_PRESTAGE_R);
  sensorLampOn(LDR_STAGE_R, LED_STAGE_R);
  sensorLampOn(LDR_GUARDBEAM_R, LED_AMBER1_R);
  sensorLampOn(LDR_P1_R, LED_AMBER2_R);
  sensorLampOn(LDR_P2_R, LED_AMBER3_R);
  sensorLampOn(LDR_SF_R, LED_GREEN_R);
  sensorLampOn(LDR_ET_R, LED_RED_R);
}

void staging() {
  if (onLaneL) {
    sensorLampOn(LDR_PRESTAGE_L, LED_PRESTAGE_L);
    sensorLampOn(LDR_STAGE_L, LED_STAGE_L);
    sensorLampOn(LDR_GUARDBEAM_L, LED_RED_L);
  }
  else if (!isHigh(LED_RED_L)) digitalWrite(LED_RED_L, HIGH);

  if (onLaneR) {
    sensorLampOn(LDR_PRESTAGE_R, LED_PRESTAGE_R);
    sensorLampOn(LDR_STAGE_R, LED_STAGE_R);
    sensorLampOn(LDR_GUARDBEAM_R, LED_RED_R);
  }
  else if (!isHigh(LED_RED_R)) digitalWrite(LED_RED_R, HIGH);

  if (!isHigh(LED_BLUE)) digitalWrite(LED_BLUE, HIGH);
}

void start() {
  status = STATUS_TIMING;

  sendConfirmation(COMMAND_START);

  unsigned long now = millis();

  if (proTree) {
    timingGreen = now + INTERVAL_PROTREE;
    timingLampInterval = INTERVAL_PROTREE;
  }
  else {
    timingGreen = now + (3 * INTERVAL_SPORTSMAN);
    timingLampInterval = INTERVAL_SPORTSMAN;
  }

  timingStart = now;
  winner = undefined;
  lampNumber = 0;

  timingReactionL = 0;
  timingP1L = 0;
  timingP2L = 0;
  timingSpeedFinalL = 0;
  timingElapsedL = 0;
  finishedL = false;

  timingReactionR = 0;
  timingP1R = 0;
  timingP2R = 0;
  timingSpeedFinalR = 0;
  timingElapsedR = 0;
  finishedR = false;

  sentRTL = false;
  sentP1L = false;
  sentP2L = false;
  sentSFL = false;
  sentETL = false;

  sentRTR = false;
  sentP1R = false; 
  sentP2R = false;
  sentSFR = false;
  sentETR = false;

  sentWinner = false;
}

void timing() {
  unsigned long now = millis();

  if (onLaneL && !finishedL) { 
    if (timingReactionL == 0 && (!isHigh(LDR_STAGE_L) || isHigh(LDR_GUARDBEAM_L))) timingReactionL = now;
    else if (timingP1L == 0 && timingReactionL > 0 && isHigh(LDR_P1_L)) timingP1L = now;
    else if (timingP2L == 0 && timingReactionL > 0 && isHigh(LDR_P2_L)) timingP2L = now;
    else if (timingSpeedFinalL == 0 && timingReactionL > 0 && isHigh(LDR_SF_L)) timingSpeedFinalL = now;
    else if (timingElapsedL == 0 && timingReactionL > 0 && isHigh(LDR_ET_L)) {
      timingElapsedL = now;
      finishedL = true;
    }
  }

  if (onLaneR && !finishedR) { 
    if (timingReactionR == 0 && (!isHigh(LDR_STAGE_R) || isHigh(LDR_GUARDBEAM_R))) timingReactionR = now;
    else if (timingP1R == 0 && timingReactionR > 0 && isHigh(LDR_P1_R)) timingP1R = now;
    else if (timingP2R == 0 && timingReactionR > 0 && isHigh(LDR_P2_R)) timingP2R = now;
    else if (timingSpeedFinalR == 0 && timingReactionR > 0 && isHigh(LDR_SF_R)) timingSpeedFinalR = now;
    else if (timingElapsedR == 0 && timingReactionR > 0 && isHigh(LDR_ET_R)) {
      timingElapsedR = now;
      finishedR = true;
    }
  }

  timingCheckWinner();
  timingSendValues();
  timingLampReaction();

  if ((now - lampLastUpdate) >= timingLampInterval) {
    lampLastUpdate = now;

    if (winner != undefined) timingLampWinner();
    else if (lampNumber <= 3) timingLampSequence();
  }

  if ((!onLaneL || finishedL) && (!onLaneR || finishedR) && (winner == undefined || lampNumber > 9)) {
    status = 0;

    setAllLampOff();
    Serial.println("END!");
  }
}

void timingCheckWinner() {
  if (winner == undefined && onLaneL && onLaneR && (finishedL || finishedR)) {
    bool validL = finishedL && timingElapsedL > 0 && timingReactionL >= timingGreen;
    bool validR = finishedR && timingElapsedR > 0 && timingReactionR >= timingGreen;

    if (validL && (!validR || timingElapsedL < timingElapsedR)) winner = left;
    else if (validR && (!validL || timingElapsedR < timingElapsedL)) winner = right;
  }
}

void timingSendValues() {
  if (!sentRTL && timingReactionL > 0) {
    sendValue("RTL", timingReactionL - timingGreen);
    sentRTL = true;
  }

  if (!sentP1L && timingP1L > 0) {
    sendValue("P1L", timingP1L - timingReactionL);
    sentP1L = true;
  }

  if (!sentP2L && timingP2L > 0) {
    sendValue("P2L", timingP2L - timingReactionL);
    sentP2L = true;
  }

  if (!sentSFL && timingSpeedFinalL > 0 && timingElapsedL > 0) {
    sendSpeed("SFL", timingSpeedFinalL, timingElapsedL);
    sentSFL = true;
  }
  
  if (!sentETL && timingElapsedL > 0) {
    sendValue("ETL", timingElapsedL - timingReactionL);
    sentETL = true;
  }

  if (!sentRTR && timingReactionR > 0) {
    sendValue("RTR", timingReactionR - timingGreen);
    sentRTR = true;
  }

  if (!sentP1R && timingP1R > 0) {
    sendValue("P1R", timingP1R - timingReactionR);
    sentP1R = true;
  }

  if (!sentP2R && timingP2R > 0) {
    sendValue("P2R", timingP2R - timingReactionR);
    sentP2R = true;
  }

  if (!sentSFR && timingSpeedFinalR > 0 && timingElapsedR > 0) {
    sendSpeed("SFR", timingSpeedFinalR, timingElapsedR);
    sentSFR = true;
  }
  
  if (!sentETR && timingElapsedR > 0) {
    sendValue("ETR", timingElapsedR - timingReactionR);
    sentETR = true;
  }

  if (!sentWinner && winner != undefined) {
    if (winner == left) Serial.println("WDL;");
    else Serial.println("WDR;");

    sentWinner = true;
  }
}

void sendValue(const String& response, long value) {
  int valInt, valFrac;

  valInt = abs(value) / 1000;
  valFrac = abs(value) - (valInt * 1000);

  Serial.print(response);

  if (value < 0) Serial.print("-");

  Serial.print(valInt);
  Serial.print(".");

  if (valFrac < 10) Serial.print("00");
  else if (valFrac < 100) Serial.print("0");

  Serial.print(valFrac);
  Serial.println(";");
}

void sendSpeed(const String& response, unsigned long startTime, unsigned long endTime) {
  if (endTime < startTime) return;

  float speed = (float)SENSOR_SPEED_DISTANCE / (endTime - startTime) * 3600;

  Serial.print(response);
  Serial.print(int(speed));
  Serial.println(";");
}

void timingLampReaction() {
  if (onLaneL) { 
    if (timingReactionL > 0) {
      if (timingReactionL < timingGreen && !isHigh(LED_RED_L)) {
        digitalWrite(LED_RED_L, HIGH);

        if (proTree) {
          digitalWrite(LED_AMBER1_L, LOW);
          digitalWrite(LED_AMBER2_L, LOW);
          digitalWrite(LED_AMBER3_L, LOW);
        }
      }
      else if (timingReactionL >= timingGreen && !isHigh(LED_GREEN_L)) {
        digitalWrite(LED_AMBER1_L, LOW);
        digitalWrite(LED_AMBER2_L, LOW);
        digitalWrite(LED_AMBER3_L, LOW);
        digitalWrite(LED_GREEN_L, HIGH);
      }

      if (isHigh(LED_PRESTAGE_L)) digitalWrite(LED_PRESTAGE_L, LOW);
      if (isHigh(LED_STAGE_L)) digitalWrite(LED_STAGE_L, LOW);
    }
    else sensorLampOn(LDR_PRESTAGE_L, LED_PRESTAGE_L);
  }

  if (onLaneR) {
    if (timingReactionR > 0) {
      if (timingReactionR < timingGreen && !isHigh(LED_RED_R)) {
        digitalWrite(LED_RED_R, HIGH);

        if (proTree) {
          digitalWrite(LED_AMBER1_R, LOW);
          digitalWrite(LED_AMBER2_R, LOW);
          digitalWrite(LED_AMBER3_R, LOW);
        }
      }
      else if (timingReactionR >= timingGreen && !isHigh(LED_GREEN_R)) {
        digitalWrite(LED_AMBER1_R, LOW);
        digitalWrite(LED_AMBER2_R, LOW);
        digitalWrite(LED_AMBER3_R, LOW);
        digitalWrite(LED_GREEN_R, HIGH);
      }

      if (isHigh(LED_PRESTAGE_R)) digitalWrite(LED_PRESTAGE_R, LOW);
      if (isHigh(LED_STAGE_R)) digitalWrite(LED_STAGE_R, LOW);
    }
    else sensorLampOn(LDR_PRESTAGE_R, LED_PRESTAGE_R);
  }
}

void timingLampSequence() {
  if (lampNumber > 3) return;

  if (isHigh(LED_BLUE)) digitalWrite(LED_BLUE, LOW);

  if (onLaneL && timingReactionL == 0) {
    if (proTree && lampNumber == 0) {
      digitalWrite(LED_AMBER1_L, HIGH);
      digitalWrite(LED_AMBER2_L, HIGH);
      digitalWrite(LED_AMBER3_L, HIGH);
    } 
    else if (lampNumber == 0) {
      digitalWrite(LED_AMBER1_L, HIGH);
    }
    else if (lampNumber == 1) {
      digitalWrite(LED_AMBER1_L, LOW);
      digitalWrite(LED_AMBER2_L, HIGH);
    }
    else if (lampNumber == 2) {
      digitalWrite(LED_AMBER2_L, LOW);
      digitalWrite(LED_AMBER3_L, HIGH);
    }
    else if (lampNumber == 3) {
      digitalWrite(LED_AMBER1_L, LOW);
      digitalWrite(LED_AMBER2_L, LOW);
      digitalWrite(LED_AMBER3_L, LOW);
      digitalWrite(LED_GREEN_L, HIGH);
    }
  }

  if (onLaneR && timingReactionR == 0) { 
    if (proTree && lampNumber == 0) {
      digitalWrite(LED_AMBER1_R, HIGH);
      digitalWrite(LED_AMBER2_R, HIGH);
      digitalWrite(LED_AMBER3_R, HIGH);
    }
    else if (lampNumber == 0) {
      digitalWrite(LED_AMBER1_R, HIGH);
    }
    else if (lampNumber == 1) {
      digitalWrite(LED_AMBER1_R, LOW);
      digitalWrite(LED_AMBER2_R, HIGH);
    }
    else if (lampNumber == 2) {
      digitalWrite(LED_AMBER2_R, LOW);
      digitalWrite(LED_AMBER3_R, HIGH);
    }
    else if (lampNumber == 3) {
      digitalWrite(LED_AMBER1_R, LOW);
      digitalWrite(LED_AMBER2_R, LOW);
      digitalWrite(LED_AMBER3_R, LOW);
      digitalWrite(LED_GREEN_R, HIGH);
    }
  }

  if (proTree && lampNumber == 0) lampNumber = 3;
  else lampNumber++;
}

void timingLampWinner() {
  if (lampNumber > 9) return;

  if (winner == left) {
    if (lampNumber == 4 || lampNumber == 8) {
      digitalWrite(LED_AMBER1_L, HIGH);
      digitalWrite(LED_AMBER2_L, LOW);
      digitalWrite(LED_AMBER3_L, LOW);
    }
    else if (lampNumber == 5 || lampNumber == 7) {
      digitalWrite(LED_AMBER1_L, LOW);
      digitalWrite(LED_AMBER2_L, HIGH);
      digitalWrite(LED_AMBER3_L, LOW);
    }
    else if (lampNumber == 6) {
      digitalWrite(LED_AMBER1_L, LOW);
      digitalWrite(LED_AMBER2_L, LOW);
      digitalWrite(LED_AMBER3_L, HIGH);
    }
    else {
      digitalWrite(LED_AMBER1_L, LOW);
      digitalWrite(LED_AMBER2_L, LOW);
      digitalWrite(LED_AMBER3_L, LOW);
    }
  }

  if (winner == right) {
    if (lampNumber == 4 || lampNumber == 8) {
      digitalWrite(LED_AMBER1_R, HIGH);
      digitalWrite(LED_AMBER2_R, LOW);
      digitalWrite(LED_AMBER3_R, LOW);
    }
    else if (lampNumber == 5 || lampNumber == 7) {
      digitalWrite(LED_AMBER1_R, LOW);
      digitalWrite(LED_AMBER2_R, HIGH);
      digitalWrite(LED_AMBER3_R, LOW);
    }
    else if (lampNumber == 6) {
      digitalWrite(LED_AMBER1_R, LOW);
      digitalWrite(LED_AMBER2_R, LOW);
      digitalWrite(LED_AMBER3_R, HIGH);
    }
    else 
    {
      digitalWrite(LED_AMBER1_R, LOW);
      digitalWrite(LED_AMBER2_R, LOW);
      digitalWrite(LED_AMBER3_R, LOW);
    }
  }

  lampNumber++;
}