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

#include <tasks/thermo.hpp>
#include <sta/debug/debug.hpp>
#include <gpio.h>
#include <spi.h>
#include <sta/MAX31855.hpp>
#include <sta/devices/stm32/bus/spi.hpp>
#include <sta/tacos.hpp>

#include <sta/rtos/mutex.hpp>

#include <cmsis_os2.h>

namespace tasks
{
	ThermoTask::ThermoTask(uint32_t canID)
		: TacosThread("Thermo", osPriorityNormal)
	{
		setCanID(canID);
	}

	void ThermoTask::init()
	{
		mutex = new sta::RtosMutex("spi2");
		spi2 = new sta::STM32SPI(&hspi2, 16000000, mutex);

		// Init cs pins
		cs_pin[0] = new sta::STM32GpioPin(GPIOE, GPIO_PIN_12);
		cs_pin[1] = new sta::STM32GpioPin(GPIOB, GPIO_PIN_10);
		cs_pin[2] = new sta::STM32GpioPin(GPIOE, GPIO_PIN_15);
		cs_pin[3] = new sta::STM32GpioPin(GPIOE, GPIO_PIN_14);
		cs_pin[4] = new sta::STM32GpioPin(GPIOE, GPIO_PIN_13);

		// init devices
		for (uint8_t i = 0; i < 5; i++){
			device_[i] = new sta::STM32SPIDevice(spi2, cs_pin[i]);
		}

		// init drivers
		for (uint8_t i = 0; i < 5; i++){
			tc_[i] = new sta::MAX31855(device_[i]); //create driver object
		}
	}

	void ThermoTask::func()
	{
		// Receiving polling message
		CanSysMsg msg;

		if (CAN_queue_.get(&msg, osWaitForever))
		{

			//fuck off other pins
			/*HAL_GPIO_WritePin(GPIOE,GPIO_PIN_12, GPIO_PIN_SET);
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_10, GPIO_PIN_SET);
			HAL_GPIO_WritePin(GPIOE,GPIO_PIN_15, GPIO_PIN_SET);
			HAL_GPIO_WritePin(GPIOE,GPIO_PIN_14, GPIO_PIN_SET);*/

			uint8_t dev = msg.payload[0] - 1; // -1 to offset start from 0

			//float temp = tc_[dev]->measureTemp();
			tc_[dev]->update(); // update internal values
			float temp = tc_[dev]->getTemp(); //read out temperature in degrees Celsius

			uint8_t* bytePointer = reinterpret_cast<uint8_t*>(&temp);

			for(uint8_t i = 0; i < 4; i++){
				msg.payload[i] = bytePointer[i]; 				// first four bytes are float of Temperature in Celcius
			}


			//msg.payload[4] = tc_[dev]->getStatus();				// Followed by status

			msg.header.sid = getCanID();
			msg.header.payloadLength = 5;
			msg.header.eid = 0;
			msg.header.format = 0;

			// Send it out
			sta::tacos::queueCanBusMsg(msg, 0);

		}

	}

} // namespace demo