/*
 * state_machine.cpp
 *
 *  Created on: Sep 4, 2023
 *      Author: Dario
 */

#include <cmsis_os2.h>

#include <tasks/statemachine.hpp>
#include <sta/debug/assert.hpp>


tacos_states_t currentState;

osTimerId_t lockout;
osTimerId_t failsafe;


void lockoutCallback(void* arg)
{

}

void failsafeCallback(void* arg)
{
	changeState(TACOS_STATE_CHG_TIMEOUT);
}



bool checkStateChangeConditions()
{
	// Only use the timers in this demo.
	return false;
}


void changeState(uint8_t flag)
{
	// Stop the old timers.
	osTimerStop(lockout);
	osTimerStop(failsafe);

	// Set the event flags to signal other tasks that a state change has occured. Reset the flags immediately.
	osEventFlagsSet(stateChangeEvent_id, flag);
	osEventFlagsClear(stateChangeEvent_id, TACOS_STATE_CHG_ALL);

	uint8_t lockoutCycles = 0;
	uint8_t failsafeCycles = 0;

	switch (currentState) {
		case tacos_states_t::init:
			currentState = tacos_states_t::started;

			lockoutCycles = 5000;
			failsafeCycles = 6000;

		case tacos_states_t::started:
			currentState = tacos_states_t::flying;

			lockoutCycles = 1000;
			failsafeCycles = 2000;

		case tacos_states_t::flying:
			currentState = tacos_states_t::landed;

			lockoutCycles = 5000;
			failsafeCycles = 6000;

			break;
		default:
			break;
	}

	// Restart the timers.
	osTimerStart(lockout, lockoutCycles);
	osTimerStart(failsafe, failsafeCycles);
}


extern "C" void startStateMachine(void*)
{
	// Initialize the stateChange event.
	stateChangeEvent_id = osEventFlagsNew(NULL);
	STA_ASSERT_MSG(stateChangeEvent_id != NULL, "Failed to initialize state change event!");

	// The timers for catching errors.
	lockout = osTimerNew(lockoutCallback, osTimerOnce, NULL, NULL);
	failsafe = osTimerNew(failsafeCallback, osTimerOnce, NULL, NULL);

	// Check if the initialization of the timers was successful.
	STA_ASSERT_MSG(lockout != 0, "Failed to initialize lockout timer");
	STA_ASSERT_MSG(failsafe != 0, "Failed to initialize failsafe timer");

	while (true)
	{
		if (checkStateChangeConditions())
		{
			changeState(TACOS_STATE_CHG_NATURAL);
		}
	}

	osThreadExit();
}