finished state machine

due_obd2/statemachine.c

@@ -1,19 +1,60 @@
 #include "statemachine.h"
 
-state_t none_state = 
-{
-    .id = 0,
-    .on_enter = &__state_none,
-    .on_run = &__state_none,
-    .on_exit = &__state_none,
+int current_state_id = 0;
+state_t states[STATE_AMOUNT] = {NULL, NULL};
+__UINT8_TYPE__ next = 0;
 
+__UINT8_TYPE__ statemachine_register_state(state_t *state, int index)
+{
+    if (index >= STATE_AMOUNT)
+    {
+        return 0;
+    }
+    if (state->id != index)
+    {
+        return 0;
+    }
+    states[index] = *state;
+    return 1;
 }
-statemachine_t statemachine = 
-{
-    .current_state = 
-};
 
-void statemachine_init()
+__UINT8_TYPE__ statemachine_init()
 {
+    for (int i = 0; i < STATE_AMOUNT; i++)
+    {
+        if (states[i].id < 0 || states[i].next < 0 || states[i].on_enter == NULL || states[i].on_run == NULL || states[i].on_exit == NULL)
+        {
+            return 0;
+        }
+    }
+    current_state_id = 0;
+    states[current_state_id].on_enter();
+}
 
+void statemachine_loop()
+{
+    if (next)
+    {
+        next = 0;
+
+        if (states[current_state_id].on_exit != NULL)
+        {
+            states[current_state_id].on_exit();
+        }
+        if (states[current_state_id].next >= 0 && states[current_state_id].next != current_state_id)
+        {
+            current_state_id = states[current_state_id].next;
+        }
+        states[current_state_id].on_enter();
+    }
+
+    if (states[current_state_id].on_run != NULL)
+    {
+        states[current_state_id].on_run();
+    }
+}
+
+void statemachine_next()
+{
+    next = 1;
 }