#include <cstdlib>

#include <iostream>

#include <wchar.h>

#include <locale.h>

#include <cstring>

#include <chrono>

#include <thread>

using namespace std;

typedef uint16_t address_t;

typedef uint8_t command_t;

typedef uint8_t sleep_t;

const address_t MEMORY_SIZE = 1024;

const command_t CMD_GOTO = 0x70;

const command_t CMD_SLEEP = 0xFF;

const command_t CMD_END = 0xED;

/**

 * Reads the command on given position in memory and increments the index (position).

 */

command_t readCommand(command_t * memory, address_t &index) {

	// TODO: map higher memory to static hardcoded areas or peripherals

	command_t * value = reinterpret_cast<command_t*> (memory + index);

	index += sizeof (*value) / sizeof (command_t);

	return *value;

}

address_t readAddress(command_t * memory, address_t &index) {

	address_t * value = reinterpret_cast<address_t*> (memory + index);

	index += sizeof (*value) / sizeof (command_t);

	return *value;

}

sleep_t readSleep(command_t * memory, address_t &index) {

	// TODO: map higher memory to static hardcoded areas or peripherals

	sleep_t * value = reinterpret_cast<sleep_t*> (memory + index);

	index += sizeof (*value) / sizeof (command_t);

	return *value;

}

void writeCommand(command_t * memory, address_t &index, const command_t value) {

	// command_t * m = (command_t*) (memory + index);

	command_t * m = reinterpret_cast<command_t*> (memory + index);

	*m = value;

	index += sizeof (value) / sizeof (command_t);

}

void writeAddress(command_t * memory, address_t &index, const address_t value) {

	// command_t * m = (command_t*) (memory + index);

	address_t * m = reinterpret_cast<address_t*> (memory + index);

	*m = value;

	index += sizeof (value) / sizeof (command_t);

}

void writeSleep(command_t * memory, address_t &index, const sleep_t value) {

	// command_t * m = (command_t*) (memory + index);

	sleep_t * m = reinterpret_cast<sleep_t*> (memory + index);

	*m = value;

	index += sizeof (value) / sizeof (command_t);

}

int main(int argc, char* argv[]) {

	setlocale(LC_ALL, "");

	command_t * memory = (command_t*) malloc(MEMORY_SIZE);

	{

		address_t a = 0;

		writeCommand(memory, a, CMD_SLEEP);

		writeSleep(memory, a, 255);

		writeCommand(memory, a, CMD_SLEEP);

		writeSleep(memory, a, 10);

		writeCommand(memory, a, CMD_SLEEP);

		writeSleep(memory, a, 255);

		writeCommand(memory, a, CMD_GOTO);

		writeAddress(memory, a, a+4);

		writeCommand(memory, a, 1);

		writeCommand(memory, a, 1);

		writeCommand(memory, a, CMD_SLEEP);

		writeSleep(memory, a, 255);

		writeCommand(memory, a, CMD_END);

	}

	for (address_t i = 0; i < MEMORY_SIZE;) {

		wprintf(L"command %d = ", i);

		command_t ch = readCommand(memory, i);

		wprintf(L"%X\n", ch);

		string command;

		switch (ch) {

			case CMD_GOTO:

				i = readAddress(memory, i);

				command.append("GOTO ");

				break;

			case CMD_SLEEP:

				command.append("SLEEP");

				this_thread::sleep_for(chrono::milliseconds(readSleep(memory, i)));

				break;

			case CMD_END:

				command.append("END");

				i = MEMORY_SIZE;

				break;

		}

		if (!command.empty()) {

			wprintf(L"\t%s\n", command.c_str());

		}

	}

	wprintf(L"all done\n");

	return 0;

}