/*
 * test.cpp
 *
 *  Created on: Oct 24, 2023
 *      Author: carlw
 */

//#include <sta/bus/can/controller.hpp>
#include <sta/devices/stm32/can.hpp>

//extern STM32CanController(CAN_HandleTypeDef * handle);


	extern "C" void testCan(CAN_HandleTypeDef * handle){

		sta::STM32CanController canController(handle);

		canController.start();

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

		// Create your message payload
		uint8_t payload[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08};

		// Send the CAN message
		while (true){
			canController.sendFrame(txHeader, payload);
			HAL_Delay(1000);
		}

	}

	extern "C" void testCanMsg(CAN_HandleTypeDef * handle, uint8_t payload[8]){

		sta::STM32CanController canController(handle);

		//canController.start();

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

		// Send the CAN message
		canController.sendFrame(txHeader, payload);


	}

	extern "C" void 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
	}

	extern "C" uint8_t packValues(uint8_t type_id, uint8_t sensor_ID, uint8_t value, uint8_t include) {
		uint8_t packedByte = 0;

		// Shifting and ORing the values
		packedByte |= (type_id & 0x03) << 6;    // Two bits for type_id, left-shifted by 6
	    packedByte |= (sensor_ID & 0x07) << 3;   // Three bits for sensorID, left-shifted by 3
	    packedByte |= (include & 0x01) << 2;          // Flag for included value
	    packedByte |= (value & 0x01) << 1;      // One bit for value

	    return packedByte;
	}
