#include "LiquidCrystal.h"
#include "nrf_delay.h"
#include "nrf_error.h"
#include "nrf_gpio.h"
#include "app_timer.h"

#include <string.h>
#include <inttypes.h>

/*
 * Arduino | LCD | nrf51
 *      D4 | DB4 | P0.16
 *      D5 | DB5 | P0.17
 *      D6 | DB6 | P0.18
 *      D7 | DB7 | P0.19
 *      D8 |  RS | P0.20, register select. 0=instruction (write), 0=busy (read), 1=data
 *      D9 |  EN | P0.23, clock, data is clocked in/out on falling edge
 *     D10 | DB4 | P0.24
 */

enum liquid_crystal_cmd {
    LIQUID_CRYSTAL_CMD_CLEAR = 0x01,
    LIQUID_CRYSTAL_CMD_RETURN_HOME = 0x02,
    LIQUID_CRYSTAL_CMD_ENTRY_MODE_SET = 0x04,
    LIQUID_CRYSTAL_CMD_DISPLAY = 0x08,
    LIQUID_CRYSTAL_CMD_SHIFT = 0x10,
    LIQUID_CRYSTAL_CMD_FUNCTION_SET = 0x20,
    LIQUID_CRYSTAL_CMD_SET_CGRAM_ADDDRESS = 0x40,
    LIQUID_CRYSTAL_CMD_SET_DDRAM_ADDDRESS = 0x80,

    LIQUID_CRYSTAL_CMD_DELAY = 0x11,
    LIQUID_CRYSTAL_CMD_INIT_1 = 0x12,
    LIQUID_CRYSTAL_CMD_INIT_2 = 0x13,
    LIQUID_CRYSTAL_CMD_INIT_3 = 0x14,
};

static app_timer_id_t timer_id;

static struct {
    uint8_t pin_db4;
    uint8_t pin_db5;
    uint8_t pin_db6;
    uint8_t pin_db7;
    uint8_t pin_rs;
    uint8_t pin_en;
} data = { .pin_db4 = 16, .pin_db5 = 17, .pin_db6 = 18, .pin_db7 = 19, .pin_rs =
        20, .pin_en = 23, };

static void write_4(uint8_t value) {
    nrf_gpio_pin_write(data.pin_db4, value & 0x01);
    nrf_gpio_pin_write(data.pin_db5, value & 0x02);
    nrf_gpio_pin_write(data.pin_db6, value & 0x04);
    nrf_gpio_pin_write(data.pin_db7, value & 0x08);

    nrf_delay_us(10);
    nrf_gpio_pin_set(data.pin_en);
    nrf_delay_us(10); // 450ns is supposed to be sufficient
    nrf_gpio_pin_clear(data.pin_en);
}

static void write_value(uint8_t value, bool is_data, uint32_t delay) {
    nrf_gpio_pin_write(data.pin_rs, is_data);

    write_4(value >> 4);
    write_4(value);

    // something is going too fast. have to investigate.
//    nrf_delay_us(delay);
    nrf_delay_ms(delay);
}

uint32_t liquid_crystal_write_char(char chr) {
    write_value(chr, true, 50);
    return NRF_SUCCESS;
}

uint32_t liquid_crystal_write_string(char *chr) {
    for (; *chr != '\0'; chr++) {
        liquid_crystal_write_char(*chr);
    }
    return NRF_SUCCESS;
}

uint32_t liquid_crystal_write_string_len(char *chr, size_t max_len) {
    for (; *chr != '\0' && max_len > 0; chr++, max_len--) {
        liquid_crystal_write_char(*chr);
    }
    return NRF_SUCCESS;
}

uint32_t liquid_crystal_clear() {
    // The documentation doesn't specify a value, but everything else is 37us
    write_value(LIQUID_CRYSTAL_CMD_CLEAR, false, 50);
    return NRF_SUCCESS;
}

uint32_t liquid_crystal_return_home() {
    write_value(LIQUID_CRYSTAL_CMD_RETURN_HOME, false, 2000);
    return NRF_SUCCESS;
}

static struct {
    union {
        uint8_t value;
        struct {
            bool shift :1;
            bool increment :1;
            int :6;
        } fields __attribute__((packed));
    };
}__attribute__((packed)) entry_mode_state;

uint32_t liquid_crystal_entry_mode_set(bool increment, bool shift) {
    /*
     uint8_t value = LIQUID_CRYSTAL_CMD_ENTRY_MODE_SET;

     value |= increment ? 0x02 : 0x00;
     value |= shift ? 0x01 : 0x00;

     write_value(value, false);
     */

    entry_mode_state.fields.increment = increment;
    entry_mode_state.fields.shift = shift;
    write_value(entry_mode_state.value, false, 50);

    return NRF_SUCCESS;
}

static struct {
    union {
        uint8_t value;
        struct {
            bool blink :1;
            bool cursor_on :1;
            bool display_on :1;
            int :5;
        } fields __attribute__((packed));
    };
}__attribute__((packed)) display_state;

uint32_t liquid_crystal_display(bool display_on, bool cursor_on, bool blink) {
    display_state.fields.display_on = display_on;
    display_state.fields.cursor_on = cursor_on;
    display_state.fields.blink = blink;

    write_value(display_state.value, false, 50);

    return NRF_SUCCESS;
}

static struct {
    union {
        uint8_t value;
        struct {
            int :2;
            bool many_dots :1;
            bool two_line :1;
            bool data_length :1;
            int :3;
        } fields __attribute__((packed));
    };
}__attribute__((packed)) function_set_state;

struct lcd_task {
    struct lcd_task* next;

    enum liquid_crystal_cmd cmd;
    uint32_t delay;
};

struct lcd_task lcd_tasks[10];
struct lcd_task* next_lcd_task = NULL;
uint32_t ready_at_tick;

static inline struct lcd_task* alloc_task(enum liquid_crystal_cmd cmd, uint32_t delay) {
    for (int i = 0; i < sizeof(lcd_tasks); i++) {
        struct lcd_task *t = &lcd_tasks[i];
        if (t->next == t) {
            t->cmd = cmd;
            t->delay = delay;
            return t;
        }
    }

    return NULL;
}

static inline struct lcd_task* pop_task() {
    struct lcd_task* task = next_lcd_task;
    if (task == NULL) {
        return NULL;
    }

    next_lcd_task = task->next;
    task->next = NULL;

    return task;
}

void add_task(struct lcd_task *task) {
    if (next_lcd_task == NULL) {
        next_lcd_task = task;
        return;
    }

    struct lcd_task *t = next_lcd_task;

    while(t->next != t) {
        t = t->next;
    }

    t->next = task;
}

static uint32_t execute_task();

static uint32_t schedule_next() {
    if (next_lcd_task == NULL) {
        return NRF_SUCCESS;
    }

    uint32_t now;

    uint32_t err = app_timer_cnt_get(&now);
    if (err) {
        return err;
    }

    uint32_t time_until_ready = ready_at_tick - now;

    if (time_until_ready <= 0) {
        return execute_task();
    } else {
        ready_at_tick = now + next_lcd_task->delay;
        return app_timer_start(timer_id, time_until_ready, NULL);
    } 
}

static uint32_t execute_task() {
    struct lcd_task *const task = pop_task(),
                    *new_task = NULL;

    if (task == NULL) {
        return NRF_SUCCESS;
    }

    switch(task->cmd) {
        case LIQUID_CRYSTAL_CMD_DELAY:
            // noop
            break;
        case LIQUID_CRYSTAL_CMD_INIT_1:
            nrf_gpio_pin_clear(data.pin_rs);
            nrf_gpio_pin_clear(data.pin_en);

            new_task = alloc_task(LIQUID_CRYSTAL_CMD_INIT_2, 50);
            if (new_task == NULL) {
                return NRF_ERROR_NO_MEM;
            }
            break;
        case LIQUID_CRYSTAL_CMD_INIT_2:
            new_task = alloc_task(LIQUID_CRYSTAL_CMD_INIT_3, 50);
            if (new_task == NULL) {
                return NRF_ERROR_NO_MEM;
            }
            break;
        case LIQUID_CRYSTAL_CMD_INIT_3:
            break;
        default:
            return NRF_ERROR_INTERNAL;
    }

    if (new_task != NULL) {
        add_task(new_task);
    }

    return schedule_next();
}

static void timeout_handler() {
    uint32_t err = execute_task();

    if (err != NRF_SUCCESS) {
        printf("lcd: error executing last task: %" PRIu32 "\r\n", err);
    }
}

uint32_t liquid_crystal_init(bool data_length, bool two_line, bool many_dots) {
    for(int i = 0; i < sizeof(lcd_tasks); i++) {
        lcd_tasks[i].next = &lcd_tasks[i];
    }
    ready_at_tick = 0;

    nrf_gpio_cfg_output(data.pin_db4);
    nrf_gpio_cfg_output(data.pin_db5);
    nrf_gpio_cfg_output(data.pin_db6);
    nrf_gpio_cfg_output(data.pin_db7);
    nrf_gpio_cfg_output(data.pin_rs);
    nrf_gpio_cfg_output(data.pin_en);

    uint32_t err = app_timer_create(&timer_id, APP_TIMER_MODE_SINGLE_SHOT, timeout_handler);
    if (err) {
        return err;
    }

    display_state.value = LIQUID_CRYSTAL_CMD_DISPLAY;
    entry_mode_state.value = LIQUID_CRYSTAL_CMD_ENTRY_MODE_SET;
    function_set_state.value = LIQUID_CRYSTAL_CMD_FUNCTION_SET;

    function_set_state.fields.data_length = data_length;
    function_set_state.fields.two_line = two_line;
    function_set_state.fields.many_dots = many_dots;

//    for (int i = 0; i < 3; i++) {
//        nrf_gpio_pin_set(data.pin_en);
//        nrf_delay_ms(100);
//        nrf_gpio_pin_clear(data.pin_en);
//        nrf_delay_ms(100);
//    }

    // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION!
    // according to datasheet, we need at least 40ms after power rises above 2.7V
    // before sending commands. Arduino can turn on way before 4.5V so we'll wait 50
    nrf_delay_ms(50);

    nrf_gpio_pin_clear(data.pin_rs);
    nrf_gpio_pin_clear(data.pin_en);

//    struct lcd_task *task = alloc_task(LIQUID_CRYSTAL_CMD_INIT_1, 50000);
//    add_task(task);

    write_4(0x03);
    nrf_delay_us(4500);

    write_4(0x03);
    nrf_delay_us(4500);

    write_4(0x03);
    nrf_delay_us(150);

    write_4(0x02);

    write_value(function_set_state.value, false, 50);

    return schedule_next();
}

uint32_t liquid_crystal_reset() {
//    liquid_crystal_clear();
//    liquid_crystal_return_home();
//    liquid_crystal_entry_mode_set(true, false);

    struct lcd_task *task = alloc_task(LIQUID_CRYSTAL_CMD_CLEAR, 50);
    if (task == NULL) {
        return NRF_ERROR_NO_MEM;
    }
    add_task(task);

    task = alloc_task(LIQUID_CRYSTAL_CMD_RETURN_HOME, 2000);
    if (task == NULL) {
        return NRF_ERROR_NO_MEM;
    }
    add_task(task);

    task = alloc_task(LIQUID_CRYSTAL_CMD_ENTRY_MODE_SET, 50);
    if (task == NULL) {
        return NRF_ERROR_NO_MEM;
    }

    add_task(task);

    return NRF_SUCCESS;
}

uint32_t liquid_crystal_set_cursor(int column, int row) {
    uint8_t value = LIQUID_CRYSTAL_CMD_SET_DDRAM_ADDDRESS;

    if (row == 1) {
        value += 0x40;
    } else if (row == 2) {
        value += 14;
    } else if (row == 3) {
        value += 54;
    }

    value += column;

    write_value(value, false, 50);

    return NRF_SUCCESS;
}