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

 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;

 /**

  * 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;

 /**

  * Placeholder for unsupported command.

  * Just for testing.

  */

 const command_t CMD_INVALID = 0x1;

 /**

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

  */

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

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

 	// TODO: sizeof (command_t) != 1 ?

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

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

 	return *value;

 }

 /**

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

  */

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

 	// TODO: sizeof (command_t) != 1 ?

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

 	T * m = reinterpret_cast<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;

 		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 + 4);

 		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_END);

 	}

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

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

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

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

 		switch (ch) {

 			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_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;

 }