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


#include <list>
#include <algorithm>

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

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


#define STATE_CHANGED_MSK 0x01
#define STATE_CHANGED_TIMOUT_MSK 0x02

// The state changed event defined somewhere else.
extern osEventFlagsId_t stateChangeEvent_id;

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


// 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;
};

std::list<tacos_task_t> tasks;



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

	osThreadExit();
}

void dummyStated(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())
				{
					osThreadCreate(thread_def, argument);
				}
			}
		}

		STA_DEBUG_PRINTLN("STARTING NEW TASKS DUE TO STATE CHANGE!");
	}

	osThreadExit();
}