/*
 * 
 * TEENSY KEYBOARD CONTROLLER by Lucas Kleiss
 *
 * This software defines two types of modifiers:
 *    * Modifier Keys eg. MODIFIERKEY_SHIFT
 *    * Modifier Scan Codes eg. 96
 * These different types of modifiers serve different purposes and great care should be taken not to get them mixed up
 *
 * This software was specifically designed for the Focus FK-2001 XT Mechanical Keyboard
 *
 */

#define systemRequest 84
#define modCode 96
#define altModCode 97

#define clock digitalRead(clockPin)
#define data digitalRead(dataPin)

int clockPin = 3;
int dataPin = 4;
int switchPin = 22;
int keyTemp = 0;
int keyState[104];
int outKeyState[6] = {0, 0, 0, 0, 0, 0};
int outModState[4] = {0, 0, 0, 0};
int keyboardCode[] = {    
  0x00,    // Position 00 Unused
  0x29,    // Position 01 Esc 
  0x1E,    // Position 02 1 !
  0x1F,    // Position 03 2 @
  0x20,    // Position 04 3 #
  0x21,    // Position 05 4 $
  0x22,    // Position 06 5 %
  0x23,    // Position 07 6 ^
  0x24,    // Position 08 7 &
  0x25,    // Position 09 8 *
  0x26,    // Position 10 9 (
  0x27,    // Position 11 0 )
  0x2D,    // Position 12 - _
  0x2E,    // Position 13 = +
  0x2A,    // Position 14 Backspace
  0x2B,    // Position 15 Tab
  0x14,    // Position 16 Q
  0x1A,    // Position 17 W
  0x08,    // Position 18 E
  0x15,    // Position 19 R
  0x17,    // Position 20 T
  0x1C,    // Position 21 Y
  0x18,    // Position 22 U
  0x0C,    // Position 23 I
  0x12,    // Position 24 O
  0x13,    // Position 25 P
  0x2F,    // Position 26 [ {
  0x30,    // Position 27 ] }
  0x28,    // Position 28 Return
  0xE0,    // Position 29 Ctrl
  0x04,    // Position 30 A
  0x16,    // Position 31 S
  0x07,    // Position 32 D
  0x09,    // Position 33 F
  0x0A,    // Position 34 G
  0x0B,    // Position 35 H
  0x0D,    // Position 36 J
  0x0E,    // Position 37 K
  0x0F,    // Position 38 L
  0x33,    // Position 39 ; :
  0x34,    // Position 40 ' "
  0x35,    // Position 41 ` ~
  0xE1,    // Position 42 Left Shift
  0x31,    // Position 43 \ |
  0x1D,    // Position 44 Z
  0x1B,    // Position 45 X
  0x06,    // Position 46 C
  0x19,    // Position 47 V
  0x05,    // Position 48 B
  0x11,    // Position 49 N
  0x10,    // Position 50 M
  0x36,    // Position 51 , <
  0x37,    // Position 52 . >
  0x38,    // Position 53 / ?
  0xE5,    // Position 54 Right Shift
  0x46,    // Position 55 PrtSc
  0xE2,    // Position 56 Alt
  0x2C,    // Position 57 Space
  0x39,    // Position 58 Caps Lock
  0x3A,    // Position 59 F1
  0x3B,    // Position 60 F2
  0x3C,    // Position 61 F3
  0x3D,    // Position 62 F4
  0x3E,    // Position 63 F5
  0x3F,    // Position 64 F6
  0x40,    // Position 65 F7
  0x41,    // Position 66 F8
  0x42,    // Position 67 F9
  0x43,    // Position 68 F10
  0x53,    // Position 69 Num Lock
  0x47,    // Position 70 Scroll Lock
  0x5F,    // Position 71 Home 7
  0x60,    // Position 72 Up 8
  0x61,    // Position 73 PgUp 9
  0x56,    // Position 74 -
  0x5C,    // Position 75 Left 4
  0x5D,    // Position 76 5
  0x5E,    // Position 77 Right 6
  0x57,    // Position 78 +
  0x59,    // Position 79 End 1
  0x5A,    // Position 80 Down 2
  0x5B,    // Position 81 PgDn 3
  0x62,    // Position 82 Ins 0
  0x63     // Position 83 Del .
} ;
#define altKeyboardCode keyboardCode

boolean modCodeAct = false;
boolean altModCodeAct = false;

void setup()
{
  Serial.begin(9600);
  Serial.println("Connected");
  pinMode(clockPin, INPUT);
  pinMode(dataPin, INPUT);
  // pinMode(switchPin, OUTPUT);
  // digitalWrite(switchPin, HIGH);
  // delay(1000);
  // digitalWrite(switchPin, LOW); // Needed to start the Focus FK-2001 in AT mode before switching to XT mode
  // Keyboard.set_modifier(0);
  // Keyboard.set_key1(0);
  // Keyboard.set_key2(0);
  // Keyboard.set_key3(0);
  // Keyboard.set_key4(0);
  // Keyboard.set_key5(0);
  // Keyboard.set_key6(0);
  // Keyboard.send_now();
}

void loop()
{
  int x = 0;
  if (clock == LOW)
  {
    while (clock == LOW){}
    while (x < 8)
    {
      if (clock == HIGH)
      {
        if (x != 7)
        {
          keyTemp += (data << x);
        }
        else
        {
          keyState[keyTemp] = data;
        }
        x++;
        while (clock == HIGH){}
      }
      while (clock == LOW){}
    }
    //Code that requires the current key code goes here
    if (keyTemp == modCode || keyTemp == altModCode) // Allows the mod codes to register correctly
    {
      delayMicroseconds(100);
    }
    keyRegister (keyTemp, !keyState[keyTemp]); //Sends out the key presses every time it is run
    keyTemp = 0;
  }
}

// Registers all key presses

void keyRegister (int key, boolean pressed)
{
  if (modCodeAct)
  {
    if (altKeyboardCode[key] == MODIFIERKEY_CTRL || altKeyboardCode[key] == MODIFIERKEY_SHIFT || altKeyboardCode[key] == MODIFIERKEY_ALT || altKeyboardCode[key] == MODIFIERKEY_GUI)
    {
      setModKey (key, pressed, true);
    }  
    else
    {
      setNormKey (key, pressed, true);
    }
    modCodeAct = false;
  }
  else if (altModCodeAct)
  {
    if (altKeyboardCode[key] == MODIFIERKEY_CTRL || altKeyboardCode[key] == MODIFIERKEY_SHIFT || altKeyboardCode[key] == MODIFIERKEY_ALT || altKeyboardCode[key] == MODIFIERKEY_GUI)
    {
      setModKey (key, pressed, true);
    }
    else
    {
      setNormKey (key, pressed, true);
    }
    modCodeAct = true;
    altModCodeAct = false;
  }
  else if (key == modCode)
  {
    modCodeAct = true;
  }
  else if (key == altModCode)
  {
    altModCodeAct = true;
  }
  else if (keyboardCode[key] == MODIFIERKEY_CTRL || keyboardCode[key] == MODIFIERKEY_SHIFT || keyboardCode[key] == MODIFIERKEY_ALT || keyboardCode[key] == MODIFIERKEY_GUI)
  {
    setModKey (key, pressed, false);
  }
  else
  {
    setNormKey (key, pressed, false);
  }
}

// Sets all normal key presses

void setNormKey (int key, boolean pressed, boolean mod)
{
  boolean set = false;
  int x = 0;
  int i = 0;
  while (i < 6 && !set)
  {
    if (outKeyState[i] == keyboardCode[key])
    {
      x = i; //Skips x to the point where the key is already pressed to prevent key from being registered twice (ghosting bug)
      set = true;
    }
    i++;
  }
  set = false;
  while (x < 6 && !set)
  {
    if ((outKeyState[x] == keyboardCode[key] && pressed && !mod) || (outKeyState[x] == altKeyboardCode[key] && pressed && mod))
    {
      set = true;
    }
    else if (((((outKeyState[x] == 0) && pressed) || ((outKeyState[x] == keyboardCode[key]) && !pressed)) && !mod) || ((((outKeyState[x] == 0) && pressed) || ((outKeyState[x] == altKeyboardCode[key]) && !pressed)) && mod))
    {
      if (pressed && !mod)
      {
        outKeyState[x] = keyboardCode[key];
      }
      else if (pressed && mod)
      {
        outKeyState[x] = altKeyboardCode[key];
      }
      else
      {
        outKeyState[x] = 0;
      }
      set = true;
    }
    Serial.println(outKeyState[x]);
    x++;
  }
  // Keyboard.set_key1(outKeyState[0]);
  // Keyboard.set_key2(outKeyState[1]);
  // Keyboard.set_key3(outKeyState[2]);
  // Keyboard.set_key4(outKeyState[3]);
  // Keyboard.set_key5(outKeyState[4]);
  // Keyboard.set_key6(outKeyState[5]);
  // Keyboard.send_now();
}

// Sets all modifier key presses

void setModKey (int key, boolean pressed, int mod)
{
  boolean set = false;
  int x = 0;
  int i = 0;
  while (i < 4 && !set)
  {
    if (outKeyState[i] == keyboardCode[key])
    {
      x = i; //Skips x to the point where the key is already pressed to prevent key from being registered twice (ghosting bug)
      set = true;
    }
    i++;
  }
  set = false;
  while (x < 4 && !set)
  {
    if ((outModState[x] == keyboardCode[key] && pressed && !mod) || (outModState[x] == altKeyboardCode[key] && pressed && mod))
    {
      set = true;
    }
    else if (((((outModState[x] == 0) && pressed) || ((outModState[x] == keyboardCode[key]) && !pressed)) && !mod) || ((((outModState[x] == 0) && pressed) || ((outModState[x] == altKeyboardCode[key]) && !pressed)) && mod))
    {
      if (pressed && !mod)
      {
        outModState[x] = keyboardCode[key];
      }
      else if (pressed && mod)
      {
        outModState[x] = altKeyboardCode[key];
      }
      else
      {
        outModState[x] = 0;
      }
      set = true;
    }
    x++;
  }
  Serial.println(outModState[0]);
  // Keyboard.set_modifier(outModState[0] | outModState[1] | outModState[2] | outModState[3]);
  // Keyboard.send_now();
}