#include <stdint.h>
#include <stdio.h>

#include "keyboard.h"
#include "scancode.h"
#include "fifo.h"
#include "coroutine.h"
#include "main.h"

volatile struct {
	struct FIFO buffer;

	uint8_t sending : 1;
	uint8_t cmd;

	struct {
		uint8_t shift : 1;
		uint8_t caps_lock : 1;
		uint8_t ctrl : 1;
	} modifier;

	uint8_t released : 1;
} keyboard = {0};

// Send a command to the keyboard (THIS BLOCKS)
void send_keyboard_cmd(uint8_t value) {
	GPIO_InitTypeDef GPIO_InitStruct = {0};

	// Pull clock low
	GPIO_InitStruct.Pin = KEYBOARD_CLK_Pin;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_PULLDOWN;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	HAL_GPIO_Init(KEYBOARD_CLK_GPIO_Port, &GPIO_InitStruct);

	HAL_Delay(1);

	// Pull data low
	GPIO_InitStruct.Pin = KEYBOARD_DATA_Pin;
	HAL_GPIO_Init(KEYBOARD_DATA_GPIO_Port, &GPIO_InitStruct);

	// Release clock
	GPIO_InitStruct.Pin = KEYBOARD_CLK_Pin;
	GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(KEYBOARD_CLK_GPIO_Port, &GPIO_InitStruct);

	keyboard.sending = 1;
	keyboard.cmd = value;

	while (keyboard.sending) {};
}

// Add keyboard command to the queue
void queue_keyboard_cmd(uint8_t value) {
	FIFO_push(&keyboard.buffer, value);
}

// Send queued command
void send_keyboard_cmd_queue() {
	while (FIFO_size(&keyboard.buffer)) {
		send_keyboard_cmd(FIFO_pop(&keyboard.buffer));
	}
}

// Calculate parity
uint8_t parity(uint8_t value) {
	value ^= value >> 4;
	value ^= value >> 2;
	value ^= value >> 1;

	return (~value) & 1;
}

void process_scancode(uint8_t value, void (*callback)(uint8_t)) {
	switch (value) {
		// Left and right shift
		case 18:
		case 89:
			keyboard.modifier.shift = !keyboard.released;
			keyboard.released = 0;
			break;

			// Caps lock
		case 0x58:
			if (!keyboard.released) {
				keyboard.modifier.caps_lock = !keyboard.modifier.caps_lock;

				queue_keyboard_cmd(0xED);
				queue_keyboard_cmd(keyboard.modifier.caps_lock << 2);
			}
			keyboard.released = 0;
			break;

			// Left ctrl
		case 0x14:
			keyboard.modifier.ctrl = !keyboard.released;
			keyboard.released = 0;
			break;

		case 240:
			keyboard.released = 1;
			break;

		default:
			if (!keyboard.released) {
				char c = convert_scancode(keyboard.modifier.shift, value & 127);

				if (keyboard.modifier.caps_lock && !keyboard.modifier.shift && c >= 'a' && c <= 'z') {
					c -= 0x20;
				}

				if (keyboard.modifier.ctrl) {
					switch (c) {
						case 'c':
							c = 0x03;
							break;

						case 'e':
							c = 0x05;
							break;

						case 'p':
							c = 0x10;
							break;

						case 'r':
							c = 0x12;
							break;

						case 's':
							c = 0x13;
							break;

						case 'u':
							c = 0x15;
							break;

						case 'x':
							c = 0x18;
							break;

						case 'z':
							c = 0x1A;
							break;

						default:
							c = 0x00;
							break;
					}
				}

				if (c) {
					callback(c);
				}
			}
			keyboard.released = 0;
			break;
	}
}

void receive_scancode_bit(void (*callback)(uint8_t)) {
	uint8_t in = HAL_GPIO_ReadPin(KEYBOARD_DATA_GPIO_Port, KEYBOARD_DATA_Pin) == GPIO_PIN_SET;

	CO_BEGIN;

	// Check that the first bit is 0
	if (in != 0) {
		CO_BREAK;
	}

	CO_YIELD;

	static uint8_t value;
	value = 0;

	static int i = 0;
	for (i = 0; i < 8; ++i) {
		value |=  in << i;
		CO_YIELD;
	}

	// Check the parity
	if (in != parity(value)) {
		CO_BREAK;
	}

	CO_YIELD;

	// Check that the last bit is 1
	if (in != 1) {
		CO_BREAK;
	}

	process_scancode(value, callback);

	CO_END;
}

void send_cmd_bit() {
	CO_BEGIN;

	static int i;
	for (i = 0; i < 8; ++i) {
		// Send bit
		if ((keyboard.cmd >> i) & 1) {
			HAL_GPIO_WritePin(KEYBOARD_DATA_GPIO_Port, KEYBOARD_DATA_Pin, GPIO_PIN_SET);
		} else {
			HAL_GPIO_WritePin(KEYBOARD_DATA_GPIO_Port, KEYBOARD_DATA_Pin, GPIO_PIN_RESET);
		}
		CO_YIELD;
	}

	// Send parity
	if (parity(keyboard.cmd)) {
		HAL_GPIO_WritePin(KEYBOARD_DATA_GPIO_Port, KEYBOARD_DATA_Pin, GPIO_PIN_SET);
	} else {
		HAL_GPIO_WritePin(KEYBOARD_DATA_GPIO_Port, KEYBOARD_DATA_Pin, GPIO_PIN_RESET);
	}

	CO_YIELD;

	// Release data line
	GPIO_InitTypeDef GPIO_InitStruct = {0};
	HAL_GPIO_DeInit(KEYBOARD_DATA_GPIO_Port, KEYBOARD_DATA_Pin);
	GPIO_InitStruct.Pin = KEYBOARD_DATA_Pin;
	GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(KEYBOARD_DATA_GPIO_Port, &GPIO_InitStruct);

	CO_YIELD;

	// Wait for data to go low as ack
	while (HAL_GPIO_ReadPin(KEYBOARD_DATA_GPIO_Port, KEYBOARD_DATA_Pin) == GPIO_PIN_SET) {
		CO_YIELD;
	}

	keyboard.sending = 0;

	CO_END;
}

void keyboard_interrupt(void (*callback)(uint8_t)) {
	uint8_t clk_state = HAL_GPIO_ReadPin(KEYBOARD_CLK_GPIO_Port, KEYBOARD_CLK_Pin) == GPIO_PIN_SET;
	if (!keyboard.sending && clk_state) {
		receive_scancode_bit(callback);
	}

	if (keyboard.sending && !clk_state) {
		send_cmd_bit();
	}
}