/*
 * =======================================================================================================
 * -------------------------------------------------------------------------------------------------------
 * ---####################-----###########-------###########-----############--############-############--
 * --######################---#############-----#############----            --            -           ---
 * --######              ##---#####     ###-----###     #####---------##-------#######------#-------------
 * --      --------------  ---     -----    ---    -----     ---------##-------#------------#-------------
 * --#####--------------------#####------####-####------#####---------##-------###########--############--
 * --     --------------------     ------         ------     ---------  -------           --            --
 * --#####--------------------#####--------#####--------#####---------------------------------------------
 * --     --------------------     --------     --------     ---------------------------------------------
 * --######--------------##---#####---------------------#####---------- CMtec CMDR Keyboard 42 -----------
 * --##################### ---#####---------------------#####---------------------------------------------
 * ---################### ----#####---------------------#####---------------------------------------------
 * ---                   -----     ---------------------     ---------------------------------------------
 * -------------------------------------------------------------------------------------------------------
 * =======================================================================================================
 *
 *  Copyright 2022 Christoffer Martinsson <cm@cmtec.se>
 *
 *  CMtec CMDR Keyboard 42 can be redistributed and/or modified under the terms of the GNU General
 *  Public License (Version 2), as published by the Free Software Foundation.
 *  A copy of the license can be found online at www.gnu.o urg/licenses.
 *
 *  CMtec CMDR Keyboard 42 is distributed in the hope that it will be useful, but WITHOUT ANY
 *  WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
 *  A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 *
 *  Keyboard/Mouse based on standard teensy "Keypad" library for button scanning, standard teensy
 *  "usb_keyboard" library for HID keyboard/mouse usb data communication.
 *
 *  Layer 0
 *  ------------------------------------------      ---------------------------------------------
 *  |  Tab/Fn2 |  Q  |  W  |  E  |  R  |  T  |      |  Y  |  U  |  I  |  O  |  P  |      Å      |
 *  | Ctrl/Esc |  A  |  S  |  D  |  F  |  G  |      |  H  |  J  |  K  |  L  |  Ö  |      Ä      |
 *  |  Shift   |  Z  |  X  |  C  |  V  |  B  |      |  N  |  M  |  ,  |  .  |  -  | Shift/Enter |
 *  ----------------| M1/M2 | Alt | BSpc/Fn1 |      | Spc/Fn1 | AltGr | Win |--------------------
 *                  --------------------------      -------------------------
 *  Layer 1
 *  ------------------------------------------      ---------------------------------------------
 *  |  Tab/Fn2 |  F1 |  F2 |  F3 |  F4 |  F5 |      |  F6 |  F7 |  F8 |  F9 | F10 |     F11     |
 *  | Ctrl/Esc |  1  |  2  |  3  |  4  |  5  |      |  6  |  7  |  8  |  9  |  0  |      +      |
 *  |  Shift   |  §  | Del | Esc |  <  |     |      |     |  '  |  ¨  |  ´  |     | Shift/Enter |
 *  ----------------| M1/M2 | Alt | BSpc/Fn1 |      | Spc/Fn1 | AltGr | Win |--------------------
 *                  --------------------------      -------------------------
 *  Layer 2
 *  ------------------------------------------      ---------------------------------------------
 *  |  Tab/Fn2 | F12 | F13 | F14 | F15 | F16 |      | ML  | MD  | MU  | MR  |     |   CapsLock  |
 *  | Ctrl/Esc | Prev| Play| Next|     |     |      | Left| Down| Up  |Right|     |             |
 *  |  Shift   |     |     |     |     |     |      | Home| PgD | PgU | End | Ins | Shift/Enter |
 *  ----------------| M1/M2 | Alt | BSpc/Fn1 |      | Spc/Fn1 | AltGr | Win |--------------------
 *                  --------------------------      -------------------------
 *  Game mode
 *  ------------------------------------------      ---------------------------------------------
 *  |   Tab    |     |     |     |     |     |      |     |     |     |     |     |             |
 *  |   Ctrl   |     |     |     |     |     |      |     |     |     |     |     |             |
 *  |          |     |     |     |     |     |      |     |     |     |     |     |             |
 *  ----------------|  Alt  | Spc |          |      |         |       |     |--------------------
 *                  --------------------------      -------------------------
 *
 *  Features:
 *
 *      * 42 keys "Split" keyboard layout. 36 finger buttons and 6 thumb buttons.
 *      * Extreme low profile (only one pcb).
 *      * Cost efficient solution with one pcb and one 3D printed cover.
 *      * Function keys with total of four layer support (Primary + 2fn layers + Game mode).
 *      * Mouse movement, wheel up, wheel down, left button, right button and middle button support
 *      * Status indication -
 *          - 0 LED off = Normal mode
 *          - 1 LED constant  on = Game mode
 *          - 2 LED flashing = Caps Lock activated
 *      * Game mode: Replaces all layer keys with a "Game mode KEY". Configurable for each button
 *      * Tap/Hold functionality (only for layer0)
 *      * Combo key (up to three simultaneous keys) support. Configurable for each button
 */

#include <Arduino.h>
#include <Keypad.h>

#define USB_LED_NUM_LOCK 0
#define USB_LED_CAPS_LOCK 1
#define USB_LED_SCROLL_LOCK 2

#define KEY_OFFSET 0xAA00 // Offset to apply for not interfere with already defined keyboard keys

#define KEY_MWU 1 + KEY_OFFSET    // Mouse wheel up
#define KEY_MWD 2 + KEY_OFFSET    // Mouse wheel down
#define KEY_M1 3 + KEY_OFFSET     // Mouse button 1 (left)
#define KEY_M2 4 + KEY_OFFSET     // Mouse button 2 (right)
#define KEY_M3 5 + KEY_OFFSET     // Mouse button 3 (middle/wheel)
#define KEY_MU 6 + KEY_OFFSET     // Mouse Y+
#define KEY_ML 7 + KEY_OFFSET     // Mouse X-
#define KEY_MD 8 + KEY_OFFSET     // Mouse Y-
#define KEY_MR 9 + KEY_OFFSET     // Mouse X+
#define KEY_FN1 10 + KEY_OFFSET   // Function layer 1 button
#define KEY_FN2 11 + KEY_OFFSET   // Function layer 2 button
#define KEY_GM 12 + KEY_OFFSET    // Toggle game mode button
#define KEY_COMBO 13 + KEY_OFFSET // Combo key (pressing all key defined in combo array)

#define TAP_TIMEOUT 160   // Key tap timeout (ms)
#define NBR_OF_BUTTONS 42 // Number of buttons used (42 in this case)

struct Button
{
    int keypad_kchar = 0;
    uint16_t keycode = NO_KEY;
    uint16_t tap_keycode = NO_KEY;
    uint16_t fn1_keycode = NO_KEY;
    uint16_t fn2_keycode = NO_KEY;
    uint16_t gm_keycode = NO_KEY;
    bool hold_direct = true;
    uint16_t combo_keycode[3] = {NO_KEY, NO_KEY, NO_KEY};
    int kstate = IDLE;
    bool run_keycode = false;
    int tap_state = 0;
    bool tap_timeout_enable = false;
    bool tap_release_enable = false;
    unsigned long tap_timeout_timestamp = 0;
    unsigned long tap_release_timestamp = 0;
    bool tap_inhibit = false;
};

const byte KP_ROWS = 4;
const byte KP_COLS = 12;

byte kp_rowPins[KP_ROWS] = {1, 2, 3, 4};
byte kp_colPins[KP_COLS] = {12, 11, 10, 9, 8, 7, 26, 25, 24, 23, 22, 21};

char kp_keys[KP_ROWS][KP_COLS] = {
    {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12},
    {13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24},
    {25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36},
    {0, 0, 0, 37, 38, 39, 40, 41, 42, 0, 0, 0}};

Keypad kp_keypad = Keypad(makeKeymap(kp_keys), kp_rowPins, kp_colPins, KP_ROWS, KP_COLS);

/* Keymap config ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
/* Valid "Fn0 (hold) key" when using tap mode are: KEY_LEFT_SHIFT, KEY_RIGHT_SHIFT, KEY_LEFT_CTRL, KEY_RIGHT_CTRL, KEY_RIGHT_ALT, KEY_LEFT_GUI, KEY_RIGHT_GUI, KEY_FN1, KEY_FN2
 * "Button ID" corresponding with the physical design of the actual keyboard. DO NOT CHANGE BTN ID!
  -------------------------------------      -------------------------------------
  |  1  |  2  |  3  |  4  |  5  |  6  |      |  7  |  8  |  9  | 1O  | 11  | 12  |
  | 13  | 14  | 15  | 16  | 17  | 18  |      | 19  | 20  | 21  | 22  | 23  | 24  |
  | 25  | 26  | 27  | 28  | 29  | 30  |      | 31  | 32  | 33  | 34  | 35  | 36  |
  ------------------| 37  | 38  | 39  |      | 40  | 41  | 42  |------------------
                    -------------------      -------------------
 * "Fn0 hold key" is normal key in non tap mode. In tap mode this key is the hold key.
 * "Fn1 key" is the layer 1 key to use. Don NOT add KEY_FN1 or KEY_FN2 to this layer.
 * "Fn2 key" is the layer 2 key to use. Don NOT add KEY_FN1 or KEY_FN2 to this layer.
   "Fn1 key" and "Fn2 key" are N/A when using tap mode and should me defined as NO_KEY.
 * "GM replace key" will override all layer keys (Fn0, Fn1, Fn2) both tap and hold while game mode are active.
 * "Hold direct" enables sending PRESS command as soon as the hold key is pressed (regardless if you intend to press the tap key).
 * "Combo keys" defines up to three keys pressed together when key KEY_COMBO used. If Fn1/2 is used in combo it must be placed in the first position od the array */

// clang-format off
Button buttons[NBR_OF_BUTTONS] =
 {
/* Btn ID  Fn0 (hold) key    Fn0 tap key     Fn1 key           Fn2 key                 GM replace key  Hold direct  Combo keys */
   {1,     KEY_FN2,          KEY_TAB,        NO_KEY,           NO_KEY,                 KEY_TAB,        true,        {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {2,     KEY_Q,            NO_KEY,         KEY_F1,           KEY_F12,                NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {3,     KEY_W,            NO_KEY,         KEY_F2,           KEY_F13,                NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {4,     KEY_E,            NO_KEY,         KEY_F3,           KEY_F14,                NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {5,     KEY_R,            NO_KEY,         KEY_F4,           KEY_F15,                NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {6,     KEY_T,            NO_KEY,         KEY_F5,           KEY_F16,                NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {7,     KEY_Y,            NO_KEY,         KEY_F6,           NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {8,     KEY_U,            NO_KEY,         KEY_F7,           NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {9,     KEY_I,            NO_KEY,         KEY_F8,           NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {10,    KEY_O,            NO_KEY,         KEY_F9,           NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {11,    KEY_P,            NO_KEY,         KEY_F10,          NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {12,    'å',              NO_KEY,         KEY_F11,          KEY_CAPS_LOCK,          NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {13,    KEY_LEFT_CTRL,    KEY_ESC,        NO_KEY,           NO_KEY,                 KEY_LEFT_CTRL,  true,        {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {14,    KEY_A,            NO_KEY,         KEY_1,            KEY_MEDIA_PREV_TRACK,   NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {15,    KEY_S,            NO_KEY,         KEY_2,            KEY_MEDIA_PLAY_PAUSE,   NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {16,    KEY_D,            NO_KEY,         KEY_3,            KEY_MEDIA_NEXT_TRACK,   NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {17,    KEY_F,            NO_KEY,         KEY_4,            NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {18,    KEY_G,            NO_KEY,         KEY_5,            NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {19,    KEY_H,            NO_KEY,         KEY_6,            KEY_LEFT_ARROW,         NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {20,    KEY_J,            NO_KEY,         KEY_7,            KEY_DOWN_ARROW,         NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {21,    KEY_K,            NO_KEY,         KEY_8,            KEY_UP_ARROW,           NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {22,    KEY_L,            NO_KEY,         KEY_9,            KEY_RIGHT_ARROW,        NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {23,    'ö',              NO_KEY,         KEY_0,            NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {24,    'ä',              NO_KEY,         KEY_EQUAL,        KEY_GM,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {25,    KEY_LEFT_SHIFT,   NO_KEY,         KEY_LEFT_SHIFT,   KEY_LEFT_SHIFT,         NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {26,    KEY_Z,            NO_KEY,         '§',              NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {27,    KEY_X,            NO_KEY,         KEY_DELETE,       NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {28,    KEY_C,            NO_KEY,         KEY_ESC,          NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {29,    KEY_V,            NO_KEY,         '<',              NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {30,    KEY_B,            NO_KEY,         KEY_ESC,          NO_KEY,                 NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {31,    KEY_N,            NO_KEY,         NO_KEY,           KEY_HOME,               NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {32,    KEY_M,            NO_KEY,         KEY_BACKSLASH,    KEY_PAGE_DOWN,          NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {33,    KEY_COMMA,        NO_KEY,         KEY_RIGHT_BRACE,  KEY_PAGE_UP,            NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {34,    KEY_PERIOD,       NO_KEY,         KEY_MINUS,        KEY_END,                NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {35,    KEY_SLASH,        NO_KEY,         NO_KEY,           KEY_INSERT,             NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {36,    KEY_RIGHT_SHIFT,  KEY_ENTER,      NO_KEY,           NO_KEY,                 NO_KEY,         true,        {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {37,    KEY_M1,           KEY_M2,         NO_KEY,           NO_KEY,                 KEY_LEFT_ALT,   false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {38,    KEY_LEFT_ALT,     NO_KEY,         KEY_LEFT_ALT,     KEY_LEFT_ALT,           KEY_SPACE,      false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {39,    KEY_FN1,          KEY_BACKSPACE,  NO_KEY,           NO_KEY,                 NO_KEY,         true,        {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {40,    KEY_FN1,          KEY_SPACE,      NO_KEY,           NO_KEY,                 NO_KEY,         true,        {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false},
   {41,    KEY_COMBO,        NO_KEY,         KEY_COMBO,        KEY_COMBO,              NO_KEY,         false,       {KEY_FN1, KEY_RIGHT_ALT, NO_KEY},    IDLE, false, 0, false, false, 0, 0, false},
   {42,    KEY_LEFT_GUI,     NO_KEY,         KEY_LEFT_GUI,     KEY_LEFT_GUI,           NO_KEY,         false,       {NO_KEY, NO_KEY, NO_KEY},            IDLE, false, 0, false, false, 0, 0, false}};
/* ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ */
// clang-format on

const int STATUS_LED = 13;
bool status_led_on = false;
int status_led_mode = 0;

unsigned long current_timestamp = 0;
unsigned long button_timestamp = 0;
unsigned long mouse_wheel_timestamp = 0;
unsigned long indicator_timestamp = 0;

bool mouse_l = false;
bool mouse_r = false;
bool mouse_u = false;
bool mouse_d = false;
int mouse_x = 0;
int mouse_y = 0;
int mouse_wheel = 0;

bool game_mode = false;
bool key_pressed = false;

/**
  Perform key action.

  @param keycode code to apply action.
  @param kstate PRESSED or RELEASED.
  @return Action applied.
*/
bool set_key(uint16_t keycode, uint8_t kstate)
{
    /* Abort if keycode is invalid */
    if (keycode == NO_KEY || keycode == KEY_FN1 || keycode == KEY_FN2 || keycode == KEY_COMBO)
    {
        return false;
    }

    /* Mouse buttons (HID mouse) */
    if (keycode >= KEY_M1 && keycode <= KEY_M3)
    {
        if (kstate == RELEASED)
        {
            Mouse.release(1 << (((keycode - KEY_OFFSET) - (KEY_M1 - KEY_OFFSET))));
        }
        else if (kstate == PRESSED)
        {
            Mouse.press(1 << (((keycode - KEY_OFFSET) - (KEY_M1 - KEY_OFFSET))));
        }
    }

    /* Mouse wheel (HID mouse) */
    else if ((keycode == KEY_MWU || keycode == KEY_MWD))
    {
        if (kstate == RELEASED)
        {
            mouse_wheel = 0;
        }
        else if (kstate == PRESSED)
        {
            if (keycode == KEY_MWU)
            {
                mouse_wheel = 1;
            }
            else
            {
                mouse_wheel = -1;
            }
        }
    }

    /* Mouse X/Y (HID mouse) */
    else if ((keycode == KEY_MU || keycode == KEY_MD || keycode == KEY_ML || keycode == KEY_MR))
    {
        if (kstate == RELEASED)
        {
            if (keycode == KEY_ML)
            {
                mouse_l = false;
            }
            if (keycode == KEY_MR)
            {
                mouse_r = false;
            }
            if (keycode == KEY_MU)
            {
                mouse_u = false;
            }
            if (keycode == KEY_MD)
            {
                mouse_d = false;
            }
        }
        else if (kstate == PRESSED)
        {
            if (keycode == KEY_MU)
            {
                mouse_u = true;
            }
            else if (keycode == KEY_MD)
            {
                mouse_d = true;
            }
            else if (keycode == KEY_MR)
            {
                mouse_r = true;
            }
            else if (keycode == KEY_ML)
            {
                mouse_l = true;
            }
        }
    }

    /* Toggle game mode */
    else if (keycode == KEY_GM)
    {
        if (kstate == PRESSED && game_mode == true)
        {
            game_mode = false;
        }
        else if (kstate == PRESSED && game_mode == false)
        {
            game_mode = true;
        }
    }

    /* Normal keyboard keys (HID keyboard) */
    else
    {
        if (kstate == RELEASED)
        {
            Keyboard.release(keycode);
        }
        else if (kstate == PRESSED)
        {
            Keyboard.press(keycode);
        }
    }

    return true;
}

/**
  Scan key matrix and perform processing for each key.

  @return void.
*/
void scan_buttons()
{
    /* Scan keypad */
    if (kp_keypad.getKeys())
    {

        /* Enter bootloader if all four corner-buttons is pressed together */
        int reboot = 0;
        for (int i = 0; i < LIST_MAX; i++)
        {
            if ((kp_keypad.key[i].kchar == 1) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
            {
                reboot += 1;
            }
            if ((kp_keypad.key[i].kchar == 6) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
            {
                reboot += 1;
            }
            if ((kp_keypad.key[i].kchar == 13) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
            {
                reboot += 1;
            }
            if ((kp_keypad.key[i].kchar == 18) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
            {
                reboot += 1;
            }
        }
        if (reboot == 4)
        {
            _reboot_Teensyduino_();
        }

        /* Check for FN mode */
        int fn_mode = 0;
        for (int i = 0; i < LIST_MAX; i++)
        {
            if (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)
            {
                for (int j = 0; j < NBR_OF_BUTTONS; j++)
                {
                    if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar)
                    {
                        /* Check if FN1 key are defined to this button (Layer 0 and first position in combo array)*/
                        if ((buttons[j].keycode == KEY_FN1 || (buttons[j].keycode == KEY_COMBO && buttons[j].combo_keycode[0] == KEY_FN1)) && buttons[j].tap_state != 3)
                        {
                            fn_mode = 1;
                        }
                        /* Check if FN2 key are defined to this button (Layer 0 and first position in combo array)*/
                        else if ((buttons[j].keycode == KEY_FN2 || (buttons[j].keycode == KEY_COMBO && buttons[j].combo_keycode[0] == KEY_FN2)) && buttons[j].tap_state != 3)
                        {
                            fn_mode = 2;
                        }
                        break;
                    }
                }
            }
        }

        /* Process key press/release */
        for (int i = 0; i < LIST_MAX; i++)
        {
            if (kp_keypad.key[i].kstate == PRESSED)
            {
                for (int j = 0; j < NBR_OF_BUTTONS; j++)
                {
                    if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar && kp_keypad.key[i].stateChanged == true)
                    {
                        /* Tap state:
                        0 = idle (not pressed for a while)
                        1 = pressed
                        2 = released within timeout, pressing tap key
                        3 = pressed again within timeout, holding tap key */
                        if (buttons[j].tap_keycode != NO_KEY && buttons[j].tap_state == 0 && (game_mode == false || buttons[j].gm_keycode == NO_KEY))
                        {
                            buttons[j].tap_timeout_timestamp = current_timestamp + TAP_TIMEOUT;
                            buttons[j].tap_timeout_enable = true;
                            buttons[j].tap_release_timestamp = current_timestamp + TAP_TIMEOUT + 10;
                            buttons[j].tap_release_enable = true;
                            buttons[j].tap_state = 1;
                        }
                        else if (buttons[j].tap_keycode != NO_KEY && buttons[j].tap_state == 2)
                        {
                            buttons[j].tap_timeout_enable = false;
                            buttons[j].tap_release_enable = false;
                            buttons[j].tap_state = 3;
                        }

                        buttons[j].run_keycode = true;
                        buttons[j].kstate = PRESSED;
                        break;
                    }
                }
            }
            else if (kp_keypad.key[i].kstate == RELEASED)
            {
                for (int j = 0; j < NBR_OF_BUTTONS; j++)
                {
                    if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar && kp_keypad.key[i].stateChanged == true)
                    {
                        /* Tap state:
                        0 = idle (not pressed for a while)
                        1 = pressed
                        2 = released within timeout, pressing tap key
                        3 = pressed again within timeout, holding tap key */
                        if (buttons[j].tap_keycode != NO_KEY && buttons[j].tap_state == 1 && (game_mode == false || buttons[j].gm_keycode == NO_KEY))
                        {
                            buttons[j].tap_release_timestamp = current_timestamp + TAP_TIMEOUT + 10;
                            buttons[j].tap_release_enable = true;
                            buttons[j].tap_state = 2;
                        }
                        else
                        {
                            buttons[j].tap_state = 0;
                        }
                        buttons[j].run_keycode = true;
                        buttons[j].kstate = RELEASED;
                        break;
                    }
                }
            }
        }

        /* Check if any "non tap keys" has been pressed */
        for (int i = 0; i < NBR_OF_BUTTONS; i++)
        {
            if (buttons[i].run_keycode == true && buttons[i].tap_keycode == NO_KEY && buttons[i].kstate == PRESSED)
            {
                for (int j = 0; j < NBR_OF_BUTTONS; j++)
                {
                    if (buttons[j].tap_keycode != NO_KEY)
                    {
                        buttons[j].tap_inhibit = true;
                    }
                }
            }
        }

        /* Execute key commands */
        for (int i = 0; i < NBR_OF_BUTTONS; i++)
        {
            /* Check if key should be processed */
            if (buttons[i].run_keycode == true)
            {
                /* Check if key pressed or released */
                if (buttons[i].kstate == PRESSED)
                {
                    /* Check if key is in tap mode and not in game mode */
                    if (buttons[i].tap_keycode != NO_KEY && (game_mode == false || buttons[i].gm_keycode == NO_KEY))
                    {
                        /* Key is in tap mode. Perform action dependant on tap state*/
                        if (buttons[i].tap_state == 1)
                        {
                            if (buttons[i].hold_direct == true)
                            {
                                /* Press hold key if "hold direct" is enabled (tap state = 1) */
                                set_key(buttons[i].keycode, PRESSED);
                            }

                            /* Reset tap inhibit flag */
                            buttons[i].tap_inhibit = false;
                        }
                    }
                    else
                    {
                        /* Key is in normal mode. Perform action dependant on layer*/
                        if (game_mode == true && buttons[i].gm_keycode != NO_KEY)
                        {
                            if (buttons[i].gm_keycode == KEY_COMBO)
                            {
                                /* Sending press command for all combo keys */
                                for (int j = 0; j < 3; j++)
                                {
                                    set_key(buttons[i].combo_keycode[j], PRESSED);
                                }
                            }
                            else
                            {
                                set_key(buttons[i].gm_keycode, PRESSED);
                            }
                        }
                        else if (fn_mode == 1)
                        {
                            if (buttons[i].fn1_keycode == KEY_COMBO)
                            {
                                /* Sending press command for all combo keys */
                                for (int j = 0; j < 3; j++)
                                {
                                    set_key(buttons[i].combo_keycode[j], PRESSED);
                                }
                            }
                            else
                            {
                                set_key(buttons[i].fn1_keycode, PRESSED);
                            }
                        }
                        else if (fn_mode == 2)
                        {
                            if (buttons[i].fn2_keycode == KEY_COMBO)
                            {
                                /* Sending press command for all combo keys */
                                for (int j = 0; j < 3; j++)
                                {
                                    set_key(buttons[i].combo_keycode[j], PRESSED);
                                }
                            }
                            else
                            {
                                set_key(buttons[i].fn2_keycode, PRESSED);
                            }
                        }
                        else
                        {
                            if (buttons[i].keycode == KEY_COMBO)
                            {
                                /* Sending press command for all combo keys */
                                for (int j = 0; j < 3; j++)
                                {
                                    set_key(buttons[i].combo_keycode[j], PRESSED);
                                }
                            }
                            else
                            {
                                set_key(buttons[i].keycode, PRESSED);
                            }
                        }
                    }
                }
                else if (buttons[i].kstate == RELEASED)
                {
                    /* Check if key is in tap mode */
                    if (buttons[i].tap_keycode != NO_KEY && (game_mode == false || buttons[i].gm_keycode == NO_KEY))
                    {
                        /* Key is in tap mode. Perform action dependant on tap state*/
                        if (buttons[i].tap_state == 2)
                        {
                            /* Press tap key if no other keys are pressed (tap state = 2) */
                            set_key(buttons[i].tap_keycode, RELEASED); // Fix for not send press and hold for the tap key
                            set_key(buttons[i].keycode, RELEASED);
                            if (buttons[i].tap_inhibit == false)
                            {
                                set_key(buttons[i].tap_keycode, PRESSED);
                            }
                        }
                        else
                        {
                            /* Release all keys (tap state = 0,1,3) */
                            set_key(buttons[i].keycode, RELEASED);
                            set_key(buttons[i].tap_keycode, RELEASED);
                            buttons[i].tap_state = 0;
                        }
                    }
                    else
                    {
                        /* Key is in normal mode. Perform action dependant on layer*/
                        if (game_mode == true && buttons[i].gm_keycode != NO_KEY)
                        {
                            if (buttons[i].gm_keycode == KEY_COMBO)
                            {
                                /* Sending release command for all combo keys */
                                for (int j = 0; j < 3; j++)
                                {
                                    set_key(buttons[i].combo_keycode[j], RELEASED);
                                }
                            }
                            else
                            {
                                set_key(buttons[i].gm_keycode, RELEASED);
                            }
                        }
                        else
                        {
                            /* Sending release command for all combo keys */
                            for (int j = 0; j < 3; j++)
                            {
                                set_key(buttons[i].combo_keycode[j], RELEASED);
                            }

                            /* Sending release command for all other keycodes related to this button */
                            set_key(buttons[i].fn2_keycode, RELEASED);
                            set_key(buttons[i].fn1_keycode, RELEASED);
                            set_key(buttons[i].keycode, RELEASED);
                        }
                    }
                }
                /* Reset run_keycode flag */
                buttons[i].run_keycode = false;
            }
        }
    }

    /* Set status indication */
    if (keyboard_leds & (1 << USB_LED_CAPS_LOCK))
    {
        /* 2 = blinking (CapsLock) */
        status_led_mode = 2;
    }
    else if (game_mode == true)
    {
        /* 1 = constant on (Game mode) */
        status_led_mode = 1;
    }
    else
    {
        /* 0 = off (normal) */
        status_led_mode = 0;
    }
}

void setup()
{
    /* Init HW */
    pinMode(STATUS_LED, OUTPUT);
    digitalWrite(STATUS_LED, LOW);
}

void loop()
{
    /* Update current time (ms) */
    current_timestamp = millis();

    /* Scan buttons 1ms */
    if (current_timestamp >= button_timestamp)
    {
        button_timestamp = current_timestamp + 1;
        scan_buttons();
    }

    /* Fn tap timeout TAP_TIMEOUT */
    for (int i = 0; i < NBR_OF_BUTTONS; i++)
    {
        if (current_timestamp >= buttons[i].tap_timeout_timestamp && buttons[i].tap_timeout_enable)
        {
            if (buttons[i].tap_state == 1)
            {
                if (buttons[i].hold_direct == false && (game_mode == false || buttons[i].gm_keycode == NO_KEY))
                {
                    set_key(buttons[i].keycode, PRESSED);
                }
                buttons[i].tap_state = 0;
            }
            else if (buttons[i].tap_state == 2)
            {
                buttons[i].tap_state = 0;
            }
            buttons[i].tap_timeout_enable = false;
        }
    }

    /* Fn tap release TAP_TIMEOUT + 10ms */
    for (int i = 0; i < NBR_OF_BUTTONS; i++)
    {
        if (current_timestamp >= buttons[i].tap_release_timestamp && buttons[i].tap_release_enable)
        {
            set_key(buttons[i].tap_keycode, RELEASED);
            buttons[i].tap_release_enable = false;
            buttons[i].tap_state = 0;
        }
    }

    /* Update mouse 20ms */
    if (current_timestamp >= mouse_wheel_timestamp && (mouse_wheel != 0 || mouse_x != 0 || mouse_y != 0 || mouse_d == true || mouse_l == true || mouse_r == true || mouse_u == true))
    {
        /* Stop movement when no buttons are pressed */
        if (mouse_d == false && mouse_u == false)
        {
            mouse_y = 0;
        }
        if (mouse_l == false && mouse_r == false)
        {
            mouse_x = 0;
        }

        /* Stop movement when both up/down or left/right are pressed */
        if (mouse_d == true && mouse_u == true)
        {
            mouse_y = 0;
        }
        if (mouse_l == true && mouse_r == true)
        {
            mouse_x = 0;
        }

        /* Move mouse cursor/wheel */
        Mouse.move(mouse_x, mouse_y, mouse_wheel);

        /* Add mouse acceleration for next movement */
        if (mouse_r == true && mouse_x < 50)
        {
            mouse_x++;
        }
        if (mouse_l == true && mouse_x > -50)
        {
            mouse_x--;
        }
        if (mouse_d == true && mouse_y < 50)
        {
            mouse_y++;
        }
        if (mouse_u == true && mouse_y > -50)
        {
            mouse_y--;
        }

        mouse_wheel_timestamp = current_timestamp + 20;
    }

    /* Update indicator 200ms */
    if (current_timestamp >= indicator_timestamp)
    {
        if (status_led_mode == 2 && status_led_on == false)
        {
            digitalWrite(STATUS_LED, HIGH);
            status_led_on = true;
        }
        else if (status_led_mode == 2 && status_led_on == true)
        {
            digitalWrite(STATUS_LED, LOW);
            status_led_on = false;
        }
        else if (status_led_mode == 1)
        {
            digitalWrite(STATUS_LED, HIGH);
            status_led_on = true;
        }
        else
        {
            digitalWrite(STATUS_LED, LOW);
            status_led_on = false;
        }

        indicator_timestamp = current_timestamp + 200;
    }
}
