#include "sensor_task.h"

#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_gpio.h"
#include "gpio.h"

#include "incphub/incphub_client.h"



StackType_t _sensor_task_stack[SENSOR_TASK_STACK_DEPTH];
StaticTask_t _sensor_task;



void sensor_task_main(void *params)
{
	(void)params;

	uint8_t *buffer;
	size_t length;
	incphub_addr_t source;

	// Init incphub client
	incphub_cli_t *cli;
	incphub_init_client(&cli, 69);

	while (1)
	{
		// Get a message (assume it is a uint32_t)
		incphub_await_msg(cli, &buffer, &length, &source, portMAX_DELAY);

		// Ignore messages of different size
		if (length != sizeof(uint32_t))
			continue;

		// Read incoming message
		uint32_t num = *(uint32_t*)buffer;

		// Release buffer once read
		incphub_release_msg_buffer(cli, buffer);

		// Process message
		num++;

		// Request buffer for reply
		incphub_get_msg_buffer(cli, &buffer, sizeof(uint32_t));

		// Prepare reply message
		*(uint32_t*)buffer = num;

		// Send reply
		incphub_send_msg(cli, buffer, sizeof(uint32_t), source, portMAX_DELAY);
	}
}