#include <UTFT.h>
#include <memory.h>
#include <utftdisplay.h>
#include <sdtape.h>
#include <keyboard.h>
#include <timed.h>

#include "io.h"
#include "config.h"
#include "hardware.h"

static byte dsp_cr, dsp, kbd_cr, kbd;
static unsigned kbd_int, dsp_out;

#define ROWS	24
#define COLS	40
static unsigned r, c;
static char screen[ROWS][COLS];

void io::reset() {
	UTFTDisplay::begin(TFT_BG, TFT_FG);
	clear();
	_cy += 2;

	r = c = 0;
	for (int j = 0; j < ROWS; j++)
		for (int i = 0; i < COLS; i++)
			screen[j][i] = ' ';

	_loading = false;

	// PIA state
	dsp_cr = kbd_cr = 0;
	kbd_int = dsp_out = 0;
}

void io::load() {
	if (tape.more()) {
		_loading = true;
		enter(tape.read());
	}
}

// ascii map for scan-codes
static const byte scanmap[] = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 	// 0x00
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x60, 0xff, 	// 0x08
	0xff, 0xff, 0xff, 0xff, 0xff, 0x51, 0x31, 0xff, 	// 0x10
	0xff, 0xff, 0x5a, 0x53, 0x41, 0x57, 0x32, 0xff, 	// 0x18
	0xff, 0x43, 0x58, 0x44, 0x45, 0x34, 0x33, 0xff, 	// 0x20
	0xff, 0x20, 0x56, 0x46, 0x54, 0x52, 0x35, 0xff, 	// 0x28
	0xff, 0x4e, 0x42, 0x48, 0x47, 0x59, 0x36, 0xff, 	// 0x30
	0xff, 0xff, 0x4d, 0x4a, 0x55, 0x37, 0x38, 0xff, 	// 0x38
	0xff, 0x2c, 0x4b, 0x49, 0x4f, 0x30, 0x39, 0xff, 	// 0x40
	0xff, 0x2e, 0x2f, 0x4c, 0x3b, 0x50, 0x2d, 0xff, 	// 0x48
	0xff, 0xff, 0x27, 0xff, 0x5b, 0x3d, 0xff, 0xff, 	// 0x50
	0xff, 0xff, 0x0d, 0x5d, 0xff, 0x23, 0xff, 0xff, 	// 0x58
	0xff, 0x5c, 0xff, 0xff, 0xff, 0xff, 0x5f, 0xff, 	// 0x60
	0xff, 0x31, 0xff, 0x34, 0x37, 0xff, 0xff, 0xff, 	// 0x68
	0x30, 0xff, 0x32, 0x35, 0x36, 0x38, 0xff, 0xff, 	// 0x70
	0xff, 0x2b, 0x33, 0x2d, 0x2a, 0x39, 0xff, 0xff, 	// 0x78
};

static const byte shiftmap[] = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,		// 0x00
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,		// 0x08
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x21, 0xff,		// 0x10
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x22, 0xff,		// 0x18
	0xff, 0xff, 0xff, 0xff, 0xff, 0x24, 0x23, 0xff,		// 0x20
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x25, 0xff,		// 0x28
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x5e, 0xff,		// 0x30
	0xff, 0xff, 0xff, 0xff, 0xff, 0x26, 0x2a, 0xff,		// 0x38
	0xff, 0x3c, 0xff, 0xff, 0xff, 0x29, 0x28, 0xff,		// 0x40
	0xff, 0x3e, 0x3f, 0xff, 0x3a, 0xff, 0x5f, 0xff,		// 0x48
	0xff, 0xff, 0x40, 0xff, 0x7b, 0x2b, 0xff, 0xff,		// 0x50
	0xff, 0xff, 0xff, 0x7d, 0xff, 0x7e, 0xff, 0xff,		// 0x58
	0xff, 0x7c, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,		// 0x60
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,		// 0x68
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,		// 0x70
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,		// 0x78
};

void io::down(byte scan) {
	kbd = 0;
	if (isshift(scan))
		_shift = true;
}

void io::enter(byte key) {
	kbd = key + 0x80;
	kbd_cr = 0xa7;
}

void io::up(byte scan) {
	if (isshift(scan)) {
		_shift = false;
		return;
	}
	enter(_shift? shiftmap[scan]: scanmap[scan]);
}

void io::draw(char ch, int i, int j) {
	if (screen[j][i] != ch) {
		screen[j][i] = ch;
		char c[2] = { ch, 0 };
		utft.print(c, i*_cx, j*_cy);
	}
}

void io::display(byte b) {
	char ch = (char)b;
	switch(ch) {
	case 0x5f:
		draw(' ', c, r);
		if (c-- == 0) {
			r--;
			c = COLS-1;
		}
		break;
	case 0x0d:
		draw(' ', c, r);
		c = 0;
		r++;
		break;
	default:
		if (ch >= 0x20 && ch < 0x7f) {
			draw(ch, c, r);
			if (++c == COLS) {
				c = 0;
				r++;
			}
		}
	}
	if (r == ROWS) {
		// scroll
		r--;
		for (int j = 0; j < (ROWS-1); j++)
			for (int i = 0; i < COLS; i++)
				draw(screen[j+1][i], i, j);
		for (int i = 0; i < COLS; i++)
			draw(' ', i, ROWS-1);
	}
	draw('_', c, r);
}

void io::operator=(byte b) {

Serial.print(">");
Serial.print(_acc, 16);
Serial.print(" ");
Serial.println(b, 16);

	switch(_acc % 4) {
	case 0:
		kbd = b;
		break;
	case 1:
		if (!kbd_int && b >= 0x80)
			kbd_int = 1;
		else
			kbd_cr = b;
		break;
	case 2:
		if (b >= 0x80)
			b -= 0x80;
		display(b);
		dsp = b;
		break;
	case 3:
		if (!dsp_out && dsp_cr >= 0x80)
			dsp_out = 1;
		else
			dsp_cr = b;
		break;
	}
}

io::operator byte() {

	switch (_acc % 4) {
	case 0:
/*
Serial.print("<");
Serial.print(_acc, 16);
Serial.print(" ");
Serial.println(kbd, 16);
*/
		return kbd;
	case 1:
		if (kbd_int && kbd_cr >= 0x80) {
			kbd_cr = 0;
/*
Serial.print("<");
Serial.print(_acc, 16);
Serial.print(" ");
Serial.println(0xa7, 16);
*/
			if (_loading) {
				if (tape.more())
					enter(tape.read());
				else
					_loading = false;
			}
			return 0xa7;
		}
//Serial.println(kbd_cr, 16);
		return kbd_cr;
	case 2:
/*
Serial.print("<");
Serial.print(_acc, 16);
Serial.print(" ");
Serial.println(dsp, 16);
*/
		return dsp;
	case 3:
/*
Serial.print("<");
Serial.print(_acc, 16);
Serial.print(" ");
Serial.println(dsp_cr, 16);
*/
		return dsp_cr;
	}
	return 0;
}

void io::checkpoint(Stream &s) {
	s.write(dsp_cr);
	s.write(dsp);
	s.write(kbd_cr);
	s.write(kbd);
	s.write(kbd_int);
	s.write(dsp_out);
	s.write(r);
	s.write(c);
	for (int j = 0; j < ROWS; j++)
		for (int i = 0; i < COLS; i++)
			s.write(screen[j][i]);
}

void io::restore(Stream &s) {
	dsp_cr = s.read();
	dsp = s.read();
	kbd_cr = s.read();
	kbd = s.read();
	kbd_int = s.read();
	dsp_out = s.read();
	r = s.read();
	c = s.read();
	for (int j = 0; j < ROWS; j++)
		for (int i = 0; i < COLS; i++) {
			char c = s.read();
			screen[j][i] = c;
			draw(c, i, j);
		}
	draw('_', c, r);
}