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

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

#include <algorithm>

#include <FreeRTOS.h>


namespace sta
{
	namespace tacos
	{
		void Manager::registerThread(std::shared_ptr<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 (std::shared_ptr<TacosThread> thread : threads_[state])
			{
				if (!thread->isRunning())
				{
					thread->start();
				}
			}
		}

		void Manager::stopThreads(uint16_t state)
		{
			std::set<std::shared_ptr<TacosThread>> terminated;

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

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

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

			stopThreads(state);
			startThreads(state);
		}

		void Manager::init()
		{
			startThreads(Statemachine::instance()->getCurrentState());
		}

		void Manager::func()
		{
			Statemachine::stateChangeEvent.wait(EventFlags::ALL, osWaitForever);

			HeapStats_t stats;
			vPortGetHeapStats(&stats);

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