#include "io.h"
// @todo We should probably add DATA_0... as defines
#define CLOCK_PIN GPIO_PIN_11
#define CLOCK_PORT GPIOB

#define RESET_PIN GPIO_PIN_3
#define RESET_PORT GPIOE

/* #define ADDRESS_PORT GPIOD */
GPIO_TypeDef* ADDRESS_PORT[] = {GPIOD,      GPIOD,      GPIOD,      GPIOD,       GPIOB,       GPIOB,      GPIOB,      GPIOB,
                                GPIOB,      GPIOE,      GPIOE,      GPIOA,       GPIOA,       GPIOD,      GPIOD,      GPIOD};
const uint16_t ADDRESS_PIN[] = {GPIO_PIN_4, GPIO_PIN_5, GPIO_PIN_6, GPIO_PIN_7,  GPIO_PIN_3,  GPIO_PIN_4, GPIO_PIN_5, GPIO_PIN_8,
                                GPIO_PIN_9, GPIO_PIN_0, GPIO_PIN_1, GPIO_PIN_14, GPIO_PIN_15, GPIO_PIN_0, GPIO_PIN_1, GPIO_PIN_3};

GPIO_TypeDef* DATA_PORT[] = {GPIOD,       GPIOD,       GPIOD,       GPIOD,       GPIOC,      GPIOC,      GPIOA,      GPIOA};
const uint16_t DATA_PIN[] = {GPIO_PIN_12, GPIO_PIN_13, GPIO_PIN_14, GPIO_PIN_15, GPIO_PIN_6, GPIO_PIN_7, GPIO_PIN_9, GPIO_PIN_10};

GPIO_TypeDef* DEV_SELECT_PORT[] = {GPIOD,       GPIOD,       GPIOD,      GPIOD,      GPIOB,       GPIOB,       GPIOB,       GPIOB};
const uint16_t DEV_SELECT_PIN[] = {GPIO_PIN_11, GPIO_PIN_10, GPIO_PIN_9, GPIO_PIN_8, GPIO_PIN_15, GPIO_PIN_14, GPIO_PIN_13, GPIO_PIN_12};

#define READ_PIN GPIO_PIN_0
#define READ_PORT GPIOC

#define WRITE_PIN GPIO_PIN_13
#define WRITE_PORT GPIOC

#define MEMORY_REQUEST_PIN GPIO_PIN_0
#define MEMORY_REQUEST_PORT GPIOA

#define IO_REQUEST_PIN GPIO_PIN_1
#define IO_REQUEST_PORT GPIOA

#define M1_PIN GPIO_PIN_2
#define M1_PORT GPIOE

#define BUSRQ_PORT GPIOE
#define BUSRQ_PIN GPIO_PIN_4

#define BUSAK_PORT GPIOE
#define BUSAK_PIN GPIO_PIN_6

#define INT_PORT GPIOC
#define INT_PIN GPIO_PIN_1

uint16_t read_address() {
	uint16_t address = 0;
	address |= (GPIOD->IDR & (GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7)) >> 4;
	address |= (GPIOB->IDR & (GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5)) << 1;
	address |= (GPIOB->IDR & (GPIO_PIN_8 | GPIO_PIN_9)) >> 1;
	address |= (GPIOE->IDR & (GPIO_PIN_0 | GPIO_PIN_1)) << 9;
	address |= (GPIOA->IDR & (GPIO_PIN_14 | GPIO_PIN_15)) >> 3;
	address |= (GPIOD->IDR & (GPIO_PIN_0 | GPIO_PIN_1)) << 13;
	address |= (GPIOD->IDR & GPIO_PIN_3) << 12;

	return address;
}

void write_address(uint16_t address) {
	for (int i = 0; i < 16; ++i) {
		HAL_GPIO_WritePin(ADDRESS_PORT[i], ADDRESS_PIN[i], address & (1 << i) ? GPIO_PIN_SET : GPIO_PIN_RESET);
	}
}

void enable_address_out(uint8_t enable) {
	GPIO_InitTypeDef GPIO_InitStruct = {0};
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

	for (int i = 0; i < 16; ++i) {
		GPIO_InitStruct.Pin = ADDRESS_PIN[i];
		GPIO_InitStruct.Mode = enable ? GPIO_MODE_OUTPUT_PP : GPIO_MODE_INPUT;
		HAL_GPIO_Init(ADDRESS_PORT[i], &GPIO_InitStruct);
	}
}

uint8_t read_data() {
	uint16_t data = 0;

	data |= (GPIOD->IDR & (GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15)) >> 12;
	data |= (GPIOC->IDR & (GPIO_PIN_6 | GPIO_PIN_7)) >> 2;
	data |= (GPIOA->IDR & (GPIO_PIN_9 | GPIO_PIN_10)) >> 3;

	return data;
}

void write_data(uint8_t value) {
	GPIOD->BSRR= ((value & 0xF) | ((~value & 0xF) << 16)) << 12;
	GPIOC->BSRR= ((value & 0x30) | ((~value & 0x30) << 16)) << 2;
	GPIOA->BSRR = ((value & 0xC0) | ((~value & 0xC0) << 16)) << 3;
}

void enable_data_out(uint8_t enable) {
	static uint8_t cur = 2;

	if (cur != enable) {
		cur = enable;

		if (enable) {
			GPIOD->MODER |= (GPIO_MODER_MODER12_0 | GPIO_MODER_MODER13_0 | GPIO_MODER_MODER14_0 | GPIO_MODER_MODER15_0);
			GPIOC->MODER |= (GPIO_MODER_MODER6_0 | GPIO_MODER_MODER7_0);
			GPIOA->MODER |= (GPIO_MODER_MODER9_0 | GPIO_MODER_MODER10_0);
		} else {
			// @todo We can optimze this in a similair manner as above
			GPIOD->MODER &= ~(GPIO_MODER_MODER12);
			GPIOD->MODER &= ~(GPIO_MODER_MODER13);
			GPIOD->MODER &= ~(GPIO_MODER_MODER14);
			GPIOD->MODER &= ~(GPIO_MODER_MODER15);

			GPIOC->MODER &= ~(GPIO_MODER_MODER6);
			GPIOC->MODER &= ~(GPIO_MODER_MODER7);

			GPIOA->MODER &= ~(GPIO_MODER_MODER9);
			GPIOA->MODER &= ~(GPIO_MODER_MODER10);
		}
	}
}

void select_device(uint8_t device) {
	static uint8_t cur = 0xFF;

	if (cur != device) {
		cur = device;

		GPIOB->BSRR = ((device & 0xF) | ((~device & 0xF) << 16)) << 12;
		GPIOD->BSRR = ((device & 0xF0) | ((~device & 0xF0) << 16)) << 4;
	}
}


void set_reset(uint8_t high) {
	GPIOE->BSRR = ((~high & 1) | ((high & 1) << 16)) << 3;
}

void set_clock(uint8_t high) {
	GPIOB->BSRR = ((high & 1) | ((~high & 1) << 16)) << 11;
}

uint8_t has_memrq() {
	return !(MEMORY_REQUEST_PORT->IDR & MEMORY_REQUEST_PIN);
}

void send_memrq(uint8_t high) {
	HAL_GPIO_WritePin(MEMORY_REQUEST_PORT, MEMORY_REQUEST_PIN, high ? GPIO_PIN_RESET : GPIO_PIN_SET);
}

void enable_memrq_out(uint8_t enable) {
	GPIO_InitTypeDef GPIO_InitStruct = {0};
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.Pin = MEMORY_REQUEST_PIN;
	GPIO_InitStruct.Mode = enable ? GPIO_MODE_OUTPUT_PP : GPIO_MODE_INPUT;
	HAL_GPIO_Init(MEMORY_REQUEST_PORT, &GPIO_InitStruct);
}

uint8_t has_ioreq() {
	return !(IO_REQUEST_PORT->IDR & IO_REQUEST_PIN);
}

uint8_t has_m1() {
	return !(M1_PORT->IDR & M1_PIN);
}

uint8_t has_rd() {
	return !(READ_PORT->IDR & READ_PIN);
}

void send_rd(uint8_t high) {
	HAL_GPIO_WritePin(READ_PORT, READ_PIN, high ? GPIO_PIN_RESET : GPIO_PIN_SET);
}

void enable_rd_out(uint8_t enable) {
	GPIO_InitTypeDef GPIO_InitStruct = {0};
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.Pin = READ_PIN;
	GPIO_InitStruct.Mode = enable ? GPIO_MODE_OUTPUT_PP : GPIO_MODE_INPUT;
	HAL_GPIO_Init(READ_PORT, &GPIO_InitStruct);
}

uint8_t has_wr() {
	return !(WRITE_PORT->IDR & WRITE_PIN);
}

void send_wr(uint8_t high) {
	HAL_GPIO_WritePin(WRITE_PORT, WRITE_PIN, high ? GPIO_PIN_RESET : GPIO_PIN_SET);
}

void enable_wr_out(uint8_t enable) {
	GPIO_InitTypeDef GPIO_InitStruct = {0};
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.Pin = WRITE_PIN;
	GPIO_InitStruct.Mode = enable ? GPIO_MODE_OUTPUT_PP : GPIO_MODE_INPUT;
	HAL_GPIO_Init(WRITE_PORT, &GPIO_InitStruct);
}

uint8_t has_busak() {
	return !(BUSAK_PORT->IDR & BUSAK_PIN);
}

void send_busrq(uint8_t control) {
	if (control) {
		// Set correct defaults
		send_memrq(0);
		send_wr(0);
		send_rd(0);
	}

	// @todo This should not happen unitl busak
	enable_memrq_out(control);
	enable_rd_out(control);
	enable_wr_out(control);
	enable_address_out(control);

	HAL_GPIO_WritePin(BUSRQ_PORT, BUSRQ_PIN, control ? GPIO_PIN_RESET : GPIO_PIN_SET);
}

void send_int(uint8_t control) {
	static uint8_t cur = 2;

	if (cur != control) {
		cur = control;
		GPIO_InitTypeDef GPIO_InitStruct = {0};
		GPIO_InitStruct.Pull = GPIO_NOPULL;
		GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
		GPIO_InitStruct.Pin = INT_PIN;
		GPIO_InitStruct.Mode = control ? GPIO_MODE_OUTPUT_PP : GPIO_MODE_INPUT;
		HAL_GPIO_Init(INT_PORT, &GPIO_InitStruct);

		HAL_GPIO_WritePin(INT_PORT, INT_PIN, control ? GPIO_PIN_RESET : GPIO_PIN_SET);
	}
}