#include <Wire.h>

#define TRACKBALL_ADDR		0x0A
#define TRACKBALL_REG_LEFT	0x04

// using my own masks here since the inbuilt ones conflict with arrow keys etc.
#define MASK_SHIFT  0x100
#define MASK_ALTGR  0x200

const byte PIN_A = 33;
const byte PIN_B = 32;
const byte PIN_C = 31;
const byte PIN_D = 24;
const byte PIN_E = 25;
const byte PIN_F = 26;

const byte PIN_MOUSE_UL = 27;
const byte PIN_MOUSE_UR = 30;
const byte PIN_MOUSE_DL = 28;
const byte PIN_MOUSE_DR = 29;

const byte PIN_LED1 = 6;
const byte PIN_LED2 = 1;
const byte PIN_LED3 = 3;

const byte PIN_BALL_SDA = 18;
const byte PIN_BALL_SCL = 19;

enum Button { A, B, C, D, E, F, MouseUL, MouseUR, MouseDL, MouseDR };
const int button_pins[10] = {
    PIN_A, PIN_B, PIN_C, PIN_D, PIN_E, PIN_F,
    PIN_MOUSE_UL, PIN_MOUSE_UR, PIN_MOUSE_DL, PIN_MOUSE_DR
};
const int debounce_time = 50;
int button_timers[10] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };

// different actions that the keyboard can take
// - press (and release) a key
// - toggle a modifier
enum ActionType { PressKey, ToggleModifier };

const int button_characters[6] = { KEY_A, KEY_B, KEY_C, KEY_D, KEY_E, KEY_F };
const int button_characters_symbol[6] = {
    KEY_1, KEY_2, KEY_3, KEY_4, KEY_5, KEY_6
};

/*
backspace, space (keep those two first since they overlap with the rest )
g, k, o, s, w, native
*/
const byte chords[8] = { 7, 56, 24, 48, 3, 6, 40, 5 };
// buttons that can be combined with a chord for a keypress
const byte chord_buttons[24] = {
    Button::D, Button::E, Button::F,
    Button::A, Button::B, Button::C,
    Button::A, Button::B, Button::C,
    Button::A, Button::B, Button::C,
    Button::D, Button::E, Button::F,
    Button::D, Button::E, Button::F,
    Button::A, Button::B, Button::C,
    Button::D, Button::E, Button::F,
};
// keypresses for chords in sets of 4
// index 0 is the chord on its own, 1-3 correspond to chord_buttons
const int chord_targets[32] = {
    KEY_BACKSPACE, KEY_LEFT, KEY_LEFT, KEY_HOME,
    KEY_SPACE, KEY_RIGHT, KEY_RIGHT, KEY_END,
    KEY_G, KEY_H, KEY_I, KEY_J,
    KEY_K, KEY_L, KEY_M, KEY_N,
    KEY_O, KEY_P, KEY_Q, KEY_R,
    KEY_S, KEY_T, KEY_U, KEY_V,
    KEY_W, KEY_X, KEY_Y, KEY_Z,
    0, 0, 0, 0
};
const int chord_targets_symbol[32] = {
    KEY_BACKSPACE, KEY_LEFT, KEY_LEFT, KEY_HOME,
    KEY_SPACE, KEY_RIGHT, KEY_RIGHT, KEY_END,
    KEY_0, KEY_7, KEY_8, KEY_9,
//  #              @                       ½                   &
    KEY_BACKSLASH, KEY_QUOTE + MASK_SHIFT, KEY_5 + MASK_ALTGR, KEY_7 + MASK_SHIFT,
//  +                       %                   =          ^
    KEY_EQUAL + MASK_SHIFT, KEY_5 + MASK_SHIFT, KEY_EQUAL, KEY_6 + MASK_SHIFT,
//  *                   $                   €                   £
    KEY_8 + MASK_SHIFT, KEY_4 + MASK_SHIFT, KEY_4 + MASK_ALTGR, KEY_3 + MASK_SHIFT,
//  (                   [               <                       {
    KEY_9 + MASK_SHIFT, KEY_LEFT_BRACE, KEY_COMMA + MASK_SHIFT, KEY_LEFT_BRACE + MASK_SHIFT,
//  )                   ]                >                        }
    KEY_0 + MASK_SHIFT, KEY_RIGHT_BRACE, KEY_PERIOD + MASK_SHIFT, KEY_RIGHT_BRACE + MASK_SHIFT,
};

/*
dash, backslash, slash, apostrophe, comma, exclamation point, question mark,
period, up, down, page up, page down, shift, symbol shift, switch keyset,
escape, control, alt, delete, insert, tab, enter
*/
// TODO: add the middle specials for function keys instead of the weird symbols
const byte specials[22] = {
    17, 51, 30, 10, 20, 12, 33, 34, 9, 36, 27,
    54, 18, 45, 63, 31, 47, 55, 62, 43, 61, 59
};
enum Modifier { Shift, SymbolShift, Keyset, Control, Alt };
const int special_action_targets[22] = {
//  -   \   /   '
    KEY_MINUS, KEY_MINUS + MASK_ALTGR, KEY_SLASH, KEY_QUOTE,
//  ,   !   ?   .
    KEY_COMMA, KEY_1 + MASK_SHIFT, KEY_SLASH + MASK_SHIFT, KEY_PERIOD,
    KEY_UP, KEY_DOWN, KEY_PAGE_UP, KEY_PAGE_DOWN,
    Modifier::Shift, Modifier::SymbolShift, Modifier::Keyset,
    KEY_ESC, Modifier::Control, Modifier::Alt, KEY_DELETE, KEY_INSERT,
    KEY_TAB, KEY_ENTER
};
const int special_action_targets_symbol[22] = {
//  _   `   ´   "
    KEY_MINUS + MASK_SHIFT, KEY_TILDE, KEY_SEMICOLON + MASK_ALTGR, KEY_2 + MASK_SHIFT,
//  ;   |   ~   :
    KEY_SEMICOLON, KEY_TILDE + MASK_ALTGR, KEY_BACKSLASH + MASK_SHIFT, KEY_SEMICOLON + MASK_SHIFT,
    0, 0, 0, 0,
    0, 0, 0,
    0, 0, 0, 0, 0,
    0, 0
};
const byte special_action_target_types[22] = {
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0, 0, 0, 0
};

byte key_pressed = 0;
byte key_pressed_total = 0;
bool chord_used = false;
bool chorded_pressed = false;

bool mod_shift = false;
bool mod_shift_lock = false;
bool mod_symbol = false;
bool mod_symbol_lock = false;
bool mod_control = false;
bool mod_alt = false;
bool mod_altgr = false;

IntervalTimer trackball_timer;
volatile bool trackball_update = false;

byte mouse_up = 0;
byte mouse_down = 0;
byte mouse_left = 0;
byte mouse_right = 0;
bool mouse_button = false;

const int num_mouse_samples = 10;
int current_mouse_sample = 0;
int mouse_movement_sample = 0;
float mouse_acceleration = 0.0f;
const float mouse_accel_factor = 0.7f;

const int mouse_scroll_sensitivity = 6;
int mouse_scroll_dist = 0;
bool mouse_middle_pressed = false;
bool mouse_scrolled = false;

void setup() {
    pinMode(PIN_A, INPUT_PULLUP);
    pinMode(PIN_B, INPUT_PULLUP);
    pinMode(PIN_C, INPUT_PULLUP);
    pinMode(PIN_D, INPUT_PULLUP);
    pinMode(PIN_E, INPUT_PULLUP);
    pinMode(PIN_F, INPUT_PULLUP);

    pinMode(PIN_MOUSE_UL, INPUT_PULLUP);
    pinMode(PIN_MOUSE_UR, INPUT_PULLUP);
    pinMode(PIN_MOUSE_DL, INPUT_PULLUP);
    pinMode(PIN_MOUSE_DR, INPUT_PULLUP);

    pinMode(PIN_LED1, OUTPUT);
    pinMode(PIN_LED2, OUTPUT);
    pinMode(PIN_LED3, OUTPUT);

    // poll trackball at 10kHz
    trackball_timer.begin(trackball_isr, 100);

    Wire.begin();
}

void loop() {
    /*
    BUTTONS
    */
    for (byte b = 0; b < 10; b++) {
        // button is pressed
        if (!digitalRead(button_pins[b])) {
            // increment button timer if it's not full
            if (button_timers[b] < debounce_time)
                button_timers[b]++;
            else {
                // update pressed gkos keys
                if (b < 6) {
                    key_pressed |= 1 << b;
                    key_pressed_total |= 1 << b;
                }
                // update pressed mouse keys
                else {
                    switch (b) {
                        case Button::MouseUL:
                            Mouse.press(MOUSE_LEFT);
                            break;
                        case Button::MouseUR:
                            Mouse.press(MOUSE_RIGHT);
                            break;
                        case Button::MouseDL:
                        case Button::MouseDR:
                            mouse_middle_pressed = true;
                            break;
                    }
                }
            }
        }
        // button is released
        else {
            // if the button had been debounced, it counts as a press
            if (button_timers[b] >= debounce_time) {
                // gkos key released
                if (b < 6) {
                    key_pressed &= ~(1 << b);
                    // check if it completed a chord
                    key_released(b);
                    update_leds();
                    if (key_pressed == 0) {
                        key_pressed_total = 0;
                        // clear flags
                        chord_used = false;
                        chorded_pressed = false;
                    }
                }
                // mouse button released
                else {
                    switch (b) {
                        case Button::MouseUL:
                            Mouse.release(MOUSE_LEFT);
                            break;
                        case Button::MouseUR:
                            Mouse.release(MOUSE_RIGHT);
                            break;
                        case Button::MouseDL:
                        case Button::MouseDR:
                            if (!mouse_scrolled)
                                Mouse.click(MOUSE_MIDDLE);
                            mouse_middle_pressed = mouse_scrolled = false;
                            mouse_scroll_dist = 0;
                            break;
                    }
                }
            }
            button_timers[b] = 0;
        }
    }

    /*
    TRACKBALL
    */
    noInterrupts();
    if (trackball_update) {
        Wire.beginTransmission(TRACKBALL_ADDR);
        Wire.write(TRACKBALL_REG_LEFT);
        Wire.endTransmission();

        Wire.requestFrom(TRACKBALL_ADDR, 5);
        mouse_left = Wire.read();
        mouse_right = Wire.read();
        mouse_up = Wire.read();
        mouse_down = Wire.read();
        mouse_button = Wire.read();

        int mouse_x = mouse_right - mouse_left;
        int mouse_y = mouse_down - mouse_up;

        mouse_movement_sample += abs(mouse_x) + abs(mouse_y);
        current_mouse_sample++;
        if (current_mouse_sample > num_mouse_samples) {
            current_mouse_sample = 0;
            mouse_acceleration = 1.0f + mouse_movement_sample * mouse_accel_factor;
            mouse_movement_sample = 0;
        }

        if (mouse_middle_pressed) {
            mouse_scroll_dist += mouse_y;
            if (abs(mouse_scroll_dist) > mouse_scroll_sensitivity) {
                Mouse.scroll(mouse_scroll_dist % mouse_scroll_sensitivity);
                mouse_scroll_dist %= mouse_scroll_sensitivity;
                mouse_scrolled = true;
            }
        } else {
            Mouse.move(mouse_x * mouse_acceleration, mouse_y * mouse_acceleration);
        }

        trackball_update = false;
    }
    interrupts();
}

// returns true if a keypress was completed
void key_released(byte key) {
    int target = 0;

    /*
    SPECIALS
    */
    bool is_special = false;
    byte special = 0;
    for (byte b = 0; b < 22; b++) {
        if (key_pressed_total == specials[b]) {
            is_special = true;
            special = b;
            break;
        }
    }

    if (is_special && !chord_used) {
        // only register a special keypress when all keys have been released
        if (key_pressed != 0)
            return;

        target = special_action_targets[special];
        if (mod_shift || mod_shift_lock || mod_symbol || mod_symbol_lock) {
            if (special_action_targets_symbol[special] != 0)
                target = special_action_targets_symbol[special];
        }

        switch (special_action_target_types[special]) {
            case ActionType::PressKey:
                press_key(target);
                return;
            case ActionType::ToggleModifier:
                switch (target) {
                    case Modifier::Shift:
                        mod_symbol = mod_symbol_lock = false;
                        if (mod_shift_lock) {
                            mod_shift_lock = false;
                        } else if (mod_shift) {
                            mod_shift = false;
                            mod_shift_lock = true;
                        } else {
                            mod_shift = true;
                        }
                        return;
                    case Modifier::SymbolShift:
                        mod_shift = mod_shift_lock = mod_symbol_lock = false;
                        mod_symbol = !mod_symbol;
                        return;
                    case Modifier::Keyset:
                        mod_shift = mod_shift_lock = mod_symbol = false;
                        mod_symbol_lock = !mod_symbol_lock;
                        return;
                    case Modifier::Control:
                        mod_control = !mod_control;
                        return;
                    case Modifier::Alt:
                        mod_alt = !mod_alt;
                        return;
                }
        }
        return;
    }

    /*
    CHORDS
    */
    bool is_chord = false;
    byte chord = 0;
    for (byte b = 0; b < 8; b++) {
        if ((key_pressed_total & chords[b]) == chords[b]) {
            is_chord = true;
            chord = b;
            chord_used = true;
            break;
        }
    }

    if (is_chord) {
        // chord on its own
        if (!chorded_pressed && key_pressed == 0 &&
                key_pressed_total == chords[chord]) {
            target = chord_targets[chord * 4];
            if (mod_symbol || mod_symbol_lock)
                target = chord_targets_symbol[chord * 4];

            press_key(target);
            return;
        }
        // keys that can be chorded with
        for (byte b = 0; b < 3; b++) {
            if (key == chord_buttons[chord * 3 + b]) {
                target = chord_targets[chord * 4 + 1 + b];
                if (mod_symbol || mod_symbol_lock)
                    target = chord_targets_symbol[chord * 4 + 1 + b];

                // erase key from total so you can hold the chord down for
                // multiple chorded keypresses
                key_pressed_total &= ~(1 << key);
                chorded_pressed = true;
                press_key(target);
                return;
            }
        }
        return;
    }

    /*
    REGULAR KEYS
    */
    // keypress
    if (mod_symbol || mod_symbol_lock)
        press_key(button_characters_symbol[key]);
    else
        press_key(button_characters[key]);
    return;
}

void press_key(int key) {
    // check if modifiers need to be forced
    if (key & MASK_SHIFT) {
        mod_shift = true;
        key &= ~(MASK_SHIFT);
    }
    if (key & MASK_ALTGR) {
        mod_altgr = true;
        key &= ~(MASK_ALTGR);
    }

    // modifiers
    if (mod_shift || mod_shift_lock)
        Keyboard.press(KEY_LEFT_SHIFT);
    if (mod_control)
        Keyboard.press(KEY_LEFT_CTRL);
    if (mod_alt)
        Keyboard.press(KEY_LEFT_ALT);
    if (mod_altgr)
        Keyboard.press(KEY_RIGHT_ALT);

    // keypress
    Keyboard.press(key);
    Keyboard.release(key);

    // release modifiers
    if (mod_shift || mod_shift_lock)
        Keyboard.release(KEY_LEFT_SHIFT);
    if (mod_control)
        Keyboard.release(KEY_LEFT_CTRL);
    if (mod_alt)
        Keyboard.release(KEY_LEFT_ALT);
    if (mod_altgr)
        Keyboard.release(KEY_RIGHT_ALT);

    // clear temporary modifiers
    mod_shift = mod_symbol = mod_control = mod_alt = mod_altgr = false;
}

void update_leds() {
    digitalWrite(PIN_LED1, mod_shift || mod_shift_lock);
    digitalWrite(PIN_LED2, mod_symbol || mod_symbol_lock);
    digitalWrite(PIN_LED3, mod_control || mod_alt);
}

int sign(int num) {
    if (num >= 0) return 1;
    return -1;
}

void trackball_isr() {
    trackball_update = true;
}