/*
 * 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 <cmsis_os2.h>

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;
		}
	}

	void CanTask::func()
	{
		//STA_DEBUG_HEAP_STATS();
		canController.sendFrame(txHeader, payload);
		//STA_DEBUG_HEAP_STATS();

		CAN_RxHeaderTypeDef rxHeader;			//CAN Bus Receive Header
		uint8_t canRX[8] = {0,0,0,0,0,0,0,0};  	//CAN Bus Receive Buffer

		STA_DEBUG_PRINTLN("WAITING");
		HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &rxHeader, canRX);

		uint8_t type_id, sensor_ID, value, include;

		unpackValues(canRX[0], &type_id, &sensor_ID, &value, &include);

		if (type_id == 0){
			if (sensor_ID == 1){

				STA_DEBUG_PRINTLN("Relay 1 triggered");
				if(value == 1){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2, GPIO_PIN_SET);
				} else if(value == 0){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2, GPIO_PIN_RESET);
				}

			}else if (sensor_ID == 2){

				STA_DEBUG_PRINTLN("Relay 2 triggered");
				if(value == 1){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1, GPIO_PIN_SET);
				} else if(value == 0){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1, GPIO_PIN_RESET);
				}
			}else if (sensor_ID == 3){

				STA_DEBUG_PRINTLN("Relay 3 triggered");
				if(value == 1){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_0, GPIO_PIN_SET);
				} else if(value == 0){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_0, GPIO_PIN_RESET);
				}
			}else if (sensor_ID == 4){

				STA_DEBUG_PRINTLN("Relay 4 triggered");
				if(value == 1){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3, GPIO_PIN_SET);
				} else if(value == 0){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3, GPIO_PIN_RESET);
				}
			}else if (sensor_ID == 5){

				STA_DEBUG_PRINTLN("Relay 5 triggered");
				if(value == 1){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6, GPIO_PIN_SET);
				} else if(value == 0){
					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6, GPIO_PIN_RESET);
				}
			}

		}

		HAL_Delay(1000);
	}

	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