#include <stdint.h>
#include <string.h>

#include <zephyr/kernel.h>

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

static void ble_input_callback(const void *buffer, uint16_t buffer_len);

routine_return_e
input_echo()
{
    ble_register_oninput_callback(ble_input_callback);

    while (routine_status() == ROUTINE_STATUS_RUNNING) {
        routine_yield();
    }

    ble_clear_oninput_callback();
    return (ROUTINE_RETURN_SUCCESS);
}

static void
ble_input_callback(const void *buffer, uint16_t buffer_len)
{
    size_t echo_length = snprintk(
        NULL, 0, "Echo: %.*s",
        buffer_len, (const char *)buffer
    );
    char *echo_buf = k_malloc(echo_length + 1);

    if (echo_buf == NULL) {
        ble_set_telemetry_data("Alloc failure", 13);
        return;
    }

    snprintk(
        echo_buf, echo_length + 1, "Echo: %.*s",
        buffer_len, (const char *)buffer
    );
    ble_set_telemetry_data(echo_buf, echo_length);
    k_free(echo_buf);
}