#include <endian.h>
#include <fcntl.h>
#include <limits.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>

#include <sys/select.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>

enum Register
{
	REGISTER_R0 = 0,
	REGISTER_R1,
	REGISTER_R2,
	REGISTER_R3,
	REGISTER_R4,
	REGISTER_R5,
	REGISTER_R6,
	REGISTER_R7,
	REGISTER_PC,
	REGISTER_PSR,
	REGISTER_SSP,
	REGISTER_USP,
	REGISTER_COUNT,
};

enum Opcode
{
	OP_BR = 0,
	OP_ADD,
	OP_LD,
	OP_ST,
	OP_JSR,
	OP_AND,
	OP_LDR,
	OP_STR,
	OP_RTI,
	OP_NOT,
	OP_LDI,
	OP_STI,
	OP_JMP,
	OP_RES,
	OP_LEA,
	OP_TRAP,
};

enum Flag
{
	FLAG_POS = 1u << 0,
	FLAG_ZRO = 1u << 1,
	FLAG_NEG = 1u << 2,
	FLAG_SUP = 1u << 15,
};

enum Trap
{
	TRAP_GETC = 0x20,
	TRAP_OUT = 0x21,
	TRAP_PUTS = 0x22,
	TRAP_IN = 0x23,
	TRAP_PUTSP = 0x24,
	TRAP_HALT = 0x25,
};

enum Memory
{
	MEMORY_TRAP = 0x0000,
	MEMORY_INT = 0x00ff,
	MEMORY_SUP = 0x0200,
	MEMORY_USER = 0x3000,
	MEMORY_KBSR = 0xfe00,
	MEMORY_KBDR = 0xfe02,
	MEMORY_DSR = 0xfe04,
	MEMORY_DDR = 0xfe06,
};

uint16_t memory[UINT16_MAX];
uint16_t registers[REGISTER_COUNT];

struct termios tio;

void die(int result, const char * str)
{
	if (-1 == result)
	{
		perror(str);
		exit(EXIT_FAILURE);
	}
}

void prepare_terminal()
{
	int r = tcgetattr(0, &tio);
	die(r, "prepare_terminal tcgetattr()");
	struct termios disabled = tio;
	disabled.c_lflag &= ~ICANON & ~ECHO;
	r = tcsetattr(0, TCSANOW, &disabled);
	die(r, "prepare_terminal tcsetattr()");
}

void restore_terminal()
{
	int r = tcsetattr(0, TCSANOW, &tio);
	die(r, "restore_terminal tcsetattr()");
}

void handle_signal(int signal)
{
	(void) signal;
	restore_terminal();
	exit(EXIT_FAILURE);
}

uint16_t sign_extend(uint16_t value, const int bits)
{
	const uint16_t mask = 1u << (bits - 1);
	value &= (1u << bits) - 1;
	return (value ^ mask) - mask;
}

uint16_t check_key()
{
	fd_set fds;
	struct timeval tv;
	FD_ZERO(&fds);
	FD_SET(0, &fds);
	tv.tv_sec = 0;
	tv.tv_usec = 0;
	const int r = select(1, &fds, NULL, NULL, &tv);
	die(r, "check_key select()");
	return 0 != r;
}

void update_keyboard()
{
	uint8_t c;
	ssize_t r = read(0, &c, 1);
	die(r, "read_memory read()");
	memory[MEMORY_KBDR] = (uint16_t) c;
}

uint16_t read_memory(const uint16_t address)
{
	return memory[address];
}

void fire(const uint16_t interrupt, const uint16_t priority)
{
	const uint16_t current_priority = (registers[REGISTER_PSR] >> 8) & 0x07;
	if (0x00 != interrupt && 0x01 != interrupt && current_priority > priority)
	{
		return;
	}
	if (0x80 == interrupt && 0 == (0x4000 & memory[MEMORY_KBSR]))
	{
		return;
	}
	if (FLAG_SUP & registers[REGISTER_PSR])
	{
		registers[REGISTER_PSR] &= ~FLAG_SUP;
		registers[REGISTER_USP] = registers[REGISTER_R6];
		registers[REGISTER_R6] = registers[REGISTER_SSP];
	}
	registers[REGISTER_R6]--;
	memory[registers[REGISTER_R6]] = registers[REGISTER_PSR];
	registers[REGISTER_R6]--;
	memory[registers[REGISTER_R6]] = registers[REGISTER_PC];
	registers[REGISTER_PC] = read_memory(0x0100 | (interrupt & 0x00ff));
	registers[REGISTER_PSR] &= 0xfeff;
	registers[REGISTER_PSR] |= priority << 8;
}

void write_memory(const uint16_t address, const uint16_t value)
{
	switch (address)
	{
		case MEMORY_KBSR:
		{
			memory[MEMORY_KBSR] &= 0x8000;
			memory[MEMORY_KBSR] |= value & 0x7fff;
			break;
		}
		case MEMORY_KBDR:
		case MEMORY_DSR:
		{
			break;
		}
		case MEMORY_DDR:
		{
			uint8_t c = (uint8_t) value;
			ssize_t r = write(1, &c, 1);
			die(r, "write_memory write()");
		} // fallthrough
		default:
		{
			memory[address] = value;
			break;
		}
	}
}

void update_cond(const uint16_t reg)
{
	registers[REGISTER_PSR] &= 0xfff8;
	if (registers[reg] == 0)
	{
		registers[REGISTER_PSR] |= FLAG_ZRO;
	}
	else if (registers[reg] >> 15)
	{
		registers[REGISTER_PSR] |= FLAG_NEG;
	}
	else
	{
		registers[REGISTER_PSR] |= FLAG_POS;
	}
}

void trap(const uint16_t instruction)
{
	switch (instruction & 0xff)
	{
		case TRAP_GETC:
		{
			uint8_t c;
			ssize_t r = read(0, &c, 1);
			die(r, "TRAP_GETC read()");
			registers[REGISTER_R0] = (uint16_t) c;
			break;
		}
		case TRAP_OUT:
		{
			uint8_t c = (uint8_t) registers[REGISTER_R0];
			ssize_t r = write(1, &c, 1);
			die(r, "TRAP_OUT write()");
			break;
		}
		case TRAP_PUTS:
		{
			uint16_t * w = memory + registers[REGISTER_R0];
			uint8_t c;
			while (*w)
			{
				c = (uint8_t) *w;
				ssize_t r = write(1, &c, 1);
				die(r, "TRAP_PUTS write()");
				++w;
			}
			break;
		}
		case TRAP_IN:
		{
			printf("> ");
			uint8_t c;
			ssize_t r = read(0, &c, 1);
			die(r, "TRAP_IN read()");
			r = write(1, &c, 1);
			die(r, "TRAP_IN write()");
			registers[REGISTER_R0] = (uint16_t) c;
			break;
		}
		case TRAP_PUTSP:
		{
			uint16_t * w = memory + registers[REGISTER_R0];
			uint8_t c;
			while (*w)
			{
				c = (uint8_t) (*w & 0xff);
				ssize_t r = write(1, &c, 1);
				die(r, "TRAP_PUTSP write()");
				c = (uint8_t) (*w >> 8);
				if (c)
				{
					r = write(1, &c, 1);
					die(r, "TRAP_PUTSP write()");
				}
			}
			break;
		}
		case TRAP_HALT:
		{
			ssize_t r = write(1, "Halt\n", 5);
			die(r, "TRAP_HALT write()");
			exit(EXIT_SUCCESS);
		}
		default:
			return;
	}
	registers[REGISTER_PC] = registers[REGISTER_R7];
}

void step(const uint16_t instruction)
{
	const uint16_t opcode = instruction >> 12;
	switch (opcode)
	{
		case OP_BR:
		{
			const uint16_t offset = sign_extend(instruction & 0x1ff, 9);
			const uint16_t flag = (instruction >> 9) & 0x07;
			if (flag & registers[REGISTER_PSR])
			{
				registers[REGISTER_PC] += offset;
			}
			break;
		}
		case OP_ADD:
		{
			const uint16_t dest = (instruction >> 9) & 0x07;
			const uint16_t lhs = (instruction >> 6) & 0x07;
			const uint16_t immediate = (instruction >> 5) & 0x01;
			if (immediate)
			{
				const uint16_t rhs = sign_extend(instruction & 0x1f, 5);
				registers[dest] = registers[lhs] + rhs;
			}
			else
			{
				const uint16_t rhs = instruction & 0x07;
				registers[dest] = registers[lhs] + registers[rhs];
			}
			update_cond(dest);
			break;
		}
		case OP_LD:
		{
			const uint16_t dest = (instruction >> 9) & 0x07;
			const uint16_t offset = sign_extend(instruction & 0x1ff, 9);
			registers[dest] = read_memory(registers[REGISTER_PC] + offset);
			update_cond(dest);
			break;
		}
		case OP_ST:
		{
			const uint16_t src = (instruction >> 9) & 0x07;
			const uint16_t offset = sign_extend(instruction & 0x1ff, 9);
			const uint16_t address = registers[REGISTER_PC] + offset;
			write_memory(address, registers[src]);
			break;
		}
		case OP_JSR:
		{
			const uint16_t long_jump = (instruction >> 11) & 1;
			registers[REGISTER_R7] = registers[REGISTER_PC];
			if (long_jump)
			{
				const uint16_t offset = sign_extend(instruction & 0x7ff, 11);
				registers[REGISTER_PC] += offset;
			}
			else
			{
				const uint16_t src = (instruction >> 6) & 0x07;
				registers[REGISTER_PC] = registers[src];
			}
			break;
		}
		case OP_AND:
		{
			const uint16_t dest = (instruction >> 9) & 0x07;
			const uint16_t lhs = (instruction >> 6) & 0x07;
			const uint16_t immediate = (instruction >> 5) & 0x01;
			if (immediate)
			{
				const uint16_t rhs = sign_extend(instruction & 0x1f, 5);
				registers[dest] = registers[lhs] & rhs;
			}
			else
			{
				const uint16_t rhs = instruction & 0x07;
				registers[dest] = registers[lhs] & registers[rhs];
			}
			update_cond(dest);
			break;
		}
		case OP_LDR:
		{
			const uint16_t dest = (instruction >> 9) & 0x07;
			const uint16_t src = (instruction >> 6) & 0x07;
			const uint16_t offset = sign_extend(instruction & 0x3f, 6);
			const uint16_t address = registers[src] + offset;
			registers[dest] = read_memory(address);
			update_cond(dest);
			break;
		}
		case OP_STR:
		{
			const uint16_t src = (instruction >> 9) & 0x07;
			const uint16_t dest = (instruction >> 6) & 0x07;
			const uint16_t offset = sign_extend(instruction & 0x3f, 6);
			const uint16_t address = registers[dest] + offset;
			write_memory(address, registers[src]);
			break;
		}
		case OP_RTI:
		{
			if (FLAG_SUP & registers[REGISTER_PSR])
			{
				fire(0x00, (registers[REGISTER_PSR] >> 8) & 0x07);
			}
			else
			{
				registers[REGISTER_PC] = memory[registers[REGISTER_R6]];
				registers[REGISTER_R6]++;
				registers[REGISTER_PSR] = memory[registers[REGISTER_R6]];
				registers[REGISTER_R6]++;
			}
			break;
		}
		case OP_NOT:
		{
			const uint16_t dest = (instruction >> 9) & 0x07;
			const uint16_t src = (instruction >> 6) & 0x07;
			registers[dest] = ~registers[src];
			update_cond(dest);
			break;
		}
		case OP_LDI:
		{
			const uint16_t dest = (instruction >> 9) & 0x07;
			const uint16_t offset = sign_extend(instruction & 0x1ff, 9);
			const uint16_t address = read_memory(registers[REGISTER_PC] + offset);
			registers[dest] = read_memory(address);
			update_cond(dest);
			break;
		}
		case OP_STI:
		{
			const uint16_t src = (instruction >> 9) & 0x07;
			const uint16_t offset = sign_extend(instruction & 0x1ff, 9);
			const uint16_t address = read_memory(registers[REGISTER_PC] + offset);
			write_memory(address, registers[src]);
			break;
		}
		case OP_JMP:
		{
			const uint16_t src = (instruction >> 6) & 0x07;
			registers[REGISTER_PC] = registers[src];
			break;
		}
		case OP_LEA:
		{
			const uint16_t dest = (instruction >> 9) & 0x07;
			const uint16_t offset = sign_extend(instruction & 0x1ff, 9);
			registers[dest] = registers[REGISTER_PC] + offset;
			update_cond(dest);
			break;
		}
		case OP_TRAP:
		{
			registers[REGISTER_R7] = registers[REGISTER_PC];
			registers[REGISTER_PC] = read_memory(0 | (instruction & 0xff));
			trap(instruction);
			break;
		}
		case OP_RES:
		{
			fire(0x01, (registers[REGISTER_PSR] >> 8) & 0x07);
			break;
		}
		default:
			exit(EXIT_FAILURE);
	}
}

void read_image(const char * path)
{
	const int fd = open(path, O_RDONLY);
	die(fd, "read_image open()");
	uint16_t origin;
	ssize_t length = read(fd, &origin, sizeof(origin));
	die(length, "read_image read origin");
	origin = be16toh(origin);
	const size_t maximum = UINT16_MAX - origin;
	uint16_t * dest = memory + origin;
	length = read(fd, dest, maximum * sizeof(origin));
	die(length, "read_image read dest");
	for (; length > 0; length -= 2)
	{
		*dest = be16toh(*dest);
		dest++;
	}
	die(close(fd), "read_image close()");
}

int main(int argc, char ** argv)
{
	if (argc < 2)
	{
		exit(EXIT_FAILURE);
	}
	for (int i = 1; i < argc; ++i)
	{
		read_image(argv[i]);
	}
	signal(SIGINT, handle_signal);
	atexit(restore_terminal);
	prepare_terminal();
	memory[MEMORY_DSR] = 0x8000;
	registers[REGISTER_PC] = MEMORY_USER;
	registers[REGISTER_PSR] |= FLAG_SUP;
	registers[REGISTER_SSP] = MEMORY_USER;
	for (;;)
	{
		const uint16_t instruction = read_memory(registers[REGISTER_PC]++);
		step(instruction);
		if (check_key())
		{
			update_keyboard();
			fire(0x80, 4);
			memory[MEMORY_KBSR] |= 0x8000;
		}
		else
		{
			memory[MEMORY_KBSR] &= 0x7fff;
		}
	}
}