/*
 * manager.cpp
 *
 *  Created on: Aug 30, 2023
 *      Author: Dario
 */


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

#include <list>
#include <algorithm>

#include <cmsis_os2.h>
#include <FreeRTOS.h>


#include <sta/rtos/system/events.hpp>

#include <tasks/statemachine.hpp>


namespace tacos
{

	// The data structure representing a TACOS task.
	struct tacos_task_t
	{
		// The code to be executed for this task.
		osThreadFunc_t func;

		// The attributes for the task.
		osThreadAttr_t attribs;

		// A list of currently running instances of this task.
		std::list<osThreadId_t> running;

		// A list of states for which this task should be running.
		std::list<tacos_states_t> states;
	};


} // namespace tacos


// The current state defined somewhere else.
extern tacos_states_t currentState;


extern "C"
{
	void managerTask(void *)
	{
		STA_DEBUG_PRINTLN("INITIALIZED MANAGER TASK");

		while (true)
		{
			// Wait until the state machine triggers an event signaling a state-change.
			uint32_t flags = osEventFlagsWait(stateChangeEvent_id, STATE_CHANGED_MSK, osFlagsWaitAll, osWaitForever);


		}
	}
}








std::list<tacos_task_t> tasks;



void dummyInit(void *)
{
	while (true)
	{
		STA_DEBUG_PRINTLN("INIT STATE");
	}

	osThreadExit();
}

void dummyStarted(void *)
{
	while (true)
	{
		STA_DEBUG_PRINTLN("STARTED STATE");
	}

	osThreadExit();
}

void dummyFlying(void *)
{
	while (true)
	{
		STA_DEBUG_PRINTLN("FLYING STATE");
	}

	osThreadExit();
}

void dummyLanded(void *)
{
	while (true)
	{
		STA_DEBUG_PRINTLN("LANDED STATE");
	}

	osThreadExit();
}


extern "C" void startManagerTask(void *)
{
	STA_DEBUG_PRINTLN("INITIALIZED MANAGER TASK");

	while (true)
	{
		// Wait until the state machine triggers an event signaling a state-change.
		osEventFlagsWait(stateChangeEvent_id, STATE_CHANGED_MSK, osFlagsWaitAll, osWaitForever);

		for (tacos_task_t task : tasks)
		{
			// Check if this task is supposed to be running for this state. If not, kill all instances, else create the desired number of instances.
			if (std::find(task.states.begin(), task.states.end(), currentState) == task.states.end())
			{
				for (osThreadId_t instance : task.running)
				{
					osThreadTerminate(instance);
				}
			}
			else
			{
				if (!task.running.empty())
				{
					osThreadDef (task.attribs.name, task.attribs.priority, 1, 0);

					osThreadCreate(osTh, argument);
				}
			}
		}
	}

	osThreadExit();
}