Implimented a callback approach to processing functions

lib/CommandHandler/CommandHandler.cpp

@@ -0,0 +1,43 @@
+#include "CommandHandler.h"
+
+CommandHandler::CommandHandler(){
+    for(uint32_t i = 0; i < MAX_COMMAND_SIZE; i++){
+        commandCallbacks[i] = nullptr;
+        commandCallbackIDs[i] = 0;
+    }
+}
+
+bool CommandHandler::RegisterCommand(uint32_t commandID, CommandCallback callback){
+    for(uint32_t i = 0; i < MAX_COMMAND_SIZE; i++){
+        if(commandCallbacks[i] == nullptr){
+            commandCallbacks[i] = callback;
+            commandCallbackIDs[i] = commandID;
+            return true;
+        }
+    }
+    return false;
+}
+
+bool CommandHandler::RemoveCommand(uint32_t commandID){
+    for(uint32_t i = 0; i < MAX_COMMAND_SIZE; i++){
+        if(commandCallbackIDs[i] == commandID){
+            commandCallbacks[i] = nullptr;
+            commandCallbackIDs[i] = -1;
+            return true;
+        }
+    }
+    return false;
+}
+
+CommandHandler::CommandStatus CommandHandler::ProcessCommand(uint32_t * command, uint32_t commandSize){
+    uint32_t commandID{command[0]};
+    // get a pointer to the second element in the array because the first element is the command ID
+    uint32_t * args{&(command[1])};
+
+    for(uint32_t i = 0; i < MAX_COMMAND_SIZE; i++){
+        if(commandCallbacks[i] != nullptr && commandCallbackIDs[i] == commandID){
+            return commandCallbacks[i](args, commandSize-1);
+        }
+    }
+    return CommandStatus::INVALID;
+}

lib/CommandHandler/CommandHandler.h

@@ -0,0 +1,49 @@
+#pragma once
+#include <cstdint>
+
+class CommandHandler{
+public:
+    // create an enum for command return values
+    enum CommandStatus{
+        SUCCESS,
+        FAILURE,
+        INVALID
+    };
+
+    // create a typedef for a command callback function which takes an array pointer and an array size as an argument and returns a CommandStatus
+    typedef CommandStatus (*CommandCallback)(uint32_t * command, uint32_t commandSize);
+
+    CommandHandler();
+    ~CommandHandler() = default;
+
+    /**
+     * @brief Register a command callback function to a command ID
+     * @param commandID The command ID to register the callback to
+     * @param callback The callback function to register
+     * @return true if the callback was registered successfully, false otherwise
+     */
+    bool RegisterCommand(uint32_t commandID, CommandCallback callback);
+
+    /**
+     * @brief Remove a command callback function from a command ID
+     * @param commandID The command ID to remove the callback from
+     * @return true if the callback was removed successfully, false otherwise
+     */
+    bool RemoveCommand(uint32_t commandID);
+
+    /**
+     * @brief Process a command
+     * @param command The command to process
+     */
+    CommandStatus ProcessCommand(uint32_t * command, uint32_t commandSize);
+
+private:
+    static constexpr uint32_t MAX_COMMAND_SIZE{10};
+    // pointer to the head of the command array
+    uint32_t * command{nullptr};
+
+    // an array of command callbacks
+    CommandCallback commandCallbacks[MAX_COMMAND_SIZE];
+    // an array of command callback IDs
+    uint32_t commandCallbackIDs[MAX_COMMAND_SIZE];
+};

src/main.cpp

@@ -15,6 +15,7 @@
 #include "BluetoothSerial.h"
 #include "SerialMessage.h"
 #include "GlobalPrint.h"
+#include "CommandHandler.h"
 
 #include "BoardManager.h"
 #include "BoardDriver.h"
@@ -36,6 +37,8 @@ std::array<std::vector<AnimationFrame>*, 1> animations = {
   // &RotatingCubes::rotating,
 };
 
+CommandHandler commandHandler{};
+
 // BluetoothSerial SerialBT;
 // BluetoothSerialMessage serialMessageBT(&SerialBT);
 SerialMessage<SERIAL_CHAR_LENGTH, SERIAL_ARG_LENGTH> serialMessage(&Serial);
@@ -73,7 +76,6 @@ void SetupBluetoothModule(){
   delay(100);
 }
 
-
 void printBoardState(){
   GlobalPrint::Print("!0,");
   String boardString;
@@ -82,7 +84,7 @@ void printBoardState(){
   GlobalPrint::Println(";");
 }
 
-void SetStackColor(uint32_t * args, int argsLength){
+void SetStackColor(uint32_t * args, uint32_t argsLength){
   uint32_t stackNum = args[1];
   uint32_t X_COORD{stackNum};
   while(X_COORD > BOARD_DIMENSIONS.x - 1){
@@ -102,43 +104,6 @@ void SetStackColor(uint32_t * args, int argsLength){
   boardManager.SetColumnColors(V3D<uint32_t>{X_COORD, Y_COORD, BOARD_TYPES::PLANE_NORMAL::Z}, colors, numColors);
 }
 
-void parseData(Message<SERIAL_CHAR_LENGTH, SERIAL_ARG_LENGTH> &message){
-  int32_t * args{message.GetArgs()};
-  uint32_t argsLength{message.GetPopulatedArgs()};
-  uint32_t command = args[0];
-  switch(command){
-    case Commands::BoardState:{
-      printBoardState();
-      break;
-    }
-    case Commands::PING:{
-      GlobalPrint::Println("!" + String(Commands::PING) + ";");
-      boardManager.PrintColorState();
-      break;
-    }
-    case Commands::SetStackColors:{
-      GlobalPrint::Println("!2;");
-      animator.isEnabled = false;
-      V3D<uint32_t> black{};
-      boardManager.FillColor(black);
-      SetStackColor(reinterpret_cast<uint32_t *>(args), argsLength);
-      break;
-    }
-    case Commands::GoToIdle:{
-      GlobalPrint::Println("!3;");
-      animator.isEnabled = true;
-      break;
-    }
-    default:{
-      GlobalPrint::Println("INVALID COMMAND");
-      break;
-    }
-  }
-
-  // now that we have run the command we can clear the data for the next command.
-  serialMessage.ClearNewData();
-}
-
 // --------------------------------------------------
 // ----------------- FREERTOS TASKS -----------------
 // --------------------------------------------------
@@ -148,12 +113,14 @@ void UpdateCommunication(void * params){
     // DO serial processing
     serialMessage.Update();
     if(serialMessage.IsNewData()){
-      parseData(serialMessage);
+      // TODO: Is it really a good idea to cast to unsigned here? do we ever use signed values? Find out.
+      commandHandler.ProcessCommand(reinterpret_cast<uint32_t *>(serialMessage.GetArgs()), serialMessage.GetPopulatedArgs());
+      serialMessage.ClearNewData();
     }
     // serialMessageBT.Update();
     // if(serialMessageBT.IsNewData()){
-    //   parseData(serialMessageBT.GetArgs(), serialMessageBT.GetArgsLength());
+    //   commandHandler.ProcessCommand(reinterpret_cast<uint32_t *>(serialMessage.GetArgs()), serialMessage.GetPopulatedArgs());
-    //   serialMessageBT.ClearNewData();
+    //   serialMessage.ClearNewData();
     // }
     vTaskDelay(3);
   }
@@ -214,6 +181,37 @@ void setup() {
   SetupBluetoothModule();
   Serial.begin(9600);
 
+  // TODO: We should define these as functions and not lambdas because I think these get 
+  // destroyed once the setup function exits and that will break everything
+  // register the commands with the command handler
+  commandHandler.RegisterCommand(Commands::BoardState, [](uint32_t * /*args*/, uint32_t /*argsLength*/){
+    printBoardState();
+    return CommandHandler::CommandStatus::SUCCESS;
+  });
+
+  // create a lambda function to handle the ping command which calls GlobalPrint::Println("!" + String(Commands::PING) + ";");
+  commandHandler.RegisterCommand(Commands::PING, [](uint32_t * /*args*/, uint32_t /*argsLength*/){
+    GlobalPrint::Println("!" + String(Commands::PING) + ";");
+    return CommandHandler::CommandStatus::SUCCESS;
+  });
+
+  commandHandler.RegisterCommand(Commands::SetStackColors, [](uint32_t * args, uint32_t argsLength){
+    GlobalPrint::Println("!2;");
+    animator.isEnabled = false;
+    V3D<uint32_t> black{};
+    boardManager.FillColor(black);
+    SetStackColor(reinterpret_cast<uint32_t *>(args), argsLength);
+    return CommandHandler::CommandStatus::SUCCESS;
+  });
+
+  commandHandler.RegisterCommand(Commands::GoToIdle, [](uint32_t * /*args*/, uint32_t /*argsLength*/){
+    GlobalPrint::Println("!3;");
+    animator.isEnabled = true;
+    return CommandHandler::CommandStatus::SUCCESS;
+  });
+
+
+
   Serial.println("Configuring communication methods");
   serialMessage.Init(9600);
   // SerialBT.begin("blockPartyBT");