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

#define RapidFireTableSize 40

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

int val = 1;
int prevVal = 1;

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 oldestXArrow;
struct arrow newestXArrow;
struct arrow oldestYArrow;
struct arrow newestYArrow;



#define ButtonInfoTableSize 9
struct ButtonInfo buttonInfoTable[ButtonInfoTableSize];


void perms() {
  if(val == 0){
	  if (rapidFireTable[index] == 1){
	    SwitchControlLibrary().PressButtonA();
	  }
	  if (rapidFireTable[index] == 2){
	    SwitchControlLibrary().ReleaseButtonA();
	  }
  }
	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();

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

  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 = 20;
  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;

}

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


  for (int i=0; i<4; i++){
    arrowTable[i].current = digitalRead(arrowTable[i].pinNumber);
    
    struct arrow *newestArrow;
    if(i % 2 == 0){ // Y axis
      newestArrow = &newestYArrow;
    }
    else { // X axis 
      newestArrow = &newestXArrow;
    }

    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(newestYArrow.older->arrowSymbol == Hat::TOP){
    if(newestXArrow.older->arrowSymbol == Hat::LEFT){
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::TOP_LEFT));
    }
    else if(newestXArrow.older->arrowSymbol == Hat::CENTER) {
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::TOP));
    }
    else { // Hat::RIGHT
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::TOP_RIGHT));
    }
  } 
  if(newestYArrow.older->arrowSymbol == Hat::CENTER){
    if(newestXArrow.older->arrowSymbol == Hat::LEFT){
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::LEFT));
    }
    else if(newestXArrow.older->arrowSymbol == Hat::CENTER) {
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::CENTER));
    }
    else { // Hat::RIGHT
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::RIGHT));
    }
  }
  if(newestYArrow.older->arrowSymbol == Hat::BOTTOM){ 
    if(newestXArrow.older->arrowSymbol == Hat::LEFT){
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::BOTTOM_LEFT));
    }
    else if(newestXArrow.older->arrowSymbol == Hat::CENTER) {
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::BOTTOM));
    }
    else { // Hat::RIGHT
      SwitchControlLibrary().MoveHat(static_cast<uint8_t>(Hat::BOTTOM_RIGHT));
    }
  }



  //SwitchControlLibrary().MoveHat(static_cast<uint8_t>(newestArrow.older->arrowSymbol));


}