Fix module logic, updated tests, add tests (#584)

TactilityKernel/include/tactility/module.h

@@ -124,7 +124,7 @@ error_t module_construct_add_start(struct Module* module);
 
 /**
  * @brief Tries to ensure the module is in a started state.
- * Calls module_construct if needed, calls module_start if needed.
+ * Calls module_construct if needed, calls module_add if needed, calls module_start if needed.
  * @param module the module
  * @return ERROR_NONE if module is in a started state
  */
@@ -132,7 +132,7 @@ error_t module_ensure_started(struct Module* module);
 
 /**
  * @brief Tries to ensure the module is in a started state.
- * Calls module_stop if needed, calls module_destruct if needed.
+ * Calls module_stop if needed, calls module_remove, calls module_destruct if needed.
  * @param module the module
  * @return ERROR_NONE if module is in a destructed state
  */

TactilityKernel/source/module.cpp

@@ -146,6 +146,9 @@ error_t module_ensure_started(Module* module) {
         if (result != ERROR_NONE) { return result; }
     }
 
+    error_t add_result = module_add(module);
+    if (add_result != ERROR_NONE && add_result != ERROR_INVALID_STATE) { return add_result; }
+
     if (!module->internal->started) {
         error_t result = module_start(module);
         if (result != ERROR_NONE) { return result; }
@@ -162,6 +165,9 @@ error_t module_ensure_destructed(Module* module) {
             if (result != ERROR_NONE) { return result; }
         }
 
+        result = module_remove(module);
+        if (result != ERROR_NONE) { return result; }
+
         result = module_destruct(module);
         if (result != ERROR_NONE) { return result; }
     }