#include <hardware/clocks.h>
#include <hardware/gpio.h>
#include <hardware/timer.h>

#include "6502.h"
#include "6522.h"
#include "drive.h"

const uint32_t GPIO_CLK = 2;
const uint32_t GPIO_DATA = 3;
const uint32_t GPIO_DATA2 = 5;
const uint32_t GPIO_ATN = 4;
const uint32_t GPIO_LED = 25;

const uint8_t DEVICE = 8;

static uint8_t ram[2048];
extern uint8_t _binary__Users_max_Documents_c64_roms_dos1541_start[16384];
extern uint8_t _binary__Users_max_Downloads_blackbird_1_2_Blackbird_1_2_d64_start[174848];
// extern uint8_t _binary__Users_max_Downloads_3sira_still_human_by_arise_3sira_by_arise_A_d64_start[174848];

static uint32_t ts;
static cpu_t cpu;
static via_t via1;
static via_t via2;
static drive_t drive;

int8_t track_step;

void via_pa_read(via_t *via, uint8_t cycle) {
	if (via == &via2) {
		via->ira = drive.latched[(drive.current_idx + cycle) & (DRIVE_LOOKAHEAD - 1)];
	} else {
		via->ira = via->ora;
	}
}

void via_pb_read(via_t *via, uint8_t cycle) {
	if (via == &via1) {
		while (ts + cycle > time_us_32()) {}
		uint8_t clk = gpio_get(GPIO_CLK) ? 0 : 0b00000100;
		uint8_t data = gpio_get(GPIO_DATA) ? 0 : 0b00000001;
		uint8_t atn = gpio_get(GPIO_ATN) ? 0 : 0b10000000;
		uint8_t dev = (DEVICE - 8) << 5;
		uint8_t atna = via->orb & 0b00010000;

		via->irb = clk | data | atn | dev | atna;
	} else {
		uint8_t low = via->orb & 0b00001111;
		uint8_t sync = drive.sync[(drive.current_idx + cycle) & (DRIVE_LOOKAHEAD - 1)] ? 0 : 0b10000000;

		via->irb = low | sync | 0b00010000;
	}
}

void via_pa_write(via_t *via, uint8_t cycle) {
}

void via_pb_write(via_t *via, uint8_t cycle) {
	if (via == &via1) {
		while (ts + cycle > time_us_32()) {}
		gpio_set_dir(GPIO_CLK, (via->orb & 0b00001000) > 0 && (via->ddrb & 0b00001000) > 0);
		gpio_set_dir(GPIO_DATA, (via->orb & 0b00000010) > 0 && (via->ddrb & 0b00000010) > 0);
		if ((via->orb & 0b00010000) && (via->ddrb & 0b00010000)) {
			gpio_set_dir(GPIO_DATA2, gpio_get(GPIO_ATN));
		} else {
			gpio_set_dir(GPIO_DATA2, !gpio_get(GPIO_ATN));
		}
	} else if (via == &via2) {
		if ((via->ddrb & 3) == 3) {
			int8_t step = via->orb & 3;
			if (step - track_step > 0) {
				drive_step_up(&drive);
			} else if (step - track_step < 0) {
				drive_step_down(&drive);
			}
			track_step = step;
		}

		drive.spinning = ((via->ddrb & via->orb) & 4) > 0;

		gpio_put(GPIO_LED, (via->orb & 0b00001000) > 0 && (via->ddrb & 0b00001000) > 0);
	}
}

uint8_t bus_read(uint16_t addr, uint8_t cycle) {
	if (addr < 2048) {
		return ram[addr];
	} else if (addr >= 0xc000) {
		return _binary__Users_max_Documents_c64_roms_dos1541_start[addr - 0xc000];
	} else if (addr >= 0x1800 && addr < 0x1810) {
		uint8_t v = 0;
		via_read(&via1, addr - 0x1800, &v, cycle);
		return v;
	} else if (addr >= 0x1c00 && addr < 0x1c10) {
		uint8_t v = 0;
		via_read(&via2, addr - 0x1c00, &v, cycle);
		return v;
	} else {
		return 0;
	}
}

void bus_write(uint16_t addr, uint8_t v, uint8_t cycle) {
	if (addr < 2048) {
		ram[addr] = v;
	} else if (addr >= 0x1800 && addr < 0x1810) {
		via_write(&via1, addr - 0x1800, v, cycle);
	} else if (addr >= 0x1c00 && addr < 0x1c10) {
		via_write(&via2, addr - 0x1c00, v, cycle);
	}
}

void atn_irq_callback(uint pin, uint32_t flags) {
	if (flags & GPIO_IRQ_EDGE_RISE) {
		if (!(via1.pcr & 1)) {
			via1.ifr |= 0b10000010;
		}
		if ((via1.orb & 0b00010000) && (via1.ddrb & 0b00010000)) {
			gpio_set_dir(GPIO_DATA2, 1);
		} else {
			gpio_set_dir(GPIO_DATA2, 0);
		}
	} else {
		if (via1.pcr & 1) {
			via1.ifr |= 0b10000010;
		}
		if ((via1.orb & 0b00010000) && (via1.ddrb & 0b00010000)) {
			gpio_set_dir(GPIO_DATA2, 0);
		} else {
			gpio_set_dir(GPIO_DATA2, 1);
		}
	}
}

int main() {
	set_sys_clock_khz(250000, true);

	gpio_init(GPIO_CLK);
	gpio_init(GPIO_DATA);
	gpio_init(GPIO_ATN);
	gpio_init(GPIO_DATA2);
	gpio_init(25);

	gpio_put(GPIO_CLK, 0);
	gpio_put(GPIO_DATA, 0);
	gpio_put(GPIO_LED, 0);
	gpio_put(GPIO_DATA2, 0);

	gpio_set_dir(GPIO_CLK, GPIO_IN);
	gpio_set_dir(GPIO_DATA, GPIO_IN);
	gpio_set_dir(GPIO_ATN, GPIO_IN);
	gpio_set_dir(GPIO_LED, GPIO_OUT);
	gpio_set_dir(GPIO_DATA2, GPIO_IN);

	gpio_set_pulls(GPIO_CLK, true, false);
	gpio_set_pulls(GPIO_DATA, true, false);
	gpio_set_pulls(GPIO_DATA2, true, false);
	gpio_set_pulls(GPIO_ATN, true, false);

	gpio_set_irq_enabled_with_callback(GPIO_ATN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &atn_irq_callback);

	track_step = 0;
	drive_init(&drive);
	drive.image = _binary__Users_max_Downloads_blackbird_1_2_Blackbird_1_2_d64_start;

	cpu_reset(&cpu);
	via_init(&via1);
	via_init(&via2);

	ts = time_us_32();
	uint16_t osc2 = 0xffff;
	while (!cpu.jammed) {
		if (ts + 100 < time_us_32()) {
			break;
		}

		uint8_t cycles = cpu_step(&cpu);

		via_cycle(&via1, cycles);
		via_cycle(&via2, cycles);
		cpu.irq = via_irq(&via1) | via_irq(&via2);

		if (drive_cycle(&drive, cycles, true) && (via2.pcr & 0b1110) == 0b1110) {
			cpu.flags |= 1 << FLAG_V_BIT;
		}

		ts += cycles;
		while (ts > time_us_32()) {}
	}

	gpio_put(GPIO_LED, 1);

	return 0;
}