#include <stdint.h>
#include <stdbool.h>

#include <zephyr/kernel.h>

#include "core/ble.h"
#include "core/routine.h"

extern const routine_listing_t *routine_list;

typedef struct routine_state_struct {
    uint8_t routine_count;
    /** Index of the current routine */
    int8_t current_routine;
    /** Index of the next routine */
    int8_t next_routine;
    /** Return status of the last routine to complete execution */
    routine_return_e last_routine_ret;
    /** Routine status */
    routine_status_e status;
    /** Lock for the */
    struct k_mutex lock;
} routine_state_t;

static routine_state_t routine_state = {
    .routine_count = 0,
    .current_routine = ROUTINE_INDEX_NONE,
    .next_routine = ROUTINE_INDEX_NONE,
};

void
routine_init()
{
    k_mutex_init(&routine_state.lock);

    while (1) {
        const routine_listing_t *listing = &routine_list[routine_state.routine_count];

        if (listing->entry == NULL) {
            break;
        }

        routine_state.routine_count++;
    }
}

void
routine_exec()
{
    while (1) {
        routine_return_e ret;

        ble_tick();

        k_mutex_lock(&routine_state.lock, K_FOREVER);

        if (routine_state.next_routine == ROUTINE_INDEX_NONE) {
            /* nothing to do */
            k_mutex_unlock(&routine_state.lock);
            k_yield();
            continue;
        }

        /* something to do! */
        routine_state.current_routine = routine_state.next_routine;
        routine_state.next_routine = ROUTINE_INDEX_NONE;
        routine_state.status = ROUTINE_STATUS_RUNNING;

        k_mutex_unlock(&routine_state.lock);


        ret = routine_list[routine_state.current_routine].entry();

        k_mutex_lock(&routine_state.lock, K_FOREVER);
        routine_state.last_routine_ret = ret;
        routine_state.current_routine = ROUTINE_INDEX_NONE;
        routine_state.status = ROUTINE_STATUS_STOPPED;
        k_mutex_unlock(&routine_state.lock);
    }
}

int
routine_select(uint8_t index)
{
    if (index >= routine_state.routine_count) {
        return (-1);
    }

    k_mutex_lock(&routine_state.lock, K_FOREVER);
    routine_state.next_routine = index;
    routine_state.status = ROUTINE_STATUS_STOPPING;
    k_mutex_unlock(&routine_state.lock);

    return (0);
}

void
routine_get_listing(char *buf, uint16_t buf_len)
{
    int written;
    uint16_t pos = 0;

    written = snprintf(&buf[pos], buf_len - pos, "Routines (%d):\r\n", routine_state.routine_count);
    pos += written;

    for (int i = 0; i < routine_state.routine_count; i++) {
        written = snprintf(
            &buf[pos], buf_len - pos,
            "%d: %s\r\n",
            i, routine_list[i].name
        );
        pos += written;
    }

    /* walk back trailing \r\n */
    buf[pos - 2] = '\0';
}

int8_t
routine_get_current()
{
    int8_t ret;

    k_mutex_lock(&routine_state.lock, K_FOREVER);
    ret = routine_state.current_routine;
    k_mutex_unlock(&routine_state.lock);

    return (ret);
}

routine_status_e
routine_status()
{
    routine_status_e ret;

    k_mutex_lock(&routine_state.lock, K_FOREVER);
    ret = routine_state.status;
    k_mutex_unlock(&routine_state.lock);

    return (ret);
}

void
routine_delay(uint32_t milliseconds)
{
    k_sleep(K_MSEC(milliseconds));
}

void
routine_yield()
{
    ble_tick();
    k_yield();
}