#include "esp_system.h"
#include "esp_log.h"
#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "u8g2.h"

#include "keyboard.h"
#include "gfx.h"
#include "fonts.h"

int key_init() {
	const char TAG[] = "key_init";
	esp_err_t ret;

	gpio_config_t conf = {
		pin_bit_mask: 1ull << 25 | 1ull << 26 | 1ull << 27,
		mode: GPIO_MODE_OUTPUT,
		pull_up_en: GPIO_PULLUP_DISABLE,
		pull_down_en: GPIO_PULLDOWN_DISABLE,
		intr_type: GPIO_INTR_DISABLE,
	};
	ret = gpio_config(&conf);
	if (ret != ESP_OK) {
		ESP_LOGE(TAG, "Could not configure keyboard GPIO: %s", esp_err_to_name(ret));
		return 0;
	}

	conf.pin_bit_mask = 1ull << 34 | 1ull << 35 | 1ull << 36 | 1ull << 39;
	conf.mode = GPIO_MODE_INPUT;
	ret = gpio_config(&conf);
	if (ret != ESP_OK) {
		ESP_LOGE(TAG, "Could not configure keyboard GPIO: %s", esp_err_to_name(ret));
		return 0;
	}

	return 1;
}

static int cols[] = { GPIO_NUM_25, GPIO_NUM_26, GPIO_NUM_27 };
static int rows[] = { GPIO_NUM_34, GPIO_NUM_35, GPIO_NUM_36, GPIO_NUM_39 };
key_state_t key_state, pressed, released;
TickType_t last_seen[KEY_COUNT];

void key_scan() {
	key_state_t new_state = 0;

	for (int c = 0; c < 3; c++) {
		gpio_set_level(cols[c], 1);
		for (int r = 0; r < 4; r++) {
			new_state = new_state | (gpio_get_level(rows[r]) << (r * 3 + c));
		}
		gpio_set_level(cols[c], 0);
	}

	pressed =   new_state & ~key_state;
	released = ~new_state &  key_state;
	TickType_t now = xTaskGetTickCount();
	for (int i = 0; i < KEY_COUNT; i++) {
		if (now - last_seen[i] < DEBOUNCE_TIMEOUT) {
			// We've already seen this one, remove it from our set
			pressed &= ~(1 << i);
			released &= ~(1 << i);
			continue;
		}
		if (((pressed | released) >> i) & 1) {
			last_seen[i] = now;
		}
	}

	key_state &= ~released;
	key_state |= pressed;

	if (pressed | released) {
		printf("pressed: %04X, released: %04X\n", pressed, released);
	}
}

key_event_t key_get_event(const TickType_t ticksToWait) {
	TickType_t ticksStart = xTaskGetTickCount();

	while (!(pressed | released)) {
		key_scan();
		if (pressed | released) {
			break;
		}
		if (ticksToWait == 0 || xTaskGetTickCount() - ticksStart > ticksToWait) {
			key_event_t e = {0, KEY_NO_EVENT};
			return e;
		}
		vTaskDelay(50 / portTICK_PERIOD_MS);
	}

	if (pressed) {
		for (int i = 0; i < KEY_COUNT; i++) {
			if ((pressed >> i) & 1) {
				// Clear this event
				pressed &= ~(1 << i);
				key_event_t e = {i, KEY_PRESSED};
				return e;
			}
		}
	} else if (released) {
		for (int i = 0; i < KEY_COUNT; i++) {
			if ((released >> i) & 1) {
				// Clear this event
				released &= ~(1 << i);
				key_event_t e = {i, KEY_RELEASED};
				return e;
			}
		}
	}

	key_event_t e = {0, KEY_NO_EVENT};
	return e;
}

keycode_t key_get_pressed() {
	key_event_t ke;
	do {
		ke = key_get_event(portMAX_DELAY);
	} while (ke.type != KEY_PRESSED);

	return ke.code;
}

void key_test() {
	// We do our own drawing, so disable the gfx thread
	gfx_set_enabled(false);

	printf("key_test\n");
	u8g2_SetFont(&u8g2, maximum_overdrift);

	while (true) {
		key_scan();

		printf("Key State: ");
		for (int i = 0; i < KEY_COUNT; i++) {
			printf("%d", (key_state >> (KEY_COUNT - i - 1)) & 1);
		}
		printf("\n");

		if (key_state == (BIT(KEY_OFF) | BIT(KEY_BAND))) {
			break;
		}

		u8g2_ClearBuffer(&u8g2);

		if (key_state & BIT(KEY_4)) {
			u8g2_DrawBox(&u8g2, 0, 0, 7, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_4)));
		u8g2_DrawStr(&u8g2, 1, 7, "4");

		if (key_state & BIT(KEY_BAND)) {
			u8g2_DrawBox(&u8g2, 48, 0, 25, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_BAND)));
		u8g2_DrawStr(&u8g2, 49, 7, "BAND");

		if (key_state & BIT(KEY_5)) {
			u8g2_DrawBox(&u8g2, 96, 0, 7, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_5)));
		u8g2_DrawStr(&u8g2, 97, 7, "5");

		if (key_state & BIT(KEY_1)) {
			u8g2_DrawBox(&u8g2, 0, 8, 7, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_1)));
		u8g2_DrawStr(&u8g2, 1, 15, "1");

		if (key_state & BIT(KEY_3)) {
			u8g2_DrawBox(&u8g2, 48, 8, 7, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_3)));
		u8g2_DrawStr(&u8g2, 49, 15, "3");

		if (key_state & BIT(KEY_2)) {
			u8g2_DrawBox(&u8g2, 96, 8, 7, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_2)));
		u8g2_DrawStr(&u8g2, 97, 15, "2");

		if (key_state & BIT(KEY_ON)) {
			u8g2_DrawBox(&u8g2, 0, 16, 13, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_ON)));
		u8g2_DrawStr(&u8g2, 1, 23, "ON");

		if (key_state & BIT(KEY_SNOOZE)) {
			u8g2_DrawBox(&u8g2, 48, 16, 37, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_SNOOZE)));
		u8g2_DrawStr(&u8g2, 49, 23, "SNOOZE");

		if (key_state & BIT(KEY_OFF)) {
			u8g2_DrawBox(&u8g2, 96, 16, 19, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_OFF)));
		u8g2_DrawStr(&u8g2, 97, 23, "OFF");

		if (key_state & BIT(KEY_TUNE_PLUS)) {
			u8g2_DrawBox(&u8g2, 0, 24, 29, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_TUNE_PLUS)));
		u8g2_DrawStr(&u8g2, 1, 31, "TUNE+");

		if (key_state & BIT(KEY_ENTER)) {
			u8g2_DrawBox(&u8g2, 48, 24, 31, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_ENTER)));
		u8g2_DrawStr(&u8g2, 49, 31, "ENTER");

		if (key_state & BIT(KEY_TUNE_MINUS)) {
			u8g2_DrawBox(&u8g2, 96, 24, 29, 7);
		}
		u8g2_SetDrawColor(&u8g2, !(key_state & BIT(KEY_TUNE_MINUS)));
		u8g2_DrawStr(&u8g2, 97, 31, "TUNE-");

		u8g2_SendBuffer(&u8g2);
	}

	gfx_set_enabled(true);
}