#include <MsTimer2.h>
#include <SwitchControlLibrary.h>

#define RapidFireTableSize 34

int loopCount = 0;
int state = 0;
int rapidFireTable[RapidFireTableSize];
bool rapidPressState = 0;
int index = 0;

int val = 1;
int prevVal = 1;

int gameMode = 0;

struct arrow
{
  int pinNumber;
  int current;
  int prev;
  struct arrow *older;
  struct arrow *newer;
  Hat arrowSymbol;
};

struct ButtonInfo
{
  int pinNumber;
  bool prev;
  bool current;
  Button buttonSymbol;
};

struct arrow arrowTable[4];
struct arrow oldestArrow;
struct arrow newestArrow;
struct arrow oldestXArrow;
struct arrow newestXArrow;
struct arrow oldestYArrow;
struct arrow newestYArrow;

#define ButtonInfoTableSize 9
struct ButtonInfo buttonInfoTable[ButtonInfoTableSize];

Hat last = Hat::CENTER;

void perms()
{
  if (val == 0)
  {
    if (rapidFireTable[index] == 1)
    {
      SwitchControlLibrary().PressButtonA();
      rapidPressState = true;
    }
    if (rapidFireTable[index] == 2)
    {
      SwitchControlLibrary().ReleaseButtonA();
      rapidPressState = false;
    }
  }
  index = (index + 1) % RapidFireTableSize;
}

void setupButton(ButtonInfo *but, int pinNumber, Button buttonSymbol)
{
  but->pinNumber = pinNumber;
  but->buttonSymbol = buttonSymbol;
  pinMode(pinNumber, INPUT_PULLUP);
  but->prev = 1;
  but->current = 1;
}

void setup()
{
  rapidFireTable[0] = 1;
  rapidFireTable[RapidFireTableSize / 2] = 2;
  SwitchControlLibrary();

  setupButton(&(buttonInfoTable[0]), 3, Button::B);
  setupButton(&(buttonInfoTable[1]), 4, Button::X);
  setupButton(&(buttonInfoTable[2]), 5, Button::Y);
  setupButton(&(buttonInfoTable[3]), 16, Button::L);
  setupButton(&(buttonInfoTable[4]), 10, Button::R);
  setupButton(&(buttonInfoTable[5]), 15, Button::PLUS);
  setupButton(&(buttonInfoTable[6]), 18, Button::HOME);
  setupButton(&(buttonInfoTable[7]), 19, Button::CAPTURE);
  setupButton(&(buttonInfoTable[8]), 14, Button::MINUS);

  pinMode(2, INPUT_PULLUP);

  arrowTable[0].pinNumber = 6;
  arrowTable[0].arrowSymbol = Hat::TOP;
  arrowTable[1].pinNumber = 7;
  arrowTable[1].arrowSymbol = Hat::LEFT;
  arrowTable[2].pinNumber = 8;
  arrowTable[2].arrowSymbol = Hat::BOTTOM;
  arrowTable[3].pinNumber = 9;
  arrowTable[3].arrowSymbol = Hat::RIGHT;

  for (int i = 0; i < 4; i++)
  {
    pinMode(arrowTable[i].pinNumber, INPUT_PULLUP);
    arrowTable[i].older = NULL;
    arrowTable[i].newer = NULL;
  }

  oldestXArrow.older = NULL;
  oldestXArrow.newer = &newestXArrow;
  oldestXArrow.arrowSymbol = Hat::CENTER;
  newestXArrow.older = &oldestXArrow;
  newestXArrow.newer = NULL;

  oldestYArrow.older = NULL;
  oldestYArrow.newer = &newestYArrow;
  oldestYArrow.arrowSymbol = Hat::CENTER;
  newestYArrow.older = &oldestYArrow;
  newestYArrow.newer = NULL;

  oldestArrow.older = NULL;
  oldestArrow.newer = &newestArrow;
  oldestArrow.arrowSymbol = Hat::CENTER;
  newestArrow.older = &oldestArrow;
  newestArrow.newer = NULL;

  if (digitalRead(5) == 0) // check Y button for setting GameMode
  {
    gameMode = 1;
  }

  if (digitalRead(4) == 0) // check X button for setting GameMode
  {
    gameMode = 2;
    setupButton(&(buttonInfoTable[7]), 8, Button::L);
    arrowTable[2].pinNumber = 19;
    setupButton(&(buttonInfoTable[1]), 4, Button::R);
  }

  if(gameMode == 1){
    MsTimer2::set(1, perms);
    MsTimer2::start();
  }
}

void handleButtonInput(ButtonInfo *but)
{
  but->current = digitalRead(but->pinNumber);

  if (but->prev == 1 && but->current == 0)
  {
    SwitchControlLibrary().PressButton(but->buttonSymbol);
  }
  else if (but->prev == 0 && but->current == 1)
  {
    SwitchControlLibrary().ReleaseButton(but->buttonSymbol);
  }

  but->prev = but->current;
}

void moveHat(Hat dir)
{
  if (last != dir)
  {
    SwitchControlLibrary().MoveHat(static_cast<uint8_t>(dir));
    last = dir;
  }
}

void loop()
{

  if (gameMode == 1)
  {
    val = digitalRead(2);
    if (prevVal == 1 && val == 0)
    {
      SwitchControlLibrary().PressButtonA();
      rapidPressState = true;
      index = 1;
    }
    else if (prevVal == 0 && val == 1)
    {
      if (rapidPressState)
      {
        SwitchControlLibrary().ReleaseButtonA();
      }
    }
    prevVal = val;

    for (int i = 0; i < ButtonInfoTableSize; i++)
    {
      handleButtonInput(&(buttonInfoTable[i]));
    }

    for (int i = 0; i < 4; i++)
    {
      arrowTable[i].current = digitalRead(arrowTable[i].pinNumber);
      if (arrowTable[i].prev == 1 && arrowTable[i].current == 0)
      {
        // add arrow to the tail of linked list
        arrowTable[i].older = newestArrow.older;
        arrowTable[i].newer = &newestArrow;
        newestArrow.older->newer = &(arrowTable[i]);
        newestArrow.older = &(arrowTable[i]);
      }
      else if (arrowTable[i].prev == 0 && arrowTable[i].current == 1)
      {
        arrowTable[i].newer->older = arrowTable[i].older;
        arrowTable[i].older->newer = arrowTable[i].newer;
        arrowTable[i].newer = NULL;
        arrowTable[i].older = NULL;
      }
      arrowTable[i].prev = arrowTable[i].current;
    }

    if (newestArrow.older->arrowSymbol != last)
    {
      last = newestArrow.older->arrowSymbol;
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(newestArrow.older->arrowSymbol));
    }
  }
  else
  {
    val = digitalRead(2);
    if (prevVal == 1 && val == 0)
    {
      SwitchControlLibrary().PressButtonA();
      index = 1;
    }
    else if (prevVal == 0 && val == 1)
    {
      SwitchControlLibrary().ReleaseButtonA();
    }
    prevVal = val;

    for (int i = 0; i < ButtonInfoTableSize; i++)
    {
      handleButtonInput(&(buttonInfoTable[i]));
    }

    for (int i = 0; i < 4; i++)
    {
      arrowTable[i].current = digitalRead(arrowTable[i].pinNumber);
    }

    if (arrowTable[1].current == 0)
    {
      if (arrowTable[3].current == 0)
      {
        if (arrowTable[0].current == 0)
        {
          moveHat(Hat::TOP);
        }
        else
        {
          if (arrowTable[2].current == 0)
          {
            moveHat(Hat::BOTTOM);
          }
          else
          {
            moveHat(Hat::CENTER);
          }
        }
      }
      else
      {
        if (arrowTable[0].current == 0)
        {
          moveHat(Hat::TOP_LEFT);
        }
        else
        {
          if (arrowTable[2].current == 0)
          {
            moveHat(Hat::BOTTOM_LEFT);
          }
          else
          {
            moveHat(Hat::LEFT);
          }
        }
      }
    }
    else
    {
      if (arrowTable[3].current == 0)
      {
        if (arrowTable[0].current == 0)
        {
          moveHat(Hat::TOP_RIGHT);
        }
        else
        {
          if (arrowTable[2].current == 0)
          {
            moveHat(Hat::BOTTOM_RIGHT);
          }
          else
          {
            moveHat(Hat::RIGHT);
          }
        }
      }
      else
      {
        if (arrowTable[0].current == 0)
        {
          moveHat(Hat::TOP);
        }
        else
        {
          if (arrowTable[2].current == 0)
          {
            moveHat(Hat::BOTTOM);
          }
          else
          {
            moveHat(Hat::CENTER);
          }
        }
      }
    }
  }
}