/*
 * sense.cpp
 *
 *  Created on: Jun 17, 2024
 *      Author: carlos
 */

#include <tasks/sense.hpp>

#include <sta/tacos.hpp>
#include <sta/rtos/signal.hpp>
#include <sta/devices/stm32/delay.hpp>
#include <shared.hpp>

#include <tim.h>
#include <adc.h>

namespace tasks
{
	/**
	 * @brief Signal for the ADC conversion complete interrupt to communicate with the ADCTask.
	 */
	sta::RtosSignal *adcCompleteSignal = nullptr;

	SenseTask::SenseTask(ADC_HandleTypeDef *handle)
		: sta::tacos::TacosThread{"Sense", osPriorityNormal},
		  adc_{handle}, result_counter_{0}
	{
	}

	void SenseTask::init()
	{
		adcCompleteSignal = new sta::RtosSignal(1);

		// Init msg_
		msg_.header.payloadLength = 8;

		msg_.header.eid = 0;
		msg_.header.format = 0;

		// Start ADC timer
		HAL_TIM_Base_Start(&htim3);

		adc_.startDMA((uint32_t *)dmaBuffer_, bufferSize_);
	}

	void SenseTask::func()
	{
		STA_ASSERT(adcCompleteSignal != nullptr);

		// Wait for the conversion to be completed.
		adcCompleteSignal->wait();

		// Determine wether to send the data or not
		uint16_t state = sta::tacos::getState();
		bool logging = !(state == stahr::CALIBRATING || state == stahr::READY_ON_PAD);
		bool send = result_counter_++ % 4 == 0;

		// Sending the first 4 ADC values (limited by the CAN message size of 8 bytes)
		for (size_t i = 0; i < 4; ++i)
		{
			// Send the MSB first.
			msg_.payload[2 * i] = (uint8_t)(dmaBuffer_[i] >> 8);

			// ... LSB second
			msg_.payload[2 * i + 1] = (uint8_t)dmaBuffer_[i];
		}

		// Save the first 4 ADC values for logging
		for (size_t i = 0; i < 4; i++)
		{
			result_[i] = msg_.payload[2 * i] << 8 | msg_.payload[2 * i + 1];
			STA_DEBUG_PRINTF("> ADC[%d]: %d", i, result_[i]);
		}

		if (send)
		{
			// Send it out while on the pad. Otherwise we only log the data.
			msg_.header.payloadLength = 8;
			msg_.header.sid = SENSE_CAN_ID;
			sta::tacos::queueCanBusMsg(msg_, 0);
		}

		// Send the last 2 ADC values (limited by the CAN message size of 8 bytes)
		for (size_t i = 4; i < bufferSize_; ++i)
		{
			// Send the MSB first.
			msg_.payload[2 * (i - 4)] = (uint8_t)(dmaBuffer_[i] >> 8);

			// ... LSB second
			msg_.payload[2 * (i - 4) + 1] = (uint8_t)dmaBuffer_[i];
		}

		// Save the last 2 ADC values for logging
		for (size_t i = 4; i < 6; i++)
		{
			result_[i] = msg_.payload[2 * (i - 4)] << 8 | msg_.payload[2 * (i - 4) + 1];
			STA_DEBUG_PRINTF("> ADC[%d]: %d", i, result_[i]);
		}

		// Get the sense pins on the fire output
		uint8_t sense_fire = 0;

		uint8_t sense_fire_1 = HAL_GPIO_ReadPin(SENSE_FIRE_GROUP1, SENSE_FIRE_PIN_1);
		uint8_t sense_fire_2 = HAL_GPIO_ReadPin(SENSE_FIRE_GROUP2, SENSE_FIRE_PIN_2);
		uint8_t sense_fire_3 = HAL_GPIO_ReadPin(SENSE_FIRE_GROUP3, SENSE_FIRE_PIN_3);
		uint8_t sense_fire_4 = HAL_GPIO_ReadPin(SENSE_FIRE_GROUP4, SENSE_FIRE_PIN_4);
		uint8_t sense_fire_5 = HAL_GPIO_ReadPin(SENSE_FIRE_GROUP5, SENSE_FIRE_PIN_5);
		uint8_t sense_fire_6 = HAL_GPIO_ReadPin(SENSE_FIRE_GROUP6, SENSE_FIRE_PIN_6);

		sense_fire = sense_fire_1 | (sense_fire_2 << 1) | (sense_fire_3 << 2) | (sense_fire_4 << 3) | (sense_fire_5 << 4) | (sense_fire_6 << 5);

		STA_DEBUG_PRINTF("> Sense Fired pins: %d", sense_fire);

		if (send)
		{
			msg_.payload[4] = sense_fire; // Send the sense pins on the fire output

			// Send second half of the ADC values, via another CAN ID (+1)
			msg_.header.sid = SENSE_CAN_ID_2;
			msg_.header.payloadLength = 5;

			// Send it out while on the pad. Otherwise we only log the data.
			sta::tacos::queueCanBusMsg(msg_, 0);

			HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_10);
		}
		else
		{
			HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_9);
		}
		if (logging)
		{
			// Log the ADC values and the sense pins on the fire output
			rres::setSenseFire(sense_fire);
			rres::setSenseADC(result_);

			HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_8);
		}

		adc_.startDMA((uint32_t *)dmaBuffer_, bufferSize_);
	}

} // namespace tasks

// Callback for HAL ADC
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc)
{
	if (tasks::adcCompleteSignal != nullptr && hadc == &hadc1)
	{
		tasks::adcCompleteSignal->notify();
	}
}