/*

     (1)               (6)
       \               /
        \             /
         \           /
          *---(F)---*
         /           \
        /             \
       /               \
(2)---*        o        *---(5)
       \               /
        \             /
         \           /
          *---(B)---*
         /           \
        /             \
       /               \
     (3)               (4)

*/

#include "robot.h"
#include "led.h"

#include <zephyr/logging/log.h>

LOG_MODULE_REGISTER(robot, LOG_LEVEL_INF);


// THREAD
static struct k_thread robot_thread_data;
static k_tid_t robot_thread_id = NULL;
#define ROBOT_THREAD_STACK_SIZE 2048
#define ROBOT_THREAD_PRIORITY 5
K_THREAD_STACK_DEFINE(robot_thread_stack, ROBOT_THREAD_STACK_SIZE);

// BUFFER
#define ROBOT_MSG_BUFFER_SIZE 10
struct command_message_t robot_msg_buffer[ROBOT_MSG_BUFFER_SIZE + 2];
struct command_message_t *robot_msg_buffer_ptr;
char robot_msgq_buffer[ROBOT_MSG_BUFFER_SIZE * sizeof(uint32_t)];
struct k_msgq robot_msgq;


static void robot_thread(void *p1, void *p2, void *p3) {
	ARG_UNUSED(p1);
	ARG_UNUSED(p2);
	ARG_UNUSED(p3);
	struct command_message_t *data;

	while (1) {
        k_msgq_get(&robot_msgq, &data, K_FOREVER);

		// TODO: handle commands
		led1_toggle();
	}
}

int robot_init() {

	// BUFFER
	memset(robot_msg_buffer, 0, sizeof(robot_msg_buffer));
	robot_msg_buffer_ptr = robot_msg_buffer;
	k_msgq_init(&robot_msgq, robot_msgq_buffer, sizeof(struct command_message_t *), ROBOT_MSG_BUFFER_SIZE);

	// THREAD
	robot_thread_id = k_thread_create(
		&robot_thread_data,
		robot_thread_stack,
		K_THREAD_STACK_SIZEOF(robot_thread_stack),
		robot_thread,
		NULL, NULL, NULL,
		ROBOT_THREAD_PRIORITY,
		0,
		K_NO_WAIT
	);

	return 0;
}

struct command_message_t* robot_get_next_tx_buf() {
	struct command_message_t *buf = robot_msg_buffer_ptr;

	// Increment the buffer pointer
	robot_msg_buffer_ptr++;

	if (robot_msg_buffer_ptr > &robot_msg_buffer[ROBOT_MSG_BUFFER_SIZE-1]) {
		robot_msg_buffer_ptr = robot_msg_buffer;
	}

	return buf;
}

int robot_notify_msg(struct command_message_t* msg) {
	k_msgq_put(&robot_msgq, &msg, K_NO_WAIT);

	return 0;
}