#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 octet_t;

typedef uint8_t command_t;

typedef uint8_t sleep_t;

typedef uint8_t color_t;

typedef uint8_t led_t;

// TODO: strong typedefs http://www.boost.org/doc/libs/1_61_0/libs/serialization/doc/strong_typedef.html ?

const address_t MEMORY_SIZE = 1024;

/**

 * Skip to the given address.

 * parameter: address_t

 */

const command_t CMD_GOTO = 0x70;

/**

 * Compare values on two addresses and go to one of given three addresses.

 * parameter: address_t a

 * parameter: address_t b

 * parameter: address_t GOTO target when a == b

 * parameter: address_t GOTO target when a > b

 * parameter: address_t GOTO target when a < b

 */

const command_t CMD_GOTO_COMPARE = 0x80;

/**

 * Wait given time in ms.

 * parameter: sleep_t

 */

const command_t CMD_SLEEP = 0xFF;

/**

 * Set RGB LED color.

 * parameter: led_t LED number

 * parameter: color_t red

 * parameter: color_t green

 * parameter: color_t blue

 */

const command_t CMD_COLOR = 0xAA;

/**

 * Stop program.

 */

const command_t CMD_END = 0xED;

/**

 * Increase value at given address

 * parameter: address_t

 */

const command_t CMD_INCREMENT = 0x11;

/**

 * Decrease value at given address

 * parameter: address_t

 */

const command_t CMD_DECREMENT = 0x12;

/**

 * Placeholder for unsupported command.

 * Just for testing.

 */

const command_t CMD_INVALID = 0x1;

// TODO: more commands, better numbers

template<typename T> T logMemoryError(const address_t &index) {

	wprintf(L"memory error: index = %d, sizeof(T) = %d, MEMORY_SIZE = %d\n", index, sizeof (T), MEMORY_SIZE);

	// TODO: return error value or throw exception

	return T();

}

/**

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

 * 

 * @param memory array of bytes / octets

 * @param index offset in same units as memory type

 * @return value found at given position

 */

template<typename T> T read(octet_t * memory, address_t &index) {

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

	if (index + sizeof (T) <= MEMORY_SIZE) {

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

		index += sizeof (T);

		return *value;

	} else {

		return logMemoryError<T>(index);

	}

}

/**

 * Writes data to given position in memory and increments the index (position).

 * @param memory array of bytes / octets

 * @param index offset in same units as memory type

 * @param value value to be written at given position

 */

template<typename T> T write(octet_t * memory, address_t &index, const T value) {

	if (index + sizeof (T) <= MEMORY_SIZE) {

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

		*m = value;

		index += sizeof (value);

	} else {

		return logMemoryError<T>(index);

	}

}

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

	setlocale(LC_ALL, "");

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

	// Sample program / data:

	// TODO: load bytes from file, stdin, serial port, network…

	{

		address_t a = 0;

		write<command_t>(memory, a, CMD_SLEEP);

		write<sleep_t>(memory, a, 255);

		write<command_t>(memory, a, CMD_SLEEP);

		write<sleep_t>(memory, a, 10);

		write<command_t>(memory, a, CMD_SLEEP);

		write<sleep_t>(memory, a, 255);

		write<command_t>(memory, a, CMD_GOTO);

		write<address_t>(memory, a, a + sizeof (address_t) + 2 * sizeof (command_t));

		write<command_t>(memory, a, CMD_INVALID);

		write<command_t>(memory, a, CMD_INVALID);

		write<command_t>(memory, a, CMD_SLEEP);

		write<sleep_t>(memory, a, 255);

		write<command_t>(memory, a, CMD_COLOR);

		write<led_t>(memory, a, 23);

		write<color_t>(memory, a, 0);

		write<color_t>(memory, a, 200);

		write<color_t>(memory, a, 255);

		write<command_t>(memory, a, CMD_INCREMENT);

		write<address_t>(memory, a, 0);

		write<command_t>(memory, a, CMD_DECREMENT);

		write<address_t>(memory, a, 0);

		write<command_t>(memory, a, CMD_GOTO_COMPARE);

		write<address_t>(memory, a, 0);

		write<address_t>(memory, a, 0 + sizeof (command_t) + sizeof (sleep_t));

		write<address_t>(memory, a, a - 3 * sizeof (address_t) - 2 * sizeof (command_t));

		write<address_t>(memory, a, 0);

		write<address_t>(memory, a, a + sizeof (address_t));

		write<command_t>(memory, a, CMD_END);

	}

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

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

		command_t command = read<command_t>(memory, i);

		wprintf(L"%02X  ", command);

		switch (command) {

			case CMD_GOTO:

			{

				i = read<address_t>(memory, i);

				wprintf(L"GOTO %*d\n", 5, i);

				break;

			}

			case CMD_SLEEP:

			{

				sleep_t delay = read<sleep_t>(memory, i);

				wprintf(L"SLEEP %*d ms\n", 4, delay);

				this_thread::sleep_for(chrono::milliseconds(delay));

				break;

			}

			case CMD_COLOR:

			{

				led_t led = read<led_t>(memory, i);

				color_t r = read<color_t>(memory, i);

				color_t g = read<color_t>(memory, i);

				color_t b = read<color_t>(memory, i);

				wprintf(L"COLOR  %02X %02X %02X → %d\n", r, g, b, led);

				break;

			}

			case CMD_INCREMENT:

			case CMD_DECREMENT:

			{

				address_t address = read<address_t>(memory, i);

				address_t address_r = address;

				address_t address_w = address_r;

				octet_t value = read<octet_t>(memory, address_r);

				value = command == CMD_INCREMENT ? value + 1 : value - 1;

				write<octet_t>(memory, address_w, value);

				wprintf(L"%sCREMENT %*d → %02X\n", (command == CMD_INCREMENT ? "IN" : "DE"), 5, address, value);

				break;

			}

			case CMD_GOTO_COMPARE:

			{

				address_t aa = read<address_t>(memory, i);

				address_t ab = read<address_t>(memory, i);

				address_t eq = read<address_t>(memory, i);

				address_t gt = read<address_t>(memory, i);

				address_t lt = read<address_t>(memory, i);

				octet_t a = read<octet_t>(memory, aa);

				octet_t b = read<octet_t>(memory, ab);

				if (a == b) i = eq;

				else if (a > b) i = gt;

				else i = lt;

				wprintf(L"GOTO COMPARE  a = %02X, b = %02X, eq = %d, gt = %d, lt = %d → %d\n", a, b, eq, gt, lt, i);

				break;

			}

			case CMD_END:

			{

				wprintf(L"END\n");

				i = MEMORY_SIZE;

				break;

			}

			default:

			{

				wprintf(L"invalid command\n");

			}

		}

	}

	free(memory);

	memory = nullptr;

	wprintf(L"all done\n");

	return 0;

}