5 files changed, 39 insertions, 34 deletions

diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c
index 6b4e736..896d0f0 100644
--- a/robot/mode_lcal.c
+++ b/robot/mode_lcal.c
@@ -1 +1,3 @@
 #include "mode_lcal.h"
+
+void w2_mode_lcal() {}
diff --git a/robot/modes.c b/robot/modes.c
index 928e2b4..f73a809 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -4,13 +4,31 @@
 #include "../shared/util.h"
 #include "sercomm.h"
 
-/** function pointer to current mode */
-// static void (*g_w2_current_mode)() = &w2_mode_halt;
-
-static void (*g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE])();
-static uint8_t g_w2_mode_history_index = 0;
+#include "mode_chrg.h"
+#include "mode_dirc.h"
+#include "mode_grid.h"
+#include "mode_halt.h"
+#include "mode_lcal.h"
+#include "mode_maze.h"
+#include "mode_scal.h"
+#include "mode_spin.h"
+
+w2_e_mode g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE];
+uint8_t g_w2_mode_history_index = 0;
+void (*g_w2_modes[W2_MODE_COUNT])();
+
+void w2_modes_init() {
+	g_w2_modes[W2_M_CHRG] = &w2_mode_chrg;
+	g_w2_modes[W2_M_DIRC] = &w2_mode_dirc;
+	g_w2_modes[W2_M_GRID] = &w2_mode_grid;
+	g_w2_modes[W2_M_HALT] = &w2_mode_halt;
+	g_w2_modes[W2_M_LCAL] = &w2_mode_lcal;
+	g_w2_modes[W2_M_MAZE] = &w2_mode_maze;
+	g_w2_modes[W2_M_SCAL] = &w2_mode_scal;
+	g_w2_modes[W2_M_SPIN] = &w2_mode_spin;
+}
 
-void w2_modes_main() { (*g_w2_mode_history[g_w2_mode_history_index])(); }
+void w2_modes_main() { (*g_w2_modes[g_w2_mode_history[g_w2_mode_history_index]])(); }
 
 void w2_modes_switch(w2_e_mode new_mode, bool replace) {
 	int16_t next_history_index =
@@ -24,7 +42,7 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
 		g_w2_mode_history_index = next_history_index;
 	} else {
 		g_w2_mode_history_index					   = next_history_index;
-		g_w2_mode_history[g_w2_mode_history_index] = W2_MODES[new_mode];
+		g_w2_mode_history[g_w2_mode_history_index] = new_mode;
 	}
 
 	// forward mode change to sercomm
diff --git a/robot/modes.h b/robot/modes.h
index 122be4a..8a53560 100644
--- a/robot/modes.h
+++ b/robot/modes.h
@@ -2,16 +2,16 @@
 
 /** @file modes.h */
 
+#include <stdint.h>
+
 #include "../shared/consts.h"
+#include "../shared/modes.h"
+
+extern w2_e_mode g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE];
+extern uint8_t g_w2_mode_history_index;
 
-#include "mode_chrg.h"
-#include "mode_dirc.h"
-#include "mode_grid.h"
-#include "mode_halt.h"
-#include "mode_lcal.h"
-#include "mode_maze.h"
-#include "mode_scal.h"
-#include "mode_spin.h"
+/** setup g_w2_modes array */
+void w2_modes_init();
 
 /**
  * mode logic
@@ -20,25 +20,6 @@
  */
 void w2_modes_main();
 
-/** mode constants */
-typedef enum {
-	W2_M_PREV = -1,
-	W2_M_MAZE = 0,
-	W2_M_GRID = 1,
-	W2_M_HALT = 2,
-	W2_M_LCAL = 3,
-	W2_M_CHRG = 4,
-	W2_M_DIRC = 5,
-	W2_M_SPIN = 6,
-	W2_M_SCAL = 7,
-} w2_e_mode;
-
-/** array that maps w2_e_mode to mode function pointers */
-static const void(*const W2_MODES[]) = {
-	&w2_mode_maze, &w2_mode_grid, &w2_mode_grid, &w2_mode_halt,
-	&w2_mode_chrg, &w2_mode_dirc, &w2_mode_spin, &w2_mode_scal,
-};
-
 /** switch current mode (allow switching back to previous mode) */
 void w2_modes_call(w2_e_mode mode);
 /** switch current mode (replace current mode keeping history index) */
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 2317d89..deef39f 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -135,6 +135,7 @@ void w2_cmd_info_rx(w2_s_bin *data) {
 	res_msg->sercomm_ms	 = (uint8_t)g_w2_hypervisor_ema_sercomm_ms;
 	res_msg->mode_ms	 = (uint8_t)g_w2_hypervisor_ema_mode_ms;
 	res_msg->uptime_s	 = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3));
+	res_msg->mode		 = g_w2_mode_history[g_w2_mode_history_index];
 
 	w2_sercomm_append_msg(res_bin);
 	free(res_bin);
diff --git a/robot/setup.c b/robot/setup.c
index 95b201e..4706d64 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -19,6 +19,9 @@ void w2_setup_main() {
 	// clear lcd
 	clear();
 
+	// modes array
+	w2_modes_init();
+
 	// start serial i/o
 	w2_cmd_setup_handlers();
 	serial_set_baud_rate(W2_SERIAL_BAUD);