#define BUTTON_COUNT 4
#define STEP_COUNT   22
#define IDLE_PERIOD      1000
#define RESET_PERIOD     5000
#define CLICK_PERIOD     100
#define DEBOUNCE_PERIOD  50

#define NOBUTTON         255
uint8_t buttons[BUTTON_COUNT] = {34, 35, 32, 33};
uint8_t leds[BUTTON_COUNT] = {14, 27, 26, 25};
#define LED_OUTPUT 12
#define ON_OFF     13
#define BUZZER     23

bool gameStarted = false;

uint8_t steps[STEP_COUNT] = {
  2, 2, 2, 2, 2, 2,
  4, 4, 4, 4, 4, 4, 4,
  1, 1, 1, 1,
  3, 3, 3, 3, 3
};

uint8_t current_step;
/***************************************************************
 ** Utility Functions
 ***************************************************************/
void init_ports(void) {
  for (uint8_t i=0; i<BUTTON_COUNT; i++) {
    pinMode(buttons[i], INPUT_PULLUP);
    pinMode(leds[i], OUTPUT);
    digitalWrite(leds[i], LOW);
  }
  pinMode(LED_OUTPUT, OUTPUT);
  digitalWrite(LED_OUTPUT, LOW);
  pinMode(ON_OFF, INPUT_PULLUP);
  pinMode(BUZZER, OUTPUT);
  digitalWrite(BUZZER, LOW);
}

void beep(uint16_t duration) {
  digitalWrite(BUZZER, HIGH);
  delay(duration);
  digitalWrite(BUZZER, LOW);
}

void all_leds(uint8_t state) {
  for (uint8_t i=0; i<BUTTON_COUNT; i++) {
    digitalWrite(leds[i], state);
  }
}

void blink_leds(uint8_t times, uint16_t ms) {
  for (uint8_t i=0; i<times; i++) {
    all_leds(LOW);
    delay(ms);
    all_leds(HIGH);
    delay(ms);
  }
}

/***************************************************************
 ** Buttons
 ***************************************************************/
uint8_t get_pressed_button(void) {
  uint8_t i=0;
  bool down = false;
  while (i<BUTTON_COUNT && !down) {
    if (digitalRead(buttons[i]) == LOW) {
      delay(DEBOUNCE_PERIOD);
      if (digitalRead(buttons[i]) == LOW) {
        while(digitalRead(buttons[i]) == LOW)
          ;
        down = true;
      }
    }
    i++;
  }
  if (down) {
    return i;
  } else {
    return NOBUTTON;
  }
}

/***************************************************************
 ** Game Handling
 ***************************************************************/
uint8_t correct_button(void) {
  return steps[current_step];
}

bool game_won(void) {
  return current_step == STEP_COUNT;
}

void start_game(bool blink) {
  if (gameStarted) return;
  Serial.println("Starting Game");
  for (uint8_t i=0; i<4; i++) {
    digitalWrite(leds[i], HIGH);
    delay(100);
    digitalWrite(leds[i], LOW);
    delay(100);
  }
  digitalWrite(LED_OUTPUT, LOW);
  if (blink) {
    blink_leds(3, 200);
  }
  gameStarted  = true;
  current_step = 0;
}

void do_success(void) {
  digitalWrite(LED_OUTPUT, HIGH);
}

void stop_game(void) {
  if (!gameStarted) return;
  Serial.println("Stopping Game");
  all_leds(LOW);
  digitalWrite(LED_OUTPUT, LOW);
  gameStarted = false;
  current_step = 0;
}

void game_loop(void) {
  uint32_t now = millis();
  uint8_t pressed_button = get_pressed_button();
  if (pressed_button != NOBUTTON) {
    if (pressed_button == correct_button()) {
      current_step++;
    }
    Serial.println(current_step);
  }
}

void handle_win(void) {
  digitalWrite(LED_OUTPUT, HIGH);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  init_ports();
}

void loop() {
  if (digitalRead(ON_OFF) == LOW) {
    if (game_won()) {
      handle_win();
    } else {
      start_game(true);
      game_loop();
    }
  } else {
    stop_game();
  }
}