clean up ugly switch case in favor of array lookup

5 files changed, 43 insertions, 91 deletions

diff --git a/robot/setup.c b/robot/setup.c
index 944ea68..f8468bf 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -25,6 +25,7 @@ void w2_setup_main() {
 	clear();
 
 	// start serial i/o
+	w2_cmd_setup_handlers();
 	serial_set_baud_rate(W2_SERIAL_BAUD);
 	serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE);
 
diff --git a/robot/tests/undefined.bin b/robot/tests/undefined.bin
new file mode 100644
index 0000000..d44f518
--- /dev/null
+++ b/robot/tests/undefined.bin
@@ -0,0 +1 @@
+ÿ€9 4­
\ No newline at end of file
diff --git a/shared/protocol.c b/shared/protocol.c
index 73063f3..77ec466 100644
--- a/shared/protocol.c
+++ b/shared/protocol.c
@@ -5,6 +5,30 @@
 #include "../robot/orangutan_shim.h"
 #endif
 
+void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *) = {0};
+void w2_cmd_setup_handlers() {
+	g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_RX] = w2_cmd_ping_rx;
+	g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_TX] = w2_cmd_ping_tx;
+	g_w2_cmd_handlers[W2_CMD_EXPT | W2_CMDDIR_TX] = w2_cmd_expt_tx;
+	g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_RX] = w2_cmd_mode_rx;
+	g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_TX] = w2_cmd_mode_tx;
+	g_w2_cmd_handlers[W2_CMD_SPED | W2_CMDDIR_RX] = w2_cmd_sped_rx;
+	g_w2_cmd_handlers[W2_CMD_DIRC | W2_CMDDIR_RX] = w2_cmd_dirc_rx;
+	g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_RX] = w2_cmd_cord_rx;
+	g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_TX] = w2_cmd_cord_tx;
+	g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_RX] = w2_cmd_bomd_rx;
+	g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_TX] = w2_cmd_bomd_tx;
+	g_w2_cmd_handlers[W2_CMD_SRES | W2_CMDDIR_RX] = w2_cmd_sres_rx;
+	g_w2_cmd_handlers[W2_CMD_MCFG | W2_CMDDIR_RX] = w2_cmd_mcfg_rx;
+	g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_RX] = w2_cmd_sens_rx;
+	g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_TX] = w2_cmd_sens_tx;
+	g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_RX] = w2_cmd_info_rx;
+	g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_TX] = w2_cmd_info_tx;
+	g_w2_cmd_handlers[W2_CMD_DISP | W2_CMDDIR_RX] = w2_cmd_disp_rx;
+	g_w2_cmd_handlers[W2_CMD_PLAY | W2_CMDDIR_RX] = w2_cmd_play_rx;
+	g_w2_cmd_handlers[W2_CMD_CLED | W2_CMDDIR_RX] = w2_cmd_cled_rx;
+}
+
 size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
 	if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx);
 	if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx);
@@ -54,101 +78,20 @@ size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) {
 }
 
 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);
-	bool unknown   = false;
-
-	switch (data[0]) {
-		case W2_CMD_PING | W2_CMDDIR_RX: {
-			w2_cmd_ping_rx(copy);
-			break;
-		}
-		case W2_CMD_PING | W2_CMDDIR_TX: {
-			w2_cmd_ping_tx(copy);
-			break;
-		}
-		case W2_CMD_EXPT | W2_CMDDIR_TX: {
-			w2_cmd_expt_tx(copy);
-			break;
-		}
-		case W2_CMD_MODE | W2_CMDDIR_RX: {
-			w2_cmd_mode_rx(copy);
-			break;
-		}
-		case W2_CMD_MODE | W2_CMDDIR_TX: {
-			w2_cmd_mode_tx(copy);
-			break;
-		}
-		case W2_CMD_SPED | W2_CMDDIR_RX: {
-			w2_cmd_sped_rx(copy);
-			break;
-		}
-		case W2_CMD_DIRC | W2_CMDDIR_RX: {
-			w2_cmd_dirc_rx(copy);
-			break;
-		}
-		case W2_CMD_CORD | W2_CMDDIR_RX: {
-			w2_cmd_cord_rx(copy);
-			break;
-		}
-		case W2_CMD_CORD | W2_CMDDIR_TX: {
-			w2_cmd_cord_tx(copy);
-			break;
-		}
-		case W2_CMD_BOMD | W2_CMDDIR_RX: {
-			w2_cmd_bomd_rx(copy);
-			break;
-		}
-		case W2_CMD_BOMD | W2_CMDDIR_TX: {
-			w2_cmd_bomd_tx(copy);
-			break;
-		}
-		case W2_CMD_SRES | W2_CMDDIR_RX: {
-			w2_cmd_sres_rx(copy);
-			break;
-		}
-		case W2_CMD_MCFG | W2_CMDDIR_RX: {
-			w2_cmd_mcfg_rx(copy);
-			break;
-		}
-		case W2_CMD_SENS | W2_CMDDIR_RX: {
-			w2_cmd_sens_rx(copy);
-			break;
-		}
-		case W2_CMD_SENS | W2_CMDDIR_TX: {
-			w2_cmd_sens_tx(copy);
-			break;
-		}
-		case W2_CMD_INFO | W2_CMDDIR_RX: {
-			w2_cmd_info_rx(copy);
-			break;
-		}
-		case W2_CMD_INFO | W2_CMDDIR_TX: {
-			w2_cmd_info_tx(copy);
-			break;
-		}
-		case W2_CMD_DISP | W2_CMDDIR_RX: {
-			w2_cmd_disp_rx(copy);
-			break;
-		}
-		case W2_CMD_PLAY | W2_CMDDIR_RX: {
-			w2_cmd_play_rx(copy);
-			break;
-		}
-		case W2_CMD_CLED | W2_CMDDIR_RX: {
-			w2_cmd_cled_rx(copy);
-			break;
-		}
-		default: {
+	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) {
 #ifdef W2_SIM
-			simwarn("unknown serial message with code 0x%02x\n", data[0]);
+		// TODO throw warning
+		simwarn("unknown serial message with code 0x%02x\n", data[0]);
 #endif
-			unknown = true;
-		}
-	}
-
+	} else {
 #ifdef W2_SIM
-	if (!unknown) w2_sim_print_serial(copy);
+		w2_sim_print_serial(copy);
 #endif
+		handler(copy);
+	}
 
 	free(copy);
 }
diff --git a/shared/protocol.h b/shared/protocol.h
index adec614..5bdfcbe 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -15,6 +15,7 @@
 #define W2_CMD_CODE_MASK (~1)
 #define W2_CMD_DIRECTION_MASK (1)
 
+#define W2_CMD_COUNT 28
 enum w2_e_scmds {
 	/** ping command */
 	W2_CMD_PING = 0x00,
@@ -162,6 +163,11 @@ typedef struct {
 
 #pragma pack(pop)
 
+/** stores message handlers in array with opcode as index */
+extern void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *);
+/** fills g_w2_cmd_handlers with functions */
+void w2_cmd_setup_handlers();
+
 /** global handler for complete messages */
 void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length);
 /** calculate message length */
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
index 3bf9c14..d755dc8 100644
--- a/shared/serial_parse.c
+++ b/shared/serial_parse.c
@@ -3,6 +3,7 @@
 #include "consts.h"
 #include "serial_parse.h"
 
+// TODO: give this function last time of byte, and measure if >5ms, throw warning
 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;