13 files changed, 182 insertions, 12 deletions

diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 4d46a12..0f754a3 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -5,6 +5,8 @@
 #include "orangutan_shim.h"
 #include "sercomm.h"
 
+uint64_t g_w2_hypervisor_cycles = 0;
+
 void w2_hypervisor_main() {
 #ifdef W2_SIM
 	w2_sim_cycle_begin();
@@ -26,6 +28,10 @@ void w2_hypervisor_main() {
 
 #ifdef W2_SIM
 	if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n");
-	usleep(100e3);
+	if (!g_w2_sim_headless) usleep(100e3);
+
+	if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) exit(0);
 #endif
+
+	g_w2_hypervisor_cycles++;
 }
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index 0e73259..35fa64c 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -1,5 +1,9 @@
 #pragma once
 
+#include <stdint.h>
+
+extern uint64_t g_w2_hypervisor_cycles;
+
 /**
  * backbone of all other modules
  *
diff --git a/robot/readme.md b/robot/readme.md
index 4a7aca3..2de14db 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -110,6 +110,8 @@ global todo:
   module (for last ping time measurement)
 - [ ] calibrate (line-detecting) light sensors in setup.c, or manually by
   placing the robot and pressing a button (maybe make this a seperate mode)
+- [ ] create labeled timer functions like nodejs `console.time()` and
+  `console.timeEnd()` (use for serial read timeout constraint)
 
 ### hypervisor
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 7072f9e..f4b7eb5 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -2,6 +2,7 @@
 #include <string.h>
 
 #include "../shared/bin.h"
+#include "../shared/serial_parse.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
 
@@ -27,11 +28,9 @@ void w2_sercomm_main() {
 		g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
 	}
 
+	// read and parse data
 	while (serial_get_received_bytes() != g_w2_serial_buffer_index) {
-		uint8_t byte = g_w2_serial_buffer[g_w2_serial_buffer_index];
-#ifdef W2_SIM
-		simprintf("serial byte: %02x\n", byte);
-#endif
+		w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]);
 		g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE;
 	}
 }
@@ -43,10 +42,7 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 	uint8_t next_index		 = (g_w2_sercomm_index + 1) % W2_SERCOMM_BUFFER_SIZE;
 	g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset;
 	free(g_w2_sercomm_buffer[g_w2_sercomm_index]);
-	w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes);
-	memcpy(&data_copy->data, data->data, data->bytes);
-	data_copy->bytes						= data->bytes;
-	g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy;
+	g_w2_sercomm_buffer[g_w2_sercomm_index] = w2_bin_s_alloc(data->bytes, data->data);
 	if (g_w2_sercomm_buffer_full) return;
 	g_w2_sercomm_index = next_index;
 }
diff --git a/robot/sim.c b/robot/sim.c
index b061c9a..8af672e 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -52,6 +52,12 @@ void serial_set_baud_rate(unsigned int rate) {
 }
 
 void serial_send(char* message, unsigned int length) {
+	if (g_w2_sim_headless) {
+		for (unsigned int byte = 0; byte < length; byte++)
+			putc(message[byte] & 0xff, stdout);
+		return;
+	}
+	if (!DBG_ENABLE_PRINTFUNC) return;
 	simprintfunc("serial_send", "<see below>, %u", length);
 	unsigned int bytes = 0;
 	simprintf("");
diff --git a/robot/sim.h b/robot/sim.h
index 501552e..336592b 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -8,10 +8,11 @@
 extern bool g_w2_sim_headless;
 
 // debug fine-tuning
-#define DBG_ENABLE_PRINTFUNC (0)
+#define DBG_ENABLE_PRINTFUNC (1)
 #define DBG_ENABLE_SIMWARN (1)
 #define DBG_ENABLE_SIMINFO (1)
 #define DBG_ENABLE_CYCLEINFO (0)
+#define DBG_MAX_CYCLES (10)
 
 // debug print options
 #define DBG_BYTES_PER_LINE 16
diff --git a/robot/tests/padded_info.bin b/robot/tests/padded_info.bin
new file mode 100644
index 0000000..2ff1d8f
--- /dev/null
+++ b/robot/tests/padded_info.bin
@@ -0,0 +1 @@
+=ãÿ=íÿÿI89ÿ
\ No newline at end of file
diff --git a/shared/bin.c b/shared/bin.c
index a2c91a4..fc6e14b 100644
--- a/shared/bin.c
+++ b/shared/bin.c
@@ -1,4 +1,5 @@
 #include <stdlib.h>
+#include <memory.h>
 
 #include "bin.h"
 
@@ -62,3 +63,10 @@ uint16_t w2_bin_hton16(uint16_t h16) {
 
 uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); }
 uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); }
+
+w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) {
+	w2_s_bin* temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes);
+	temp->bytes = bytes;
+	memcpy(&temp->data, data, bytes);
+	return temp;
+}
diff --git a/shared/bin.h b/shared/bin.h
index 1c9b951..d9eacd7 100644
--- a/shared/bin.h
+++ b/shared/bin.h
@@ -27,6 +27,9 @@ typedef struct {
 	uint8_t data[];
 } w2_s_bin;
 
+/** allocate new w2_s_bin struct and fill with `*data` for `bytes` bytes */
+w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data);
+
 typedef struct {
 	enum w2_e_struct_types type;
 	uint16_t length;
diff --git a/shared/protocol.c b/shared/protocol.c
new file mode 100644
index 0000000..2707725
--- /dev/null
+++ b/shared/protocol.c
@@ -0,0 +1,10 @@
+#include "protocol.h"
+
+W2_PROTOCOL_DEFINE(W2_PROTOCOL_CMD_PING_RX)
+
+w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) {
+	w2_s_cmd_ping_rx *parsed = malloc(data->bytes);
+	memcpy(parsed, data->data, data->bytes);
+	return parsed;
+}
+
diff --git a/shared/protocol.h b/shared/protocol.h
index b400252..b0602cf 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -1,7 +1,18 @@
 #pragma once
 
-#define W2_CMDDIR_RX (0)
-#define W2_CMDDIR_TX (1)
+#include <stdint.h>
+#include <stdlib.h>
+#include <memory.h>
+
+#include "bin.h"
+
+#define W2_SERIAL_START_BYTE 0xff
+
+#define W2_CMDDIR_RX 0
+#define W2_CMDDIR_TX 1
+
+#define W2_CMD_CODE_MASK (~1)
+#define W2_CMD_DIRECTION_MASK (1)
 
 enum w2_e_serial_commands {
 	/** ping command */
@@ -33,3 +44,62 @@ enum w2_e_serial_commands {
 	/** control leds command */
 	W2_CMD_CLED = 0x1a,
 };
+
+// TODO
+// array met indicies die structs opslaan met eigenschappen over de protocol bericht
+//
+// belangrijke eigenschappen:
+// lengte!!!
+// generic struct (voor parsen)
+// parse functie
+// dump functie
+// 
+//
+// als het kan deze allemaal met macro's op deze manier definieren:
+//
+#define W2_PROTOCOL_CMD(name, direction, ...)
+#define W2_PROTOCOL_DEFINE(a)
+#define W2_PROTOCOL_DECLARE(a)
+#define W2_PROTOCOL_PROP(type, name)
+
+#define W2_CMDDIR_NAME_0 rx
+#define W2_CMDDIR_NAME_1 tx
+
+#define W2_CMDDIR(dir) W2_CMDDIR_NAME_##dir
+
+#define W2_PROTOCOL_UINT8_T
+#define W2_PROTOCOL_UINT16_T
+#define W2_PROTOCOL_UINT32_T
+#define W2_PROTOCOL_INT8_T
+#define W2_PROTOCOL_INT16_T
+#define W2_PROTOCOL_INT32_T
+
+#define W2_PROTOCOL_UINT8_T_TYPE          uint8_t
+#define W2_PROTOCOL_UINT16_T_TYPE         uint16_t
+#define W2_PROTOCOL_UINT32_T_TYPE         uint32_t
+#define W2_PROTOCOL_INT8_T_TYPE           int8_t
+#define W2_PROTOCOL_INT16_T_TYPE          int16_t
+#define W2_PROTOCOL_INT32_T_TYPE          int32_t
+
+#define W2_PROTOCOL_UINT8_T_SIZE  1
+#define W2_PROTOCOL_UINT16_T_SIZE 2
+#define W2_PROTOCOL_UINT32_T_SIZE 4
+#define W2_PROTOCOL_INT8_T_SIZE   1
+#define W2_PROTOCOL_INT16_T_SIZE  2
+#define W2_PROTOCOL_INT32_T_SIZE  4
+
+#define W2_PROTOCOL_CMD_PING_RX \
+W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \
+	W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \
+	W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id) \
+)
+
+W2_PROTOCOL_DECLARE(W2_PROTOCOL_CMD_PING_RX)
+
+typedef struct {
+	uint8_t opcode;
+	uint8_t id;
+} w2_s_cmd_ping_rx;
+
+w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data);
+
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
new file mode 100644
index 0000000..c8a3fae
--- /dev/null
+++ b/shared/serial_parse.c
@@ -0,0 +1,54 @@
+#include <stdbool.h>
+
+#include "consts.h"
+#include "serial_parse.h"
+#include "bin.h"
+#ifdef W2_SIM
+#include "../robot/orangutan_shim.h"
+#endif
+
+void w2_serial_handle(w2_s_bin *code) {
+#ifdef W2_SIM
+	serial_send((char*)code->data, code->bytes);
+	// simprintf("yeah: %02x\n", code);
+#endif
+}
+
+void w2_serial_parse(uint8_t byte) {
+	static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0};
+	static uint8_t current_message_index = 0;
+	static uint8_t complete_message_length = 2;
+
+	static bool attentive = false;
+	static bool listening = false;
+	
+	if (byte == W2_SERIAL_START_BYTE) {
+		attentive = !attentive;
+		// if (attentive && listening) {
+		// 	current_message[current_message_index++] = byte;
+		// }
+	} else {
+		// activate listen after non-0xff byte after 0xff
+		if (attentive && !listening) {
+			attentive = false;
+			listening = true;
+		}
+	}
+
+	if (!listening) return;
+	current_message[current_message_index++] = byte;
+
+	if (current_message_index == complete_message_length) {
+		w2_s_bin *copy = w2_bin_s_alloc(current_message_index, current_message);
+		w2_serial_handle(copy);
+		free(copy);
+
+		memset(&current_message, 0, W2_SERIAL_READ_BUFFER_SIZE);
+		current_message_index = 0;
+		complete_message_length = 1;
+		attentive = false;
+		listening = false;
+		return;
+	}
+}
+
diff --git a/shared/serial_parse.h b/shared/serial_parse.h
new file mode 100644
index 0000000..516bf8a
--- /dev/null
+++ b/shared/serial_parse.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "protocol.h"
+
+/** parse serial data byte by byte */
+void w2_serial_parse(uint8_t byte);
+