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

#include <tasks/fire.hpp>
#include <sta/debug/debug.hpp>
#include "can.h"
#include <sta/tacos.hpp>
#include <sta/config.hpp>
#include <shared.hpp>

#include <cmsis_os2.h>

namespace tasks
{
    FireTask::FireTask() : TacosThread("IGNITOR_FIRE", osPriorityHigh)
    {
    }

    void FireTask::init()
    {
        rres::setFired(0);
    }

    void FireTask::func()
    {

        // Firing the igniters

        CanSysMsg msg;

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

        if (sta::tacos::getState() == stahr::DROGUE)
        {
            // Fire
            HAL_GPIO_WritePin(FIRE_HATCH_GROUP1, FIRE_HATCH_PIN_1, GPIO_PIN_SET);
            HAL_GPIO_WritePin(FIRE_HATCH_GROUP2, FIRE_HATCH_PIN_2, GPIO_PIN_SET);

            // Send out fire information
            msg.payload[0] = 1; // Event 1 = HATCH fired / drogue deployed
            sta::tacos::queueCanBusMsg(msg, 0);

            rres::setFired(1);

            // Only leave on for a second to avoid shorting via the igniter
            this->sleep(1000); // 1 second

            HAL_GPIO_WritePin(FIRE_HATCH_GROUP1, FIRE_HATCH_PIN_1, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(FIRE_HATCH_GROUP2, FIRE_HATCH_PIN_2, GPIO_PIN_RESET);

            this->sleep(50); // Delay before disarming igniters

            // Disarm the igniters
            HAL_GPIO_WritePin(ARMING_GROUPE, ARMING_PIN_E, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(ARMING_GROUPF, ARMING_PIN_F , GPIO_PIN_RESET);

            // Short ignitors
            HAL_GPIO_WritePin(SHORTING_GROUPE, SHORTING_PIN_E, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(SHORTING_GROUPF, SHORTING_PIN_F, GPIO_PIN_RESET);
        }
        else if (sta::tacos::getState() == stahr::MAIN)
        {
            HAL_GPIO_WritePin(FIRE_MAIN_GROUP1, FIRE_MAIN_PIN_1, GPIO_PIN_SET);
            HAL_GPIO_WritePin(FIRE_MAIN_GROUP2, FIRE_MAIN_PIN_2, GPIO_PIN_SET);

            // Send out fire information
            msg.payload[0] = 2; // Event 2 = MAIN Parachute
            sta::tacos::queueCanBusMsg(msg, 0);

            rres::setFired(2);

            // Only leave on for a second to avoid shorting via the igniter
            this->sleep(1000); // 1 second

            HAL_GPIO_WritePin(FIRE_MAIN_GROUP1, FIRE_MAIN_PIN_1, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(FIRE_MAIN_GROUP2, FIRE_MAIN_PIN_2, GPIO_PIN_RESET);

            this->sleep(50); // Delay before disarming igniters

            // Disarm the igniters
            HAL_GPIO_WritePin(ARMING_GROUPC, ARMING_PIN_C, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(ARMING_GROUPD, ARMING_PIN_D, GPIO_PIN_RESET);

            // Short ignitors
            HAL_GPIO_WritePin(SHORTING_GROUPC, SHORTING_PIN_C, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(SHORTING_GROUPD, SHORTING_PIN_D, GPIO_PIN_RESET);
        }
        else if (sta::tacos::getState() == stahr::DISREEFING)
        {
            HAL_GPIO_WritePin(FIRE_DEREEFING_GROUP1, FIRE_DEREEFING_PIN_1, GPIO_PIN_SET);
            HAL_GPIO_WritePin(FIRE_DEREEFING_GROUP2, FIRE_DEREEFING_PIN_2, GPIO_PIN_SET);

            // Send out fire information
            msg.payload[0] = 3; // Event 3 = Drogue Parachute
            sta::tacos::queueCanBusMsg(msg, 0);

            rres::setFired(3);

            // Only leave on for a second to avoid shorting via the igniter
            this->sleep(1000); // 1 second

            HAL_GPIO_WritePin(FIRE_DEREEFING_GROUP1, FIRE_DEREEFING_PIN_1, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(FIRE_DEREEFING_GROUP2, FIRE_DEREEFING_PIN_2, GPIO_PIN_RESET);

            this->sleep(50); // Delay before disarming igniters

            // Disarm the igniters
            HAL_GPIO_WritePin(ARMING_GROUPA, ARMING_PIN_A, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(ARMING_GROUPB, ARMING_PIN_B, GPIO_PIN_RESET);

            // Short ignitors
            HAL_GPIO_WritePin(SHORTING_GROUPA, SHORTING_PIN_A, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(SHORTING_GROUPB, SHORTING_PIN_B, GPIO_PIN_RESET);
        }

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

        this->requestTermination();
    }

} // namespace tasks