/*
 * can_task.cpp
 *
 *  Created on: 10 Dec 2023
 *      Author: Carl
 */

#include <tasks/can_task.hpp>
#include <sta/debug/debug.hpp>
#include <sta/rtos/debug/heap_stats.hpp>
#include "can.h"
#include <sta/tacos.hpp>

#include <cmsis_os2.h>

namespace demo
{
	CanTask::CanTask(const char* name, uint32_t canID)
		: TacosThread(name, osPriorityNormal)
	{
		setCanID(canID);
	}

	void CanTask::init()
	{
	}

	void CanTask::func()
	{
		STA_DEBUG_PRINTLN("Can Task awaiting message");
		if (CAN_queue_.available() > 0)
		{
			// Receiving message
			CanSysMsg msg;
			CAN_queue_.get(&msg);
			STA_DEBUG_PRINTLN("Can Task received message");

			// Sending it back
			sta::tacos::queueCanBusMsg(msg, 0);
			STA_DEBUG_PRINTLN("Can Task sent message");
		}
	}
} // namespace demo