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

#include <tasks/arming.hpp>
#include <sta/debug/debug.hpp>
#include "can.h"
#include <sta/tacos.hpp>

#include <cmsis_os2.h>

namespace tasks
{
	ArmingTask::ArmingTask() : TacosThread("ARMING", osPriorityHigh)
	{
	}

	void ArmingTask::init()
	{
		// Stop shorting the igniters (NC so they are always shorted until high signal is sent)
		HAL_GPIO_WritePin(SHORTING_GROUPA, SHORTING_PIN_A, GPIO_PIN_SET);
		HAL_GPIO_WritePin(SHORTING_GROUPB, SHORTING_PIN_B, GPIO_PIN_SET);
		HAL_GPIO_WritePin(SHORTING_GROUPC, SHORTING_PIN_C, GPIO_PIN_SET);
		HAL_GPIO_WritePin(SHORTING_GROUPD, SHORTING_PIN_D, GPIO_PIN_SET);
		HAL_GPIO_WritePin(SHORTING_GROUPE, SHORTING_PIN_E, GPIO_PIN_SET);
		HAL_GPIO_WritePin(SHORTING_GROUPF, SHORTING_PIN_F, GPIO_PIN_SET);

		this->sleep(500); // 500ms delay

		// Arming the igniters
		HAL_GPIO_WritePin(ARMING_GROUPA, ARMING_PIN_A, GPIO_PIN_SET);
		HAL_GPIO_WritePin(ARMING_GROUPB, ARMING_PIN_B, GPIO_PIN_SET);
		HAL_GPIO_WritePin(ARMING_GROUPC, ARMING_PIN_C, GPIO_PIN_SET);
		HAL_GPIO_WritePin(ARMING_GROUPD, ARMING_PIN_D, GPIO_PIN_SET);
		HAL_GPIO_WritePin(ARMING_GROUPE, ARMING_PIN_E, GPIO_PIN_SET);
		HAL_GPIO_WritePin(ARMING_GROUPF, ARMING_PIN_F, GPIO_PIN_SET);

#ifdef STA_TACOS_CAN_BUS_ENABLED
		CanSysMsg msg;

		msg.payload[0] = 1; // Send 1 to indicate that the igniters are armed

		msg.header.payloadLength = 1;
		msg.header.sid = ARMING_CAN_ID;
		msg.header.eid = 0;
		msg.header.format = 0;

		sta::tacos::queueCanBusMsg(msg, 0);
#endif // STA_TACOS_CAN_BUS_ENABLED

// Terminate the thread until next state
#ifdef STA_TACOS_WATCHDOG_ENABLED
		this->watchdogIgnore();
#endif // STA_TACOS_WATCHDOG_ENABLED
	}

	void ArmingTask::func()
	{
		sleep(osWaitForever);
	}

} // namespace tasks