/*
 * =======================================================================================================
 * -------------------------------------------------------------------------------------------------------
 * ---####################-----###########-------###########-----############--############-############--
 * --######################---#############-----#############----            --            -           ---
 * --######              ##---#####     ###-----###     #####---------##-------#######------#-------------
 * --      --------------  ---     -----    ---    -----     ---------##-------#------------#-------------
 * --#####--------------------#####------####-####------#####---------##-------###########--############--
 * --     --------------------     ------         ------     ---------  -------           --            --
 * --#####--------------------#####--------#####--------#####---------------------------------------------
 * --     --------------------     --------     --------     ---------------------------------------------
 * --######--------------##---#####---------------------#####---------- CMtec CMDR Keyboard --------------
 * --##################### ---#####---------------------#####---------------------------------------------
 * ---################### ----#####---------------------#####---------------------------------------------
 * ---                   -----     ---------------------     ---------------------------------------------
 * -------------------------------------------------------------------------------------------------------
 * =======================================================================================================
 *
 *  Copyright 2020 Christoffer Martinsson <cm@cmtec.se>
 *
 *  CMtec CMDR Keyboard 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 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/Joystick based on standard teensy "Keypad" library for button scanning, standard teensy  
 *  "usb_keyboard" library, standard "usb_mouse" and custom "usb_joystick" library for sending usb data.
 *  
 *  Features:
 *
 *      56 keys Split keyboard/keypads (28 Left + 28 Right)
 *      Two function buttons with total of four key-layer support (Primary + 3fn layers)
 *      Mouse wheel up, wheel down, back button, forward button and middle button support
 *      Dedicated PTT button with four channel support
 *   
 *      2x joysticks each having 2 axis, 4 buttons 
 */


#include <Arduino.h>
#include <EEPROM.h>
#include <Keypad.h>
#include "USBHost_t36.h"

#define HID_AXIS_MAX 1023
#define HID_AXIS_MIN 0
#define HID_AXIS_CENTER 512
#define AXIS_MAX 4096 
#define AXIS_MIN 0
#define AXIS_CENTER 2048 
#define DEADZONE_X 50
#define DEADZONE_Y 50

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

#define KEY_OFFSET 0xAA00 
#define NO_JOY 0 

#define JOY_A1 1 + KEY_OFFSET
#define JOY_A2 2 + KEY_OFFSET
#define JOY_A3 3 + KEY_OFFSET
#define JOY_A4 4 + KEY_OFFSET
#define JOY_A5 5 + KEY_OFFSET
#define JOY_A6 6 + KEY_OFFSET
#define JOY_A7 7 + KEY_OFFSET
#define JOY_A8 8 + KEY_OFFSET
#define JOY_A9 9 + KEY_OFFSET
#define JOY_A10 10 + KEY_OFFSET
#define JOY_A11 11 + KEY_OFFSET
#define JOY_A12 12 + KEY_OFFSET
#define JOY_A13 13 + KEY_OFFSET
#define JOY_A14 14 + KEY_OFFSET
#define JOY_A15 15 + KEY_OFFSET
#define JOY_A16 16 + KEY_OFFSET
#define JOY_A17 17 + KEY_OFFSET
#define JOY_A18 18 + KEY_OFFSET
#define JOY_A19 19 + KEY_OFFSET
#define JOY_A20 20 + KEY_OFFSET
#define JOY_A21 21 + KEY_OFFSET
#define JOY_A22 22 + KEY_OFFSET
#define JOY_A23 23 + KEY_OFFSET
#define JOY_A24 24 + KEY_OFFSET
#define JOY_A25 25 + KEY_OFFSET
#define JOY_A26 26 + KEY_OFFSET 
#define JOY_A27 27 + KEY_OFFSET
#define JOY_A28 28 + KEY_OFFSET
#define JOY_A29 29 + KEY_OFFSET
#define JOY_A30 30 + KEY_OFFSET
#define JOY_A31 31 + KEY_OFFSET 
#define JOY_A32 32 + KEY_OFFSET

#define JOY_AHU1 49 + KEY_OFFSET 
#define JOY_AHR1 50 + KEY_OFFSET 
#define JOY_AHD1 51 + KEY_OFFSET 
#define JOY_AHL1 52 + KEY_OFFSET 

#define KEY_MWU 53 + KEY_OFFSET 
#define KEY_MWD 54 + KEY_OFFSET 
#define KEY_M1 55 + KEY_OFFSET 
#define KEY_M2 56 + KEY_OFFSET 
#define KEY_M3 57 + KEY_OFFSET 
#define KEY_MB 58 + KEY_OFFSET 
#define KEY_MF 59 + KEY_OFFSET

#define KEY_FN1 60 + KEY_OFFSET 
#define KEY_FN2 61 + KEY_OFFSET 
#define KEY_FN3 62 + KEY_OFFSET 
#define KEY_FN4 63 + KEY_OFFSET 
#define KEY_FN2L 64 + KEY_OFFSET 
#define KEY_FN2R 65 + KEY_OFFSET 

#define JOY_BTN1 71 + KEY_OFFSET 
#define JOY_BTN2 72 + KEY_OFFSET 
#define JOY_BTN3 73 + KEY_OFFSET 
#define JOY_BTN4 74 + KEY_OFFSET 
#define JOY_BTN5 75 + KEY_OFFSET 
#define JOY_BTN6 76 + KEY_OFFSET 
#define JOY_BTN7 77 + KEY_OFFSET 
#define JOY_BTN8 78 + KEY_OFFSET 

#define JOY_FN1 JOY_BTN3 
#define JOY_FN2 JOY_BTN7 

#define KEY_PST 79 + KEY_OFFSET // Port toggle
#define KEY_PS0 80 + KEY_OFFSET // Port select 
#define KEY_PS1 81 + KEY_OFFSET 
#define KEY_PS2 82 + KEY_OFFSET 
#define KEY_D1S0 83 + KEY_OFFSET // Display 1 select 
#define KEY_D1S1 84 + KEY_OFFSET 
#define KEY_D1S2 85 + KEY_OFFSET 
#define KEY_D2S0 86 + KEY_OFFSET // Display 2 select 
#define KEY_D2S1 87 + KEY_OFFSET 
#define KEY_D2S2 88 + KEY_OFFSET 
#define KEY_D2S3 89 + KEY_OFFSET 

#define NBR_OF_FN 4+1

byte kp_buttons[64][3];
byte tb_buttons[8][3];


// Keypad button mapping
const uint16_t kp_keys[64][NBR_OF_FN] = {
    // Left keypad 
    // Fn 0             Fn 1                Fn 2                    Fn 3                    Fn 4
    // Standard keys    Sec Standard keys   F and Special keys      Sec F and Special keys  Special keys
    {KEY_ESC,           KEY_TILDE,          NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_1,             NO_KEY,             KEY_F1,                 KEY_D1S0,               NO_KEY},
    {KEY_2,             NO_KEY,             KEY_F2,                 KEY_D1S1,               NO_KEY},
    {KEY_3,             NO_KEY,             KEY_F3,                 KEY_D1S2,               NO_KEY},
    {KEY_4,             NO_KEY,             KEY_F4,                 NO_KEY,                 NO_KEY},
    {KEY_5,             NO_KEY,             KEY_F5,                 NO_KEY,                 NO_KEY},
    {KEY_FN2L,          KEY_FN2L,           KEY_FN2L,               KEY_FN2L,               KEY_FN2L}, 
    {KEY_Q,             NO_KEY,             KEY_PST,                KEY_D2S3,               NO_KEY},
    {KEY_W,             NO_KEY,             KEY_PS0,                KEY_D2S1,               NO_KEY},
    {KEY_E,             NO_KEY,             KEY_PS1,                KEY_D2S2,               NO_KEY},
    {KEY_R,             NO_KEY,             KEY_PS2,                NO_KEY,                 NO_KEY},
    {KEY_T,             NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_LEFT_CTRL,     KEY_LEFT_CTRL,      KEY_LEFT_CTRL,          KEY_LEFT_CTRL,          NO_KEY},
    {KEY_A,             NO_KEY,             KEY_MEDIA_PLAY_PAUSE,   NO_KEY,                 NO_KEY},
    {KEY_S,             NO_KEY,             KEY_MEDIA_NEXT_TRACK,   NO_KEY,                 NO_KEY},
    {KEY_D,             NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_F,             NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_G,             NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_LEFT_SHIFT,    KEY_LEFT_SHIFT,     KEY_LEFT_SHIFT,         KEY_LEFT_SHIFT,         NO_KEY},
    {KEY_Z,             '<',                NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_X,             NO_KEY,             KEY_M3,                 NO_KEY,                 NO_KEY},
    {KEY_C,             NO_KEY,             NO_KEY,                 NO_KEY,                 KEY_CAPS_LOCK},
    {KEY_V,             NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_B,             NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_LEFT_GUI,      KEY_LEFT_GUI,       KEY_LEFT_GUI,           KEY_LEFT_GUI,           NO_KEY},
    {KEY_FN1,           KEY_FN1,            KEY_FN1,                KEY_FN1,                KEY_FN1},
    {KEY_LEFT_ALT,      KEY_LEFT_ALT,       KEY_LEFT_ALT,           KEY_LEFT_ALT,           NO_KEY},
    {KEY_SPACE,         KEY_SPACE,          KEY_SPACE,              KEY_SPACE,              NO_KEY},
    {JOY_BTN1,          JOY_BTN1,           JOY_BTN1,               JOY_BTN1,               JOY_BTN1},  
    {JOY_BTN2,          JOY_BTN2,           JOY_BTN2,               JOY_BTN2,               JOY_BTN2},  
    {JOY_BTN3,          JOY_BTN3,           JOY_BTN3,               JOY_BTN3,               JOY_BTN3},  
    {JOY_BTN4,          JOY_BTN4,           JOY_BTN4,               JOY_BTN4,               JOY_BTN4},  
    // Right keypad 
    // Fn 0             Fn 1                Fn 2                    Fn 3                    Fn 4
    // Standard keys    Sec Standard keys   F and Special keys      Sec F and Special keys  Special keys
    {KEY_6,             NO_KEY,             KEY_F6,                 KEY_HOME,               NO_KEY},
    {KEY_7,             NO_KEY,             KEY_F7,                 KEY_END,                NO_KEY},
    {KEY_8,             NO_KEY,             KEY_F8,                 KEY_INSERT,             KEY_F13},
    {KEY_9,             KEY_MINUS,          KEY_F9,                 KEY_F11,                KEY_F14},
    {KEY_0,             KEY_EQUAL,          KEY_F10,                KEY_F12,                KEY_F15},
    {KEY_DELETE,        KEY_PRINTSCREEN,    NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_Y,             NO_KEY,             JOY_A28,                NO_KEY,                 NO_KEY},
    {KEY_U,             NO_KEY,             JOY_A29,                NO_KEY,                 NO_KEY},
    {KEY_I,             NO_KEY,             JOY_A30,                NO_KEY,                 KEY_F16},
    {KEY_O,             'å',                JOY_A31,                NO_KEY,                 KEY_F17},
    {KEY_P,             KEY_RIGHT_BRACE,    JOY_A32,                NO_KEY,                 KEY_F18},
    {KEY_FN2R,          KEY_FN2R,           KEY_FN2R,               KEY_FN2R,               NO_KEY}, 
    {KEY_H,             NO_KEY,             KEY_LEFT,               NO_KEY,                 NO_KEY},
    {KEY_J,             KEY_PAGE_DOWN,      KEY_DOWN,               NO_KEY,                 NO_KEY},
    {KEY_K,             KEY_PAGE_UP,        KEY_UP,                 NO_KEY,                 KEY_F19},
    {KEY_L,             'ä',                KEY_RIGHT,              NO_KEY,                 KEY_F20},
    {'ö',               KEY_BACKSLASH,      NO_KEY,                 NO_KEY,                 KEY_F21},
    {KEY_ENTER,         KEY_ENTER,          KEY_ENTER,              KEY_ENTER,              NO_KEY},
    {KEY_N,             KEY_M2,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_M,             NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_COMMA,         NO_KEY,             NO_KEY,                 NO_KEY,                 KEY_F22},
    {KEY_PERIOD,        NO_KEY,             NO_KEY,                 NO_KEY,                 KEY_F23},
    {KEY_SLASH,         NO_KEY,             NO_KEY,                 NO_KEY,                 KEY_F24},
    {KEY_RIGHT_SHIFT,   KEY_RIGHT_SHIFT,    KEY_RIGHT_SHIFT,        KEY_RIGHT_SHIFT,        NO_KEY},
    {KEY_SPACE,         NO_KEY,             NO_KEY,                 NO_KEY,                 NO_KEY},
    {KEY_RIGHT_ALT,     KEY_RIGHT_ALT,      KEY_RIGHT_ALT,          KEY_RIGHT_ALT,          NO_KEY},
    {KEY_FN1,           KEY_FN1,            KEY_FN1,                KEY_FN1,                KEY_FN1},
    {KEY_FN4,           KEY_FN4,            KEY_FN4,                KEY_FN4,                KEY_FN4},
    {JOY_BTN5,          JOY_BTN5,           JOY_BTN5,               JOY_BTN5,               JOY_BTN5},  
    {JOY_BTN6,          JOY_BTN6,           JOY_BTN6,               JOY_BTN6,               JOY_BTN6},  
    {JOY_BTN7,          JOY_BTN7,           JOY_BTN7,               JOY_BTN7,               JOY_BTN7},  
    {JOY_BTN8,          JOY_BTN8,           JOY_BTN8,               JOY_BTN8,               JOY_BTN8},  
};

// Joystick button mapping
const uint16_t joy_keys[8][NBR_OF_FN] = {
    // Fn 0             Fn 1                Fn 2                    Fn 3                    N/A
    {JOY_A1,            JOY_A9,             JOY_A14,                JOY_A19,                NO_JOY}, // L1 
    {JOY_A2,            JOY_A10,            JOY_A15,                JOY_A20,                NO_JOY}, // L2 
    {JOY_A3,            JOY_A3,             JOY_A3,                 JOY_A3,                 NO_JOY}, // L3 Joy Fn1
    {JOY_A4,            JOY_A11,            JOY_A16,                JOY_A21,                NO_JOY}, // L4
    {JOY_A5,            JOY_A12,            JOY_A17,                JOY_A22,                NO_JOY}, // R1 
    {JOY_A6,            JOY_A6,             JOY_A6,                 JOY_A23,                NO_JOY}, // R2
    {JOY_A7,            JOY_A7,             JOY_A7,                 JOY_A7,                 NO_JOY}, // R3 Joy Fn2
    {JOY_A8,            JOY_A13,            JOY_A18,                JOY_A24,                NO_JOY}, // R4 
}; 

// Trackball button mapping
const uint16_t tb_keys[8][NBR_OF_FN] = {
    // Fn 0             Fn 1                Fn 2                    Fn 3                    Fn 4
    {KEY_M2,            KEY_M2,             KEY_M2,                 KEY_M3,                 KEY_M2},
    {KEY_M1,            KEY_M1,             KEY_M1,                 KEY_M1,                 KEY_M1},
    {KEY_FN3,           KEY_FN3,            KEY_FN3,                KEY_FN3,                KEY_FN3},
    {KEY_FN3,           KEY_FN3,            KEY_FN3,                KEY_FN3,                KEY_FN3},
    {KEY_FN3,           KEY_FN3,            KEY_FN3,                KEY_FN3,                KEY_FN3},
    {KEY_FN3,           KEY_FN3,            KEY_FN3,                KEY_FN3,                KEY_FN3},
    {KEY_FN3,           KEY_FN3,            KEY_FN3,                KEY_FN3,                KEY_FN3},
    {KEY_FN3,           KEY_FN3,            KEY_FN3,                KEY_FN3,                KEY_FN3},
};

struct Fn_tap{
    int state = 0;
    bool timeout_enable = false;
    bool release_enable = false;
    unsigned long timeout_timestamp = 0;
    unsigned long release_timestamp = 0;
    unsigned long timeout_time = 150;
    uint16_t trigger_keycode = NO_KEY;
    uint16_t target_keycode = NO_KEY;
    int kp_fn_mode = 0;
    int joy_fn_mode = 0;
    bool fn_fast_switch = true;
};

int fn_kp_key_found = 0;
int fn_joy_key_found = 0;
int kp_fn_mode = 0;
int joy_fn_mode = 0;
Fn_tap fn_tap[4];

int mouse_wheel = 0;

const int POWER_LED = 13;

enum EEPROM_ADR
{
    MAX_X_ADR_HIGH,
    MAX_X_ADR_LOW,
    MIN_X_ADR_HIGH,
    MIN_X_ADR_LOW,
    CNT_X_ADR_HIGH,
    CNT_X_ADR_LOW,
    MAX_Y_ADR_HIGH,
    MAX_Y_ADR_LOW,
    MIN_Y_ADR_HIGH,
    MIN_Y_ADR_LOW,
    CNT_Y_ADR_HIGH,
    CNT_Y_ADR_LOW,
    EEPROM_ADR_NBR_OF_BYTES
};

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

const byte KP_ROWS = 5;
const byte KP_COLS = 6;

const byte JOY_ROWS = 2;
const byte JOY_COLS = 2;

byte buttons[32];

int joystick_counter = 0;
int joystick_x = 0;
int joystick_x_raw = 0;
int joystick_x_max = 4096;
int joystick_x_min = 0;
int joystick_x_center = joystick_x_max/2;
int joystick_y = 0;
int joystick_y_raw = 0;
int joystick_y_max = 4096;
int joystick_y_min = 0;
int joystick_y_center = joystick_y_max/2;

float exp_constant = 0.2;

#define CALIBRATION_OFF 0
#define CALIBRATION_CENTER 1
#define CALIBRATION_MINMAX 2
int joystick_calibration_mode = 0;

byte kp_rowPins[KP_ROWS] = {2,3,4,5,6};
byte kp_colPins[KP_COLS] = {9,10,11,12,14,15};

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

byte joy_rowPins[JOY_ROWS] = {21,18};
byte joy_colPins[JOY_COLS] = {20,19};

Keypad joy_keypad = Keypad(makeKeymap(joy_keys), joy_rowPins, joy_colPins, JOY_ROWS, JOY_COLS);

USBHost myusb;
USBHIDParser hid1(myusb);
MouseController mouse1(myusb);

int mouse_x = 0;
int mouse_y = 0;



void save_to_eeprom(){
    EEPROM.write(MAX_X_ADR_LOW, joystick_x_max);
    EEPROM.write(MAX_X_ADR_HIGH, joystick_x_max >> 8);
    EEPROM.write(MIN_X_ADR_LOW, joystick_x_min);
    EEPROM.write(MIN_X_ADR_HIGH, joystick_x_min >> 8);
    EEPROM.write(CNT_X_ADR_LOW, joystick_x_center);
    EEPROM.write(CNT_X_ADR_HIGH, joystick_x_center >> 8);
    
    EEPROM.write(MAX_Y_ADR_LOW, joystick_y_max);
    EEPROM.write(MAX_Y_ADR_HIGH, joystick_y_max >> 8);
    EEPROM.write(MIN_Y_ADR_LOW, joystick_y_min);
    EEPROM.write(MIN_Y_ADR_HIGH, joystick_y_min >> 8);
    EEPROM.write(CNT_Y_ADR_LOW, joystick_y_center);
    EEPROM.write(CNT_Y_ADR_HIGH, joystick_y_center >> 8);
}


void load_from_eeprom(){
    joystick_x_max = (EEPROM.read(MAX_X_ADR_HIGH) << 8);
    joystick_x_max |= EEPROM.read(MAX_X_ADR_LOW);
    joystick_x_min = (EEPROM.read(MIN_X_ADR_HIGH) << 8);
    joystick_x_min |= EEPROM.read(MIN_X_ADR_LOW);
    joystick_x_center = (EEPROM.read(CNT_X_ADR_HIGH) << 8);
    joystick_x_center |= EEPROM.read(CNT_X_ADR_LOW);

    joystick_y_max = (EEPROM.read(MAX_Y_ADR_HIGH) << 8);
    joystick_y_max |= EEPROM.read(MAX_Y_ADR_LOW);
    joystick_y_min = (EEPROM.read(MIN_Y_ADR_HIGH) << 8);
    joystick_y_min |= EEPROM.read(MIN_Y_ADR_LOW);
    joystick_y_center = (EEPROM.read(CNT_Y_ADR_HIGH) << 8);
    joystick_y_center |= EEPROM.read(CNT_Y_ADR_LOW);
}


void update_key(uint16_t keycode, uint8_t kstate) {
    // Mouse buttons
    if (keycode >= KEY_M1 && keycode <= KEY_MF) {
        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
    else if ((keycode == KEY_MWU || keycode == KEY_MWD)) {
        if (kstate == RELEASED) {
            mouse_wheel = 0;
        } else if (kstate == PRESSED || kstate == HOLD) {
            if (keycode == KEY_MWU) {
                mouse_wheel = 1;
            } else {
                mouse_wheel = -1;
            }
        }
    }

    // Keypad Joystick buttons
    else if (keycode >= JOY_BTN1 && keycode <= JOY_BTN8) {
        int joy_keys_index = ((keycode-KEY_OFFSET)-(JOY_BTN1-KEY_OFFSET));
        if (kstate == RELEASED) {
            // Release all fn buttons related to this button
            for (int j = 0; j < NBR_OF_FN; j++){
                update_key(joy_keys[joy_keys_index][j], RELEASED);
            }
        } else if (kstate == PRESSED) {
            update_key(joy_keys[joy_keys_index][joy_fn_mode], PRESSED);
        }
    }

    // Joystick buttons
    else if (keycode >= JOY_A1 && keycode <= JOY_A32) {
        if (kstate == RELEASED) {
            Joystick.button(keycode-KEY_OFFSET, false);
        } else if (kstate == PRESSED) {
            Joystick.button(keycode-KEY_OFFSET, true);
        }
    }

    // Joystick hat 
    else if (keycode >= JOY_AHU1 && keycode <= JOY_AHL1) {
        if (kstate == RELEASED) {
            Joystick.hat(-1);
        } else if (kstate == PRESSED) {
            Joystick.hat(((keycode-KEY_OFFSET)-(JOY_AHU1-KEY_OFFSET)) * 90);
        }
    }

    // Normal keyboard keys
    else {
        if (((
            keycode == KEY_MEDIA_PLAY_PAUSE || keycode == KEY_MEDIA_NEXT_TRACK ||
            keycode == KEY_F13 || keycode == KEY_F14 ||
            keycode == KEY_F15 || keycode == KEY_F16 ||
            keycode == KEY_F17 || keycode == KEY_F18 ||
            keycode == KEY_F19 || keycode == KEY_F20 ||
            keycode == KEY_F21 || keycode == KEY_F22 ||
            keycode == KEY_F23 || keycode == KEY_F24)) || 
            ((
            keycode != KEY_MEDIA_PLAY_PAUSE && keycode != KEY_MEDIA_NEXT_TRACK &&
            keycode != KEY_F13 && keycode != KEY_F14 &&
            keycode != KEY_F15 && keycode != KEY_F16 &&
            keycode != KEY_F17 && keycode != KEY_F18 &&
            keycode != KEY_F19 && keycode != KEY_F20 &&
            keycode != KEY_F21 && keycode != KEY_F22 &&
            keycode != KEY_F23 && keycode != KEY_F24))
            ) {
            if (kstate == RELEASED) {
                Keyboard.release(keycode);
            } else if (kstate == PRESSED) {
                Keyboard.press(keycode);
            }
        }
    }
}


void process_table(byte buttons[][3], const uint16_t keys[][5], int nbr_of_element){
    for (int i = 0; i < nbr_of_element; i++) {
        if (buttons[i][2] == PRESSED && 
                keys[i][0] != JOY_FN1 &&
                keys[i][0] != JOY_FN2 &&
                keys[i][0] != KEY_FN1 &&
                keys[i][0] != KEY_FN2 && 
                keys[i][0] != KEY_FN2L && 
                keys[i][0] != KEY_FN2R && 
                keys[i][0] != KEY_FN3 && 
                keys[i][0] != KEY_FN4) {
            // Press key linked to the FN layer for this button
            if (keys[i][kp_fn_mode] != NO_KEY) {
                update_key(keys[i][kp_fn_mode], PRESSED);
            }

            // Check if fn related button is pressed
            if (kp_fn_mode > 0){
                fn_kp_key_found = keys[i][0];
            }
            if (joy_fn_mode > 0){
                fn_joy_key_found = keys[i][0];
            }
        }

        else if (buttons[i][2] == RELEASED && 
                keys[i][0] != JOY_FN1 &&
                keys[i][0] != JOY_FN2 &&
                keys[i][0] != KEY_FN1 &&
                keys[i][0] != KEY_FN2 && 
                keys[i][0] != KEY_FN2L && 
                keys[i][0] != KEY_FN2R && 
                keys[i][0] != KEY_FN3 && 
                keys[i][0] != KEY_FN4) {
            
            // Release all keys linked to this button
            update_key(keys[i][0], RELEASED);
                
            // Check if fn related button is released 
            if (keys[i][0] == fn_kp_key_found){
                fn_kp_key_found = 0;
            }
            if (keys[i][0] == fn_joy_key_found){
                fn_joy_key_found = 0;
            }
            
            // Check to not relase the same key one more time
            if (keys[i][1] != NO_KEY && keys[i][1] != keys[i][0]) {
                update_key(keys[i][1], RELEASED);
            }

            // Check to not relase the same key one more time
            if (keys[i][2] != NO_KEY && keys[i][2] != keys[i][0] &&
                keys[i][2] != keys[i][1]) {
                update_key(keys[i][2], RELEASED);
            }

            // Check to not relase the same key one more time
            if (keys[i][3] != NO_KEY && keys[i][3] != keys[i][0] &&
                keys[i][3] != keys[i][1] &&
                keys[i][3] != keys[i][2]) {
                update_key(keys[i][3], RELEASED);
            }

            // Check to not relase the same key one more time
            if (keys[i][4] != NO_KEY && keys[i][4] != keys[i][0] &&
                keys[i][4] != keys[i][1] &&
                keys[i][4] != keys[i][2] &&
                keys[i][4] != keys[i][3]) {
                update_key(keys[i][4], RELEASED);
            }
        }
    }

    for (int i = 0; i < nbr_of_element; i++) {
        // 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
        for (int j = 0; j < (sizeof(fn_tap) / sizeof(fn_tap[0])); j++){
            // Press
            if (buttons[i][2] == PRESSED && keys[i][0] == fn_tap[j].trigger_keycode) {
                if (fn_tap[j].state == 0) {
                    fn_tap[j].timeout_timestamp = current_timestamp + fn_tap[j].timeout_time;
                    fn_tap[j].timeout_enable = true;
                    fn_tap[j].release_enable = false;
                    fn_tap[j].state = 1;
                
                } else if (fn_tap[j].state == 2) {
                    fn_tap[j].timeout_enable = false;
                    fn_tap[j].release_enable = false;
                    fn_tap[j].state = 3;
                }
            // Release 
            } else if (buttons[i][2] == RELEASED && keys[i][0] == fn_tap[j].trigger_keycode) {
                if (fn_tap[j].state == 1) {
                    update_key(fn_tap[j].target_keycode, RELEASED);
                    if ((fn_kp_key_found == 0 && fn_tap[j].kp_fn_mode > 0) || (fn_joy_key_found == 0 && fn_tap[j].joy_fn_mode > 0)) {
                        update_key(fn_tap[j].target_keycode, PRESSED);
                        fn_tap[j].release_timestamp = current_timestamp + fn_tap[j].timeout_time + 10;
                        fn_tap[j].release_enable = true;
                        fn_tap[j].state = 2;
                    } else {
                        fn_tap[j].timeout_enable = false;
                        fn_tap[j].release_enable = false;
                        fn_tap[j].state = 0;
                    }
                } else {
                    update_key(fn_tap[j].target_keycode, RELEASED);
                    fn_tap[j].state = 0;
                }
            }
        }
        // Reset key change status
        buttons[i][2] = IDLE;
    }
}


void update_analog(){

    if(joystick_counter == 0){
        joystick_x_raw = analogRead(2);
        joystick_counter++;
    }
    else if(joystick_counter == 1){
        joystick_y_raw = analogRead(3);
        joystick_counter = 0;
        
        if (joystick_calibration_mode == CALIBRATION_OFF){

            // ----------------------------------------------------------		
            // Map X joystick values to proper HID values	
            // ----------------------------------------------------------		
            if(joystick_x_raw > (joystick_x_center + DEADZONE_X)){
                joystick_x = constrain(map(joystick_x_raw, (joystick_x_center + DEADZONE_X), joystick_x_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
            }
            else if(joystick_x_raw < (joystick_x_center - DEADZONE_X)){
                joystick_x = constrain(map(joystick_x_raw, joystick_x_min, (joystick_x_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
            }
            else{
                joystick_x = AXIS_CENTER;
            }
            // ----------------------------------------------------------		
            // Map Y joystick values to proper HID values	
            // ----------------------------------------------------------		
            if(joystick_y_raw > (joystick_y_center + DEADZONE_Y)){
                joystick_y = constrain(map(joystick_y_raw, (joystick_y_center + DEADZONE_Y), joystick_y_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
            }
            else if(joystick_y_raw < (joystick_y_center - DEADZONE_Y)){
                joystick_y = constrain(map(joystick_y_raw, joystick_y_min, (joystick_y_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
            }
            else{
                joystick_y = AXIS_CENTER;
            }
            
            // ----------------------------------------------------------		
            // Calculate new axis values after applying exp curve 
            // ----------------------------------------------------------
			
			// Normal mode
			exp_constant = 0.2;
            
			if (joystick_x != AXIS_CENTER){
                float joystick_x_float = joystick_x / float(AXIS_MAX);
                float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
                joystick_x = int(joystick_x_exp * float(HID_AXIS_MAX));
                joystick_x = constrain(joystick_x, HID_AXIS_MIN, HID_AXIS_MAX);
            }
            else{
                joystick_x = HID_AXIS_CENTER;
            }
            if (joystick_y != AXIS_CENTER){
                float joystick_y_float = joystick_y / float(AXIS_MAX);
                float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float;
                joystick_y = int(joystick_y_exp * float(HID_AXIS_MAX));
                joystick_y = constrain(joystick_y, HID_AXIS_MIN, HID_AXIS_MAX);
            }
            else{
                joystick_y = HID_AXIS_CENTER;
            }
        }
        else{

            // ----------------------------------------------------------		
            // Calibraton mode.
            // ----------------------------------------------------------		
            joystick_x = HID_AXIS_CENTER;
            joystick_y = HID_AXIS_CENTER;

            if (joystick_calibration_mode == CALIBRATION_CENTER){
                joystick_x_center = joystick_x_raw;
                joystick_y_center = joystick_y_raw;
                joystick_x_max = joystick_x_center;
                joystick_x_min = joystick_x_center;
                joystick_y_max = joystick_y_center;
                joystick_y_min = joystick_y_center;
            }
            else if (joystick_calibration_mode == CALIBRATION_MINMAX){
                if(joystick_x_raw > joystick_x_max){
                    joystick_x_max = joystick_x_raw;
                }
                if(joystick_x_raw < joystick_x_min){
                    joystick_x_min = joystick_x_raw;
                }
                if(joystick_y_raw > joystick_y_max){
                    joystick_y_max = joystick_y_raw;
                }
                if(joystick_y_raw < joystick_y_min){
                    joystick_y_min = joystick_y_raw;
                }
            }
        }
    }
}


void update_buttons(){

    // Scan all buttons
    if(kp_keypad.getKeys()){

        int reboot = 0;
       
        // Enter bootloader if all four corner-buttons is pressed together on the left keypad 
        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 == 24) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)){
                reboot += 1;
            }
            if((kp_keypad.key[i].kchar == 25) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)){
                reboot += 1;
            }
            if (reboot == 2) {
                if ((kp_keypad.key[i].kchar == 2) &&
                    (kp_keypad.key[i].kstate == PRESSED ||
                     kp_keypad.key[i].kstate == HOLD)) {
                    joystick_calibration_mode = CALIBRATION_CENTER;
                }
                if ((kp_keypad.key[i].kchar == 3) &&
                    (kp_keypad.key[i].kstate == PRESSED ||
                     kp_keypad.key[i].kstate == HOLD)) {
                    joystick_calibration_mode = CALIBRATION_MINMAX;
                }
                if ((kp_keypad.key[i].kchar == 4) &&
                    (kp_keypad.key[i].kstate == PRESSED ||
                     kp_keypad.key[i].kstate == HOLD)) {
                    joystick_calibration_mode = CALIBRATION_OFF;
                    save_to_eeprom();
                }
            }
            if(reboot == 4){
                asm("bkpt #251");
                return;
            }
        }

        // Check button press
        for(int i=0; i<LIST_MAX; i++){
            if (kp_keypad.key[i].kchar != 0){
                if(kp_keypad.key[i].stateChanged){
                    if(kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD){
                        buttons[kp_keypad.key[i].kchar-1] = PRESSED;
                    }
                    else if(kp_keypad.key[i].kstate == RELEASED){
                        buttons[kp_keypad.key[i].kchar-1] = RELEASED;
                    }
                }
            }
        }
    }
    
    // Scan joystick buttons
    if(joy_keypad.getKeys()){

        // Check button press
        for(int i=0; i<LIST_MAX; i++){
            if(joy_keypad.key[i].stateChanged){
                if(joy_keypad.key[i].kstate == PRESSED || joy_keypad.key[i].kstate == HOLD){
                    buttons[joy_keypad.key[i].kchar-1] = PRESSED;
                }
                else if(joy_keypad.key[i].kstate == RELEASED){
                    buttons[joy_keypad.key[i].kchar-1] = RELEASED;
                }
            }
        }
    }
}


void setup() {

    // Turn on and off power led.
    pinMode(POWER_LED, OUTPUT);
    digitalWrite(POWER_LED, LOW);

	// Set ADC resolution to 12bit
	analogReadResolution(12);
    analogReadAveraging(32);
	delay(500);
    
    myusb.begin();
    
    load_from_eeprom();
} 


void loop() {

    current_timestamp = millis();

	// ----------------------------------------------------------		
	// Update joystick values as often as possible
	// ----------------------------------------------------------		
	update_analog();

    // ----------------------------------------------------------		
	// Update USB host 
	// ----------------------------------------------------------		
    myusb.Task();
	
	// Update button status every 100 millisecond as fallback
    // to be able to enter bootloader
	// ----------------------------------------------------------		
    if (current_timestamp >= button_timestamp) {
        update_buttons();
        button_timestamp = current_timestamp + 100;
    }

	// ----------------------------------------------------------
    // Update mouse wheel
    // ----------------------------------------------------------
    if (current_timestamp >= mouse_wheel_timestamp && mouse_wheel != 0) {
        Mouse.move(0, 0, mouse_wheel);
        mouse_wheel_timestamp = current_timestamp + 20;
    }

    // ----------------------------------------------------------
    // Fn tap timeout
    // ----------------------------------------------------------
    for (int j = 0; j < (sizeof(fn_tap) / sizeof(fn_tap[0])); j++){
        if (current_timestamp >= fn_tap[j].timeout_timestamp && fn_tap[j].timeout_enable) {
            if (fn_tap[j].state == 1 || fn_tap[j].state == 2) {
                fn_tap[j].state = 0;
            }
            fn_tap[j].timeout_enable = false;
        }
    }

    // ----------------------------------------------------------
    // Fn tap release 
    // ----------------------------------------------------------
    for (int j = 0; j < (sizeof(fn_tap) / sizeof(fn_tap[0])); j++){
        if (current_timestamp >= fn_tap[j].release_timestamp && fn_tap[j].release_enable) {
            update_key(fn_tap[j].target_keycode, RELEASED);
            fn_tap[j].release_enable = false;
            fn_tap[j].state = 0;
            Joystick.send_now();
        }
    }

}

