From f09665ad2851c1012dc99cfadc4db46c246fdab9 Mon Sep 17 00:00:00 2001
From: "Jasper v. Blanckenburg" <j.blanckenburg@fasttube.de>
Date: Sat, 15 Apr 2023 22:23:18 +0200
Subject: [PATCH] Decouple slave IDs from slaves array indices

---
 Core/Src/slave_monitoring.c | 52 ++++++++++++++++++++++++++-----------
 1 file changed, 37 insertions(+), 15 deletions(-)

diff --git a/Core/Src/slave_monitoring.c b/Core/Src/slave_monitoring.c
index edfa2ce..659cf51 100644
--- a/Core/Src/slave_monitoring.c
+++ b/Core/Src/slave_monitoring.c
@@ -15,9 +15,29 @@ SlaveHandle slaves[N_SLAVES];
 uint16_t min_voltage;
 int16_t max_temp;
 
+static uint8_t slave_id_to_index[128] = {0xFF};
+
+static size_t get_slave_index(uint8_t slave_id) {
+  // Slave IDs are 7-bit, so we can use a 128-element array to map them to
+  // indices. 0xFF is used to mark unseen slave IDs, since the highest index we
+  // could need is N_SLAVES - 1 (i.e. 5).
+  static size_t next_slave_index = 0;
+  if (slave_id_to_index[slave_id] == 0xFF) {
+    if (next_slave_index >= N_SLAVES) {
+      // We've seen more than N_SLAVES slave IDs, this shouldn't happen.
+      Error_Handler();
+    }
+    slave_id_to_index[slave_id] = next_slave_index;
+    slaves[next_slave_index].id = slave_id;
+    next_slave_index++;
+  }
+  return slave_id_to_index[slave_id];
+}
+
 void slaves_init() {
+  memset(slave_id_to_index, 0xFF, sizeof(slave_id_to_index));
   for (int i = 0; i < N_SLAVES; i++) {
-    slaves[i].id = i;
+    slaves[i].id = 0xFF;
     slaves[i].error.kind = SLAVE_ERR_NONE;
     slaves[i].last_message = 0;
     slaves[i].min_voltage = 0;
@@ -69,41 +89,43 @@ void slaves_check() {
 
 void slaves_handle_panic(const uint8_t *data) {
   uint8_t slave_id = ftcan_unmarshal_unsigned(&data, 1);
+  uint8_t idx = get_slave_index(slave_id);
   uint8_t error_kind = ftcan_unmarshal_unsigned(&data, 1);
   switch (error_kind) {
   case SLAVE_PANIC_OT:
-    slaves[slave_id].error.kind = SLAVE_ERR_OT;
+    slaves[idx].error.kind = SLAVE_ERR_OT;
     break;
   case SLAVE_PANIC_UT:
-    slaves[slave_id].error.kind = SLAVE_ERR_UT;
+    slaves[idx].error.kind = SLAVE_ERR_UT;
     break;
   case SLAVE_PANIC_OV:
-    slaves[slave_id].error.kind = SLAVE_ERR_OV;
+    slaves[idx].error.kind = SLAVE_ERR_OV;
     break;
   case SLAVE_PANIC_UV:
-    slaves[slave_id].error.kind = SLAVE_ERR_UV;
+    slaves[idx].error.kind = SLAVE_ERR_UV;
     break;
   }
-  slaves[slave_id].error.data = ftcan_unmarshal_unsigned(&data, 4);
-  slaves[slave_id].last_message = HAL_GetTick();
+  slaves[idx].error.data = ftcan_unmarshal_unsigned(&data, 4);
+  slaves[idx].last_message = HAL_GetTick();
 }
 
 void slaves_handle_status(const uint8_t *data) {
   uint8_t slave_id = data[0] & 0x7F;
+  uint8_t idx = get_slave_index(slave_id);
   int error = data[0] & 0x80;
   if (error) {
-    if (slaves[slave_id].error.kind == SLAVE_ERR_NONE) {
-      slaves[slave_id].error.kind = SLAVE_ERR_UNKNOWN;
+    if (slaves[idx].error.kind == SLAVE_ERR_NONE) {
+      slaves[idx].error.kind = SLAVE_ERR_UNKNOWN;
     }
   } else {
-    slaves[slave_id].error.kind = SLAVE_ERR_NONE;
+    slaves[idx].error.kind = SLAVE_ERR_NONE;
   }
-  slaves[slave_id].soc = data[1];
+  slaves[idx].soc = data[1];
   const uint8_t *ptr = &data[2];
-  slaves[slave_id].min_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
-  slaves[slave_id].max_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
-  slaves[slave_id].max_temp = ftcan_unmarshal_unsigned(&ptr, 2);
-  slaves[slave_id].last_message = HAL_GetTick();
+  slaves[idx].min_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
+  slaves[idx].max_voltage = ftcan_unmarshal_unsigned(&ptr, 2);
+  slaves[idx].max_temp = ftcan_unmarshal_unsigned(&ptr, 2);
+  slaves[idx].last_message = HAL_GetTick();
 }
 
 void slaves_handle_log(const uint8_t *data) {