#include <EEPROM.h>
#include "ps2_Keyboard.h"
#include "ps2_NullDiagnostics.h"
#include "ps2_UsbTranslator.h"
#include "HID-Project.h"

#define DEBUG 1

// with serial.print...
#ifdef DEBUG
#define SP(a) Serial.print(a);
#define SPLN(a) Serial.println(a);
#else
#define SP(a) ;
#define SPLN(a) ;
#endif

static ps2::NullDiagnostics nd;
static ps2::UsbTranslator<ps2::NullDiagnostics> keyMapping(nd);
static ps2::Keyboard<4, 2, 16, ps2::NullDiagnostics> ps2Keyboard(nd);
static ps2::UsbKeyboardLeds ledValueLastSentToPs2 = ps2::UsbKeyboardLeds::none;
uint8_t activeFilter = 255;

class Filter {
 private:
  uint8_t m_keys[8] = {0};
  uint8_t m_number;
  uint8_t m_numKeys;
 public:
  Filter(uint8_t number) {
    m_number = number;
    m_numKeys = 0;
  }
  bool loadFromEEPROM() {
    uint8_t offset = m_number * 10;    // 10 bytes per filter config
    m_numKeys = EEPROM.read(offset); // number of keys
    if (m_numKeys > 8 || m_numKeys == 0) {
      m_numKeys = 0;
      return false;
    }
    uint8_t chksum = 0;
    for (uint8_t i = 0; i < m_numKeys; ++i) {
      uint8_t key = EEPROM.read(offset + i + 1);
      m_keys[i] = key;
      chksum += key;
    }
    // validate checksum
    if (chksum != EEPROM.read(offset + m_numKeys + 1)) {
      SP(m_number);
      SPLN(F(" checksum failed"));
      m_numKeys = 0;
      return false;
    }
    SP(m_number);
    SP(F(" loaded "));
    SP(m_numKeys);
    SPLN(F(" keys"));
    return true;
  }
  void clear() {
    for (uint8_t i = 0; i < m_numKeys; ++i) {
      m_keys[i] = 0;
    }
    m_numKeys = 0;
  }
  void addKey(uint8_t key) {
    for (uint8_t i = 0; i < m_numKeys; ++i) {
      if (m_keys[i] == key) {
        return;
      }
    }
    if (m_numKeys < 7) {
      m_keys[m_numKeys++] = key;
    }
  }
  bool writeToEEPROM() {
    uint8_t offset = m_number * 10;    // 10 bytes per filter config
    EEPROM.write(offset, m_numKeys); // number of keys
    uint8_t chksum = 0;
    for (uint8_t i = 0; i < m_numKeys; ++i) {
      EEPROM.write(offset + i + 1, m_keys[i]);
      chksum += m_keys[i];
    }
    // save checksum
    EEPROM.write(offset + m_numKeys + 1, chksum);
    SP(m_number);
    SP(F(" saved "));
    SP(m_numKeys);
    SPLN(F(" keys"));
    return true;
  }
  bool isFiltered(uint8_t key) {
    for (uint8_t i = 0; i < m_numKeys; ++i) {
      if (m_keys[i] == key) {
        return true;
      }
    }
    return false;
  }
};

#define NUM_FILTERS 12
Filter* filters[NUM_FILTERS] = {0};
enum States {
  NONE = 0,
  PROGRAMMING_SHIFT,
  PROGRAMMING_SCROLL,
  PROGRAMMING_SELECT,
  PROGRAMMING_ADD,
  PROGRAMMING_DONE,
  APPLY_SHIFT,
  APPLY_SCROLL,
  APPLY_SELECT,
  APPLY_DONE
} currentState = NONE;
uint32_t pressedTime = 0;
uint8_t programFilter = 255;

// the setup function runs once when you press reset or power the board
void setup() {
  ps2Keyboard.begin();
  BootKeyboard.begin();
#ifdef DEBUG
  delay(2000);
  Serial.begin(115200);
  SPLN(F("ready"));
#endif
  for (uint8_t i = 0; i < NUM_FILTERS; ++i) {
    filters[i] = new Filter(i);
    filters[i]->loadFromEEPROM();
  }
  filters[11]->clear();
  filters[11]->addKey(KEY_E);
  activeFilter = 11;
  
}
#define BLINK_OFF  0
#define BLINK_VERY_SLOW  2000
#define BLINK_SLOW  1000
#define BLINK_MEDIUM  500
#define BLINK_FAST  250
uint16_t blinkTimer = 0;
void loop() {
    uint32_t now = millis();
    ps2::UsbKeyboardLeds newLedState = (ps2::UsbKeyboardLeds)BootKeyboard.getLeds();
    if (blinkTimer != BLINK_OFF) {
      uint8_t diff = (now - pressedTime) / blinkTimer;
      if (diff & 1) newLedState = (ps2::UsbKeyboardLeds)((uint8_t)newLedState ^ (uint8_t)ps2::UsbKeyboardLeds::scrollLock);
    }
    if (newLedState != ledValueLastSentToPs2)
    {
        ps2Keyboard.sendLedStatus(keyMapping.translateLeds(newLedState));
        ledValueLastSentToPs2 = newLedState;
    }

    ps2::KeyboardOutput scanCode = ps2Keyboard.readScanCode();
    if (scanCode != ps2::KeyboardOutput::none && scanCode != ps2::KeyboardOutput::garbled) {
        ps2::UsbKeyAction action = keyMapping.translatePs2Keycode(scanCode);
        KeyboardKeycode hidCode = (KeyboardKeycode)action.hidCode;
        switch(currentState) {
          case NONE:
            if (action.gesture == ps2::UsbKeyAction::KeyDown) {
              if (hidCode == KeyboardKeycode::KEY_LEFT_SHIFT) {
                currentState = PROGRAMMING_SHIFT;
                SPLN(F("to PROGRAMMING_SHIFT"));
              } else if (hidCode == KeyboardKeycode::KEY_RIGHT_SHIFT) {
                currentState = APPLY_SHIFT;
                SPLN(F("to APPLY_SHIFT"));
              }
            }
            break;
          case PROGRAMMING_SHIFT:
            if (action.gesture == ps2::UsbKeyAction::KeyDown && hidCode == KeyboardKeycode::KEY_SCROLL_LOCK) {
              currentState = PROGRAMMING_SCROLL;
              blinkTimer = BLINK_SLOW;
              pressedTime = now;
              SPLN(F("to PROGRAMMING_SCROLL"));
            } else if (action.gesture != ps2::UsbKeyAction::KeyDown || hidCode != KeyboardKeycode::KEY_LEFT_SHIFT) {
              currentState = NONE;
              SPLN(F("PROGRAMMING_SHIFT_to NONE"));
            }
            break;
          case PROGRAMMING_SCROLL:
            // to work around a library quirk insted of 
            // if (action.gesture == ps2::UsbKeyAction::KeyUp && hidCode == KeyboardKeycode::KEY_SCROLL_LOCK && (now - pressedTime) > 5000) {
             if (action.gesture == ps2::UsbKeyAction::None && hidCode == 0 && (now - pressedTime) > 5000) {
              blinkTimer = BLINK_MEDIUM;
              currentState = PROGRAMMING_SELECT;
              pressedTime = now;
              SPLN(F("to PROGRAMMING_SELECT"));
            } else if (action.gesture == ps2::UsbKeyAction::KeyUp || (hidCode != KEY_LEFT_SHIFT && hidCode != KEY_SCROLL_LOCK)) {
              currentState = NONE;
              blinkTimer = BLINK_OFF;
              SPLN(F("PROGRAMMING_SCROLL to NONE"));
            }
            break;
          case PROGRAMMING_SELECT:
            if (action.gesture == ps2::UsbKeyAction::KeyDown) {
              if(hidCode >= KeyboardKeycode::KEY_F1 && hidCode <= KeyboardKeycode::KEY_F12) {
                programFilter = hidCode - KeyboardKeycode::KEY_F1;
                filters[programFilter]->clear();
                blinkTimer = BLINK_FAST;
                SP(F("set filter to "));
                SPLN(programFilter);
                currentState = PROGRAMMING_ADD;
                SPLN(F("to PROGRAMMING_ADD"));
                return; // do not send
              } else {
                currentState = NONE;
                programFilter = 255;
                blinkTimer = BLINK_OFF;
                SP(F("PROGRAMMING_SELECT != F to NONE"));
                SPLN(hidCode);
              }
              return;
            }
            break;
          case PROGRAMMING_ADD:
            if (action.gesture == ps2::UsbKeyAction::KeyDown && hidCode == KeyboardKeycode::KEY_SCROLL_LOCK) {
              SPLN(F("to PROGRAMMING_DONE"));
              currentState = PROGRAMMING_DONE;
            }
            if (action.gesture == ps2::UsbKeyAction::KeyUp && hidCode != (KeyboardKeycode::KEY_F1 + programFilter)) {
              SP(F("add key "));
              SPLN(hidCode);
              filters[programFilter]->addKey(hidCode);
            }
            return;
          case PROGRAMMING_DONE:
            if (action.gesture == ps2::UsbKeyAction::KeyUp && hidCode == KeyboardKeycode::KEY_SCROLL_LOCK) {
              SPLN(F("to NONE"));
              filters[programFilter]->writeToEEPROM();
              currentState = NONE;
              blinkTimer = BLINK_OFF;
            }
            return;
          case APPLY_SHIFT:
            if (action.gesture == ps2::UsbKeyAction::KeyDown && hidCode == KeyboardKeycode::KEY_SCROLL_LOCK) {
              currentState = APPLY_SCROLL;
              blinkTimer = BLINK_SLOW;
              pressedTime = now;
              SPLN(F("to APPLY_SCROLL"));
            } else if (action.gesture != ps2::UsbKeyAction::KeyDown || hidCode != KeyboardKeycode::KEY_RIGHT_SHIFT) {
              currentState = NONE;
              SPLN(F("APPLY_SHIFT_to NONE"));
            }
            break;
          case APPLY_SCROLL:
            // to work around a library quirk insted of 
            // if (action.gesture == ps2::UsbKeyAction::KeyUp && hidCode == KeyboardKeycode::KEY_SCROLL_LOCK && (now - pressedTime) > 1000) {
            if (action.gesture == ps2::UsbKeyAction::None && hidCode == 0 && (now - pressedTime) > 1000) {
              blinkTimer = BLINK_MEDIUM;
              currentState = APPLY_SELECT;
              pressedTime = now;
              SPLN(F("to APPLY_SELECT"));
            } else if (action.gesture == ps2::UsbKeyAction::KeyUp || (hidCode != KEY_RIGHT_SHIFT && hidCode != KEY_SCROLL_LOCK)) {
              currentState = NONE;
              blinkTimer = BLINK_OFF;
              SPLN(F("APPLY_SCROLL to NONE"));
            }
            break;
          case APPLY_SELECT:
            if (action.gesture == ps2::UsbKeyAction::KeyDown) {
              if(hidCode == KeyboardKeycode::KEY_ESC || (hidCode >= KeyboardKeycode::KEY_F1 && hidCode <= KeyboardKeycode::KEY_F12)) {
                activeFilter = hidCode == KeyboardKeycode::KEY_ESC ? 255 : hidCode - KeyboardKeycode::KEY_F1;
                SP(F("set filter to "));
                SPLN(activeFilter);
                currentState = APPLY_DONE;
                SPLN(F("to APPLY_DONE"));
                return; // do not send
              } else {
                currentState = NONE;
                blinkTimer = BLINK_OFF;
                SP(F("APPLY_SELECT != F to NONE"));
                SPLN(hidCode);
              }
              return;
            }
            break;
          case APPLY_DONE:
            if (action.gesture == ps2::UsbKeyAction::KeyUp) {
              if(hidCode == KeyboardKeycode::KEY_ESC) {
                blinkTimer = BLINK_OFF;
                currentState = NONE;
                SPLN(F("to NONE"));
              }
              if (hidCode >= KeyboardKeycode::KEY_F1 && hidCode <= KeyboardKeycode::KEY_F12) {
                blinkTimer = BLINK_VERY_SLOW;
                currentState = NONE;
                SPLN(F("to NONE"));
              }
            }
            return;
        }
        if (activeFilter != 255) {
          if (filters[activeFilter]->isFiltered(hidCode)) {
            action.gesture = ps2::UsbKeyAction::None;
          }
        }
        
        switch (action.gesture) {
          case ps2::UsbKeyAction::KeyDown:
            BootKeyboard.press(hidCode);
            break;
          case ps2::UsbKeyAction::KeyUp:
            BootKeyboard.release(hidCode);
            break;
        }
    }
}