#include <stdio.h>
#include <pico/stdio.h>
#include "i2ckbd.h"
#include "debug.h"

static uint8_t i2c_inited = 0;

void init_i2c_kbd() {
    gpio_set_function(I2C_KBD_SCL, GPIO_FUNC_I2C);
    gpio_set_function(I2C_KBD_SDA, GPIO_FUNC_I2C);
    i2c_init(I2C_KBD_MOD, I2C_KBD_SPEED);
    gpio_pull_up(I2C_KBD_SCL);
    gpio_pull_up(I2C_KBD_SDA);

    i2c_inited = 1;
}

int read_i2c_kbd() {
    int retval;
    static int ctrlheld = 0;
    uint16_t buff = 0;
    unsigned char msg[2];
    int c = -1;
    msg[0] = 0x09;

    if (i2c_inited == 0) return -1;

    retval = i2c_write_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, msg, 1, false, 500000);
    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT) {
        DEBUG_PRINT("I2C write err\n");
        return -1;
    }

    sleep_ms(16);
    retval = i2c_read_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, (unsigned char *) &buff, 2, false, 500000);
    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT) {
        DEBUG_PRINT("I2C read err\n");
        return -1;
    }

    if (buff != 0) {
        if (buff == 0x7e03)ctrlheld = 0;
        else if (buff == 0x7e02) {
            ctrlheld = 1;
        } else if ((buff & 0xff) == 1) {//pressed
            c = buff >> 8;
            int realc = -1;
            switch (c) {
                default:
                    realc = c;
                    break;
            }
            c = realc;
            if (c >= 'a' && c <= 'z' && ctrlheld)c = c - 'a' + 1;
        }
        return c;
    }
    return -1;
}

int read_battery() {
    int retval;
    uint16_t buff = 0;
    unsigned char msg[2];
    msg[0] = 0x0b;

    if (i2c_inited == 0) return -1;

    retval = i2c_write_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, msg, 1, false, 500000);
    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT) {
        DEBUG_PRINT("Batt I2C write err\n");
        return -1;
    }
    sleep_ms(16);
    retval = i2c_read_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, (unsigned char *) &buff, 2, false, 500000);
    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT) {
        DEBUG_PRINT("Batt I2C read err\n");
        return -1;
    }

    if (buff != 0) {
        return buff;
    }
    return -1;
}

int read_bootmode() {
    int retval;
    unsigned char msg[2];
    msg[0] = 0x0e; // REG_ID_BOOT

    if (i2c_inited == 0) return -1;

    retval = i2c_write_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, msg, 1, false, 500000);
    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT) {
        DEBUG_PRINT("Boot I2C write err\n");
        return -1;
    }
    sleep_ms(16);
    retval = i2c_read_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, (unsigned char *) msg, 2, false, 500000);
    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT || msg[0] != 0x0e) {
        DEBUG_PRINT("Boot I2C read err\n");
        return -1;
    }

    return msg[1];
}