/*
 * manager.cpp
 *
 *  Created on: Sep 19, 2023
 *      Author: Dario
 */

#include <sta/tacos/manager.hpp>
#include <sta/tacos/statemachine.hpp>
#include <sta/debug/debug.hpp>


namespace sta
{
	namespace tacos
	{
		void Manager::registerThread(TacosThread thread, std::list<uint16_t> states)
		{
			for (uint16_t state : states)
			{
				STA_ASSERT(state < STA_TACOS_NUM_STATES);

				threads_[state].emplace(thread);
			}
		}

		void Manager::startThreads(uint16_t state)
		{
			STA_ASSERT(state < STA_TACOS_NUM_STATES);

			for (TacosThread thread : threads_[state])
			{
				if (!thread.isRunning())
				{
					thread.start();
				}
			}
		}

		void Manager::stopThreads(uint16_t state)
		{
			uint16_t currentState = Statemachine::instance()->getCurrentState();

			for (uint16_t state = 0; state < STA_TACOS_NUM_STATES; ++state)
			{
				if (state == currentState)
				{
					continue;
				}

				for (TacosThread thread : threads_[state])
				{
					// If the thread is currently running but not part of the set of threads that should be running...
					if (thread.isRunning() && threads_[currentState].count(thread) == 0)
					{
						// ...politely request termination.
						thread.requestTermination();
					}
				}
			}
		}

		void Manager::updateThreads()
		{
			uint16_t state = Statemachine::instance()->getCurrentState();

			startThreads(state);
			stopThreads(state);
		}

		void Manager::init()
		{
			STA_DEBUG_PRINTLN("INITIALIZING MANAGER!");

			startThreads(Statemachine::instance()->getCurrentState());
		}

		void Manager::func()
		{
			if (true)
			{
				// STA_DEBUG_PRINTLN("LOOPY LOOP IN MANAGER TASK");
				osDelay(1000);
			}


			// Wait for either the termination request or the state change flag.
			uint32_t flags = osEventFlagsWait(getInstance(), STA_RTOS_THREAD_FLAG_TERMINATE, osFlagsWaitAny, osWaitForever);

			if ((flags & STA_RTOS_THREAD_FLAG_TERMINATE) != 0)
			{
				// The loop implemented by the TacosThread class should handle termination.
				return;
			}

			// Start all new tasks and stop all the tasks that aren't supposed to be running.
			updateThreads();
		}

		Manager::Manager()
			: TacosThread{"Manager", STA_TACOS_MANAGER_PRIORITY}
		{

		}

		//Manager::~Manager(){}

		Manager* Manager::_instance = nullptr;

	}  // namespace tacos
}  // namespace sta