1 files changed, 54 insertions, 0 deletions
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;
+	}
+}
+