#include "statemachine.h"

int current_state_id = 0;
state_t states[STATE_AMOUNT] = {NULL, NULL};
__UINT8_TYPE__ next = 0;
static char has_init = 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;
}

__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;
}

void statemachine_loop()
{

  if (has_init == 0)
  {
    has_init = 1;
        states[current_state_id].on_enter();
  }
  
    if (states[current_state_id].next == current_state_id)
    {
        next = 0;
    }

    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)
        {
            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;
}