/*
 * 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 <sta/tacos/manager.hpp>

#include <cmsis_os2.h>

namespace tasks
{
	CanTask::CanTask(uint32_t canID)
		: TacosThread("SYS_MSG_HANDLE", osPriorityNormal)
	{
		setCanID(canID);
	}

	void CanTask::init()
	{
	}

	void CanTask::func()
	{
		CanSysMsg msg;
		if (CAN_queue_.get(&msg, osWaitForever))
		{

			// Interpret received
			uint8_t type_id, sensor_ID, value, include;
			unpackValues(msg.payload[0], &type_id, &sensor_ID, &value, &include);

			// Reformat for internal use
			msg.header.payloadLength = 2;
			msg.payload[0] = sensor_ID;			// First Byte determines sensor number
			msg.payload[1] = value;				// Second Byte determines value

			uint32_t target_ID;

			switch (type_id){
				case 0: // Relay
					target_ID = SYSTEM_CAN_RELAY;
					break;
				case 1: // Thermocouple
					target_ID = SYSTEM_CAN_THERMO;
					break;
				case 2: // V_IN
					target_ID = SYSTEM_CAN_V_IN;
					break;

				default:
					target_ID = SYSTEM_CAN_ERROR;
			}

			// Append to the correct thread's queue
			for (std::shared_ptr<TacosThread> thread : sta::tacos::Manager::instance()->getActiveThreads()){
				if (thread->getCanID() == target_ID){
					thread->CAN_queue_.put(msg);
					break;
				}
			}
		}
	}

	void CanTask::unpackValues(uint8_t packedByte, uint8_t* type_id, uint8_t* sensor_ID, uint8_t* value, uint8_t* include) {
		*type_id = (packedByte >> 6) & 0x03;       // Extracting two bits for type_id
		*sensor_ID = (packedByte >> 3) & 0x07;     // Extracting three bits for sensorID
		*include = (packedByte >> 2) & 0x01;       // Extracting the flag for included value
		*value = (packedByte >> 1) & 0x01;         // Extracting one bit for value
	}
} // namespace demo