/*
 * 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 <sta/rtos/system/can_bus.hpp>

#include <cmsis_os2.h>

#include <sta/tacos/can_bus.hpp>

namespace demo
{
	CanTask::CanTask(const char* name, CAN_HandleTypeDef * handle)
		: TacosThread(name, osPriorityNormal), canController(handle)
	{

	}

	void CanTask::init()
	{

		canController.start();

		txHeader.id.format = sta::CanIdFormat::STD; // Set to EXT for extended ID
		txHeader.id.sid = 0x040; // Set the standard ID or extended ID
		txHeader.payloadLength = 8; // Set the payload length (max 8 bytes)

		// Create your message payload
		for (int i = 0; i < 8; ++i) {
			payload[i] = i + 1;
		}

		struct CanMsgHeader header ={
				txHeader.id.sid, txHeader.id.eid, (uint8_t)txHeader.id.format, txHeader.payloadLength
		};

		CanDataMsg myCanDataMsg;
		myCanDataMsg.header = header;
		for (int i = 0; i < 64; ++i) {
			myCanDataMsg.payload[i] = payload[i];
		}

		sta::tacos::CanBus::instance()->queueCanBusMsg(myCanDataMsg, osWaitForever);
	}

	void CanTask::func()
	{
		
		STA_DEBUG_PRINTLN("Wasting time in CAN task...");
		HAL_Delay(1000);
	}
} // namespace demo