const int Z = -1;
const int RESET_PIN = 12;
const int STAGING_PIN = 11;
enum RaceState {
  STAGING,
  STAGED,
  YELLOWS,
  GREEN,
  FINISHED,
  RED
};
RaceState rState;
unsigned long stagedStartTime;
unsigned long yellowsStartTime;
unsigned long finishedStartTime;
const int blinkInterval = 200;
int blinkState = LOW;
bool staged = false;

void setup() {
  pinMode(RESET_PIN, INPUT);
  pinMode(STAGING_PIN, INPUT);

  rState = RaceState::STAGING;
  reset();
}

void loop() {
  stagingChecks();
  yellowChecks();
  greenChecks();
  finishedChecks();
  redChecks();
  resetBtnCheck();
}

void stagingChecks() {
  staged = isStaged();

  if (rState == RaceState::STAGING || rState == RaceState::STAGED)
  {
    if (staged)
      stagingLight(HIGH);
    else
      stagingLight(LOW);
  }

  if (rState == RaceState::STAGING) {
    // Toggle on staging light
    if (staged) {
      rState = RaceState::STAGED;
      stagedStartTime = micros();
    } else {
      // If they were staged and now no longer, reset timer before yellows start.
      if (rState == RaceState::STAGED)
        stagedStartTime = 0;
      // if the yellows started and they unstage, red light
      else if (rState == RaceState::YELLOWS) {
        stagedStartTime = 0;
        rState = RaceState::RED;
      }
    }
  }

  if (rState == RaceState::STAGED && !staged) {
    rState = RaceState::STAGING;
  }

  // If staged for 2.5 seconds, start yellows
  unsigned long timeSinceStaged = micros() - stagedStartTime;
  if (rState == RaceState::STAGED && timeSinceStaged >= 2500000) {
    rState = RaceState::YELLOWS;
    stagedStartTime = 0;
    yellowsStartTime = micros();
  }
}

void yellowChecks() {
  unsigned long timeSinceYellowsStarted = micros() - yellowsStartTime;
  if (rState == RaceState::YELLOWS) {
    if (timeSinceYellowsStarted < 500000)
      yellow1st();
    else if (timeSinceYellowsStarted >= 500000 && timeSinceYellowsStarted < 1000000)
      yellow2nd();
    else if (timeSinceYellowsStarted >= 1000000 && timeSinceYellowsStarted < 1500000)
      yellow3rd();
    else if (timeSinceYellowsStarted >= 1500000) {
      yellowsStartTime = 0;
      rState = RaceState::GREEN;
    }

    if (!staged) {
      rState = RaceState::RED;
    }
  }
}

void greenChecks() {
  if (rState == RaceState::GREEN) {
    greenLight();

    // TODO: remove this delay that simulates a race
    delay(3000);
    finishedStartTime = micros();
    rState = RaceState::FINISHED;
  }
}

void redChecks() {
  if (rState == RaceState::RED) {
    redLight();
  }
}

void finishedChecks() {
  unsigned long timeSinceFinished = millis() - finishedStartTime;
  // TODO: Finish line sensor would determine this
  bool winner = true;
  if (rState == RaceState::FINISHED && winner) {
    if (timeSinceFinished >= blinkInterval) {
      finishedStartTime = millis();
      if (blinkState == LOW) {
        stagingLight(HIGH);
        blinkState = HIGH;
      }
      else {
        stagingLight(LOW);
        blinkState = LOW;
      }
    }
  }
}

void resetBtnCheck() {
  int buttonState = digitalRead(RESET_PIN);
  if (buttonState == HIGH)
    reset();
}

bool isStaged() {
  return digitalRead(STAGING_PIN) == HIGH;
}

void greenLight() {
  charlieplex(LOW, HIGH, Z);
}

void stagingLight(int state) {
  pinMode(13, OUTPUT);
  digitalWrite(13, state);
}

void yellow1st() {
  charlieplex(Z, HIGH, LOW);
}

void yellow2nd() {
  charlieplex(Z, LOW, HIGH);
}

void yellow3rd() {
  charlieplex(LOW, Z, HIGH);
}

void redLight() {
  charlieplex(HIGH, LOW, Z);
}

void shutOffCharlieplex() {
    // Turn off all pins
  pinMode(2, INPUT);
  digitalWrite(2, LOW);

  pinMode(3, INPUT);
  digitalWrite(3, LOW);

  pinMode(4, INPUT);
  digitalWrite(4, LOW);
}


void charlieplex(int pin1, int pin2, int pin3) {
  shutOffCharlieplex();

  // Set pins state and voltage.
  pinMode(2, pin1 >= 0 ? OUTPUT : INPUT);
  digitalWrite(2, pin1 >= 0 ? pin1 : LOW);

  pinMode(3, pin2 >= 0 ? OUTPUT : INPUT);
  digitalWrite(3, pin2 >= 0 ? pin2 : LOW);

  pinMode(4, pin3 >= 0 ? OUTPUT : INPUT);
  digitalWrite(4, pin3 >= 0 ? pin3 : LOW);
}

void reset() {
  shutOffCharlieplex();
  stagingLight(LOW);
  stagedStartTime = micros();
  yellowsStartTime = 0;
  finishedStartTime = 0;
  rState = RaceState::STAGING;
}