#include <stdint.h>
#include <stdbool.h>

bool digitalRead(int);
void digitalWrite(int, bool);

#define KEY_COUNT       6
#define KEY_EMPTY       0xff

uint8_t key_pins[KEY_COUNT] = { 1, 2, 3, 4, 5, 6 };
uint8_t led_pins[KEY_COUNT] = { 7, 8, 9, 10, 11, 12 };

bool key_input[KEY_COUNT];
uint8_t key_order[KEY_COUNT];

void setup(void)
{
  for (uint8_t i = 0; i < KEY_COUNT; i++)
    key_order[i] = KEY_EMPTY;
}

static void input_keys(void)
{
  for (uint8_t i = 0; i < KEY_COUNT; i++)
    key_input[i] = digitalRead( key_pins[i] );
}

static void remove_first_key(uint8_t key)
{
  if (key_order[0] != key)                      // not at first
    return;
  for (uint8_t i = 0; i < KEY_COUNT-1; i++)     // copy down
    key_order[i] = key_order[i+1];
  key_order[KEY_COUNT-1] = KEY_EMPTY;
}

static void insert_next_key(uint8_t key)
{
  for (uint8_t i = 0; i < KEY_COUNT; i++)
  {
    if (key_order[i] == key)                    // already in list
      break;
    if (key_order[i] == KEY_EMPTY)              // insert here
    {
      key_order[i] = key;
      break;
    }
  }
}

static void handle_keys(void)
{
  for (uint8_t i = 0; i < KEY_COUNT; i++)
    if (key_input[i] == true)
      insert_next_key(i);
    else
      remove_first_key(i);
}

static void first_led_on(void)
{
  for (uint8_t i = 0; i < KEY_COUNT; i++)
    digitalWrite(led_pins[i], i == key_order[0]);       // only first LED on
}

void loop(void)
{
  input_keys();
  handle_keys();
  first_led_on();
}