#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <string.h>
#include "control.h"
#include "io.h"
#include "bsp_driver_sd.h"
#include "profiling.h"

// The top 4 address bits determine which device is used. (16 pages, 256 devices)
// 0xFF means that the device does not exist
// WATCH OUT THE DEVICE ADDRESS NEED TO BE REVERSED (MSB IS ON THE RIGHT)
uint8_t memory_map_0[8] = {0b00010000, 0b10001000, 0b01001000, 0b11001000, 0b00101000, 0b10101000, 0b01101000, 0b11101000};
uint8_t memory_map_1[8] = {0b00001000, 0b10001000, 0b01001000, 0b11001000, 0b00101000, 0b10101000, 0b01101000, 0b11101000};

Control control;

// @todo For some reason increasing SD_PAGES does not work
#define SD_PAGES 1
#define CPM_PAGE_SIZE 128

#define SD_PAGE_SIZE 512
#define CPM_PAGES (SD_PAGE_SIZE/CPM_PAGE_SIZE*SD_PAGES)

uint8_t get_device(uint16_t address) {
	uint8_t page = address >> 12;
	if (page >= 8) {
		return 0xFF;
	}

	if (control.memory_config == 0) {
		return memory_map_0[page];
	} else if (control.memory_config == 1) {
		return memory_map_1[page];
	}

	return 0xFF;
}

uint32_t calculate_lba() {
	uint32_t temp = (control.storage.lba_1) + (control.storage.lba_2 << 8) + (control.storage.lba_3 << 16);
	if (temp/CPM_PAGES != control.storage.lba/CPM_PAGES) {
		control.storage.dirty |= 1;
	}

	/* printf("LBA [1]: %li\n\r", control.storage.lba_1); */
	/* printf("LBA [2]: %li\n\r", control.storage.lba_2); */
	/* printf("LBA [3]: %li\n\r", control.storage.lba_3); */
	/* printf("LBA: %li\n\r", control.storage.lba); */

	return temp;
}

void program_eeprom() {
	static int address_counter = 0;
	static int read_counter = 0;
	static int write_counter = 0;

	// Check if we are done
	if (address_counter == control.eeprom.length) {
		// Mark as done and trigger Z80 reset
		control.eeprom.programming = 0;
		control.state = CONTROL_RESET_BEGIN;

		// Reset counters
		address_counter = 0;
		read_counter = 0;
		write_counter = 0;
		send_busrq(0);

		// Reset signals
		send_memrq(0);
		send_wr(0);
		send_rd(0);

		// Free memory used to store program
		free(control.eeprom.data);
		control.eeprom.data = NULL;
		control.eeprom.length = 0;

		// Notify the user
		printf("\nEEPROM Programmed!\n\r");

		// Turn of the update light
		HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);

		return;
	}

	// Turn on the update light
	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_SET);

	// Check if the data is written
	if (write_counter < 100) {
		// Select correct device and address
		select_device(memory_map_0[0]);
		write_address(address_counter);

		// Write the data
		write_data(control.eeprom.data[address_counter]);

		// Send write request
		enable_data_out(1);
		send_memrq(1);
		send_wr(1);

		write_counter++;
	} else {
		// Stop writing
		enable_data_out(0);
		send_memrq(1);
		send_wr(0);
		send_rd(1);

		uint8_t d = read_data();
		if (read_counter > 5 && d == control.eeprom.data[address_counter]) {
			address_counter++;
			write_counter = 0;
			read_counter = 0;

			send_rd(0);

			printf("Progress: %i/%i\r", address_counter, control.eeprom.length);
		} else if (read_counter > 4000) {
			// In case we failed, try again
			printf("Trying again! %x != %x (%i)\n\r", d, control.eeprom.data[address_counter], address_counter);
			read_counter = 0;
			write_counter = 0;
			send_rd(0);
		} else {
			read_counter++;
		}
	}
}

void handle_memrq() {
	uint16_t address = read_address();
	uint8_t device = get_device(address);
	select_device(device);
}

void handle_io_read() {
	uint8_t address = read_address() & 0xFF;

	switch (address) {
		case 0x02:
			write_data(control.input.c);
			control.input.received = 0;
			break;

		case 0x03:
			write_data(0x01 | 0x02*control.input.received);
			break;

		case 0x08:
			if (control.storage.ready && control.storage.action == 0x20) {
				write_data(control.storage.buffer[control.storage.counter + (control.storage.lba % CPM_PAGES)*CPM_PAGE_SIZE]);
				control.storage.counter++;

				if (control.storage.counter >= CPM_PAGE_SIZE) {
					control.storage.ready = 0;
					control.storage.action = 0;
				}
			} else {
				write_data(0x00);
			}

			break;

		case 0x0f:
			// Check if we need to read and in that case to the read
			if (control.storage.action && !control.storage.ready) {
				if (!control.storage.dirty || BSP_SD_ReadBlocks((uint32_t*)control.storage.buffer, control.storage.lba/CPM_PAGES, SD_PAGES, SD_DATATIMEOUT) == MSD_OK) {
					// Indicate that we are ready to read/write
					control.storage.ready = 1;
					control.storage.dirty = 0;
				} else {
					// If we failed to read we will try again next time
					/* printf("READ FAIL!\n\r"); */
				}
			}

			write_data(0x08*control.storage.ready);
			break;

		default:
			printf("IO Read: 0x00 @ %.2X\n\r", address);
			write_data(0x00);
	}

	enable_data_out(1);
}

void handle_io_write() {
	uint8_t address = read_address() & 0xFF;
	uint8_t value = read_data();

	switch (address) {
		case 0x00:
			control.memory_config = 0;
			break;

		case 0x01:
			control.memory_config = 1;
			break;

		case 0x02:
			printf("%c", value);
			break;

		case 0x08:
			if (control.storage.ready && control.storage.action == 0x30) {
				control.storage.buffer[control.storage.counter + (control.storage.lba % CPM_PAGES)*CPM_PAGE_SIZE] = value;
				control.storage.counter++;

				if (control.storage.counter >= CPM_PAGE_SIZE) {
					// @todo We need to figure out some way to actually prevent write fails from occuring
					if (BSP_SD_WriteBlocks((uint32_t*)control.storage.buffer, control.storage.lba/CPM_PAGES, SD_PAGES, SD_DATATIMEOUT) != MSD_OK) {
						printf("WRITE FAIL!!\n\r");
					}
					control.storage.ready = 0;
					control.storage.action = 0;
				}
			}
			break;

		case 0x0b:
			control.storage.lba_1 = value;
			break;

		case 0x0c:
			control.storage.lba_2 = value;
			break;

		case 0x0d:
			control.storage.lba_3 = value;
			break;

		case 0x0f:
			if (value == 0x20 || value == 0x30) {
				control.storage.lba = calculate_lba();
				control.storage.action = value;
				control.storage.counter = 0;
			}
			break;

		default:
			printf("IO Write: %.2X @ %.2X\n\r", value, address);
			break;
	}
}

void handle_ioreq() {
		if (has_wr()) {
			handle_io_write();
		} else if (has_rd()) {
			handle_io_read();
		}
}

void cycle() {
	// Make sure data get reset as input
	if (!control.eeprom.programming) {
		enable_data_out(0);
	}

	// We need this not detect IO multiple times
	static uint8_t had_ioreq = 0;
	if (!has_ioreq()) {
		had_ioreq = 0;
	}

	// @todo Enable this one we start doing things using interrupts again
	/* send_int(control.interrupt.type != CONTROL_INT_NONE); */

	if (control.eeprom.programming && has_busak()) {
		if (control.eeprom.programming) {
			program_eeprom();
		}
	} else if (control.eeprom.programming != has_busak()) {
		send_busrq(control.eeprom.programming);
	} else if (has_memrq()) {
		handle_memrq();
	} else if (has_ioreq() && !has_m1()) {
		had_ioreq++;
		if (had_ioreq == 3) {
			handle_ioreq();
		}
	} else if (has_ioreq() && has_m1()) {
		printf("Interrupt ackknowledged\n\r");
	}
}

void control_execute_state() {
	switch (control.state) {
		case CONTROL_STOP:
			// OK
			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
			// UPDATE
			HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
			// OTHER
			HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET);
			return;

		case CONTROL_RESET_BEGIN:
			control.state++;
			set_reset(1);

			// OK
			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
			// UPDATE
			HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
			// OTHER
			HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET);
			break;

		case CONTROL_RESET_BEGIN+1 ... CONTROL_RESET_END-1:
			control.state++;
			set_clock(control.state % 2);
			break;

		case CONTROL_RESET_END:
			control.state++;
			set_reset(0);
			break;

		case CONTROL_CLOCK_LOW:
			control.state++;
			set_clock(1);
			break;

		case CONTROL_CLOCK_HIGH:
			control.state--;
			cycle();
			set_clock(0);
			break;
	}
}

// @todo Properly reset everything
void control_reset() {
	free(control.storage.buffer);
	Control temp = {CONTROL_RESET_BEGIN, 0, {1, 0, 0, 0, 0, 0, 0, 0, NULL}, {0,0}, {0, 0, NULL}};
	control = temp;
	control.storage.buffer = (uint8_t*)malloc(SD_PAGES*SD_PAGE_SIZE);
}


uint8_t control_receive_program(uint8_t byte) {
	static uint16_t i = 0;
	static uint16_t c = 0;

	if (c <= 1) {
		control.eeprom.length += byte << c*8;
		c++;
	} else {
		if (c == 2) {
			control.eeprom.data = malloc(control.eeprom.length);
			c++;
		}
		control.eeprom.data[i] = byte;
		i++;
	}

	if (i >= control.eeprom.length) {
		i = 0;
		control.eeprom.programming = 1;
		return 1;
	}

	return 0;
}

void send_key(uint8_t c) {
	control.input.c = c;
	control.input.received = 1;
}