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

#define tableSize 40

int loopCount = 0;
int state = 0;
int table[tableSize];
int index = 0;

int val = 1;
int prevVal = 1;

struct arrow {
  int pinNumber;
  int state;
};

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

struct arrow arrowTable[4];
#define ButtonInfoTableSize 2
struct ButtonInfo buttonInfoTable[ButtonInfoTableSize];


void perms() {
  if(val == 0){
	  if (table[index] == 1){
	    SwitchControlLibrary().PressButtonA();
	  }
	  if (table[index] == 2){
	    SwitchControlLibrary().ReleaseButtonA();
	  }
  }
	index = (index + 1) % 40;
}
/*
void button() {
  if (state == 0) {
    SwitchControlLibrary().PressButtonA();
    state = 1;
  }
  else {
    SwitchControlLibrary().ReleaseButtonA();
    state = 0;
  }
}
*/

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()
{
  table[0] = 1;
  table[tableSize/2] = 2;
  SwitchControlLibrary();

  MsTimer2::set(1, perms);
  MsTimer2::start();

  setupButton(&(buttonInfoTable[0]), 3, Button::B);
  setupButton(&(buttonInfoTable[1]), 15, Button::PLUS);

/*
  bButton.pinNumber = 3;
  pinMode(bButton.pinNumber, INPUT_PULLUP);
  bButton.prev = 1;
  bButton.current = 1;

  plusButton.pinNumber = 15;
  pinMode(plusButton.pinNumber, INPUT_PULLUP);
  plusButton.prev = 1;
  plusButton.current = 1;
*/


  pinMode(2, INPUT_PULLUP);

  arrowTable[0].pinNumber = 6;
  arrowTable[1].pinNumber = 7;
  arrowTable[2].pinNumber = 8;
  arrowTable[3].pinNumber = 9;
  
  for (int i=0; i<4; i++){
    pinMode(arrowTable[i].pinNumber, INPUT_PULLUP);
    arrowTable[i].state = 1;
  }


}

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 loop()
{
  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]));
}

/*
  bButton.current = digitalRead(bButton.pinNumber);

  if(bButton.prev == 1 && bButton.current ==0 ){
    SwitchControlLibrary().PressButtonB();
  }
  else if(bButton.prev == 0 && bButton.current ==1 ) {
    SwitchControlLibrary().ReleaseButtonB();
  }

  bButton.prev = bButton.current;



  plusButton.current = digitalRead(plusButton.pinNumber);

  if(plusButton.prev == 1 && plusButton.current ==0 ){
    SwitchControlLibrary().PressButtonPlus();
  }
  else if(plusButton.prev == 0 && plusButton.current ==1 ) {
    SwitchControlLibrary().ReleaseButtonPlus();
  }

  plusButton.prev = plusButton.current;
*/


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

  if(arrowTable[0].state == 0){
    SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::TOP));
  } 
  else if(arrowTable[2].state == 0){
    SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::BOTTOM));
  }
  else if(arrowTable[1].state == 0){
    SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::LEFT));
  }
  else if(arrowTable[3].state == 0){
    SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::RIGHT));
  }
  else{
    SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::CENTER));
  }

}