serial parsing working on client

3 files changed, 102 insertions, 1 deletions

diff --git a/shared/errcatch.c b/shared/errcatch.c
new file mode 100644
index 0000000..ff7fc93
--- /dev/null
+++ b/shared/errcatch.c
@@ -0,0 +1,46 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include "errcatch.h"
+
+w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {};
+uint8_t g_w2_error_index							= 0;
+uint8_t g_w2_error_offset							= 0;
+bool g_w2_error_buffer_full						= 0;
+bool g_w2_error_uncaught							= 0;
+
+void w2_errcatch_main() {
+	while (g_w2_error_offset != g_w2_error_index) {
+		w2_s_error *error = g_w2_error_buffer[g_w2_error_offset];
+		w2_errcatch_handle_error(error);
+		g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE;
+	}
+	if (g_w2_error_buffer_full) {
+		w2_errcatch_throw(W2_E_WARN_ERR_BUFFER_FULL);
+		g_w2_error_buffer_full = 0;
+	}
+	if (g_w2_error_uncaught) {
+		w2_errcatch_throw(W2_E_WARN_UNCAUGHT_ERROR);
+		g_w2_error_uncaught = 0;
+	}
+}
+
+w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message) {
+	w2_s_error *error = malloc(sizeof(w2_s_error) + length);
+	memcpy(error, &(w2_s_error const){.code = code, .message_length = length}, sizeof(w2_s_error));
+	strncpy(error->message, message, length);
+
+	return error;
+}
+
+void w2_errcatch_throw(w2_e_errorcode code) { w2_errcatch_throw_msg(code, 0, ""); }
+void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message) {
+	uint8_t next_index	   = (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE;
+	g_w2_error_buffer_full = next_index == g_w2_error_offset;
+	free(g_w2_error_buffer[g_w2_error_index]);
+	w2_s_error *error					= w2_alloc_error(code, length, message);
+	g_w2_error_buffer[g_w2_error_index] = error;
+	if (g_w2_error_buffer_full) return;
+	g_w2_error_index = next_index;
+}
+
diff --git a/shared/errors.h b/shared/errcatch.h
index 344a506..9b14cc2 100644
--- a/shared/errors.h
+++ b/shared/errcatch.h
@@ -1,9 +1,12 @@
 #pragma once
 
-/** @file errors.h */
+/** @file errcatch.h */
 
 #include <stdint.h>
 
+#include "consts.h"
+#include "bool.h"
+
 #define W2_E_TYPE_MASK (0b11 << 6)
 
 #define W2_E_TYPE_CRIT (0b00 << 6)
@@ -62,3 +65,34 @@ typedef struct {
 	uint8_t message_length;
 	char message[];
 } w2_s_error;
+
+/** error ring buffer */
+extern w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE];
+/** stores head of ring buffer */
+extern uint8_t g_w2_error_index;
+/** stores start of ring buffer */
+extern uint8_t g_w2_error_offset;
+/** error buffer full flag */
+extern bool g_w2_error_buffer_full;
+/** uncaught error flag */
+extern bool g_w2_error_uncaught;
+
+/** error-handler module main */
+void w2_errcatch_main();
+
+/** handle error */
+void w2_errcatch_handle_error(w2_s_error *error);
+
+/** append error to error buffer */
+void w2_errcatch_throw(w2_e_errorcode code);
+
+/** append error to error buffer (with debug message) */
+void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message);
+
+/**
+ * allocate and initialize error struct
+ *
+ * TODO: doesn't handle null pointers from malloc
+ */
+w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message);
+
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
index b1b4f50..49f4cbf 100644
--- a/shared/serial_parse.c
+++ b/shared/serial_parse.c
@@ -2,6 +2,10 @@
 
 #include "consts.h"
 #include "serial_parse.h"
+#include "errcatch.h"
+#ifdef W2_SIM
+#include "../robot/orangutan_shim.h"
+#endif
 
 bool w2_serial_parse(uint8_t byte) {
 	static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0};
@@ -38,3 +42,20 @@ bool w2_serial_parse(uint8_t byte) {
 
 	return W2_SERIAL_READ_SUCCESS;
 }
+
+void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+	w2_s_bin *copy				= w2_bin_s_alloc(data_length, data);
+	void (*handler)(w2_s_bin *) = g_w2_cmd_handlers[data[0]];
+
+	if (handler == NULL) {
+		w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY);
+	} else {
+#ifdef W2_SIM
+		w2_sim_print_serial(copy);
+#endif
+		handler(copy);
+	}
+
+	free(copy);
+}
+