/*
 * statemachine.cpp
 *
 *  Created on: 21 Sep 2023
 *      Author: Dario
 */

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

#include <sta/debug/debug.hpp>
#include <FreeRTOS.h>


namespace sta
{
	namespace tacos
	{
		Statemachine::Statemachine()
			: TacosThread{"Statemachine", STA_TACOS_STATEMACHINE_PRIORITY, STA_TACOS_STATEMACHINE_STACK_SIZE},
			  currentState_{STA_TACOS_INITIAL_STATE},
			  lockoutTimer_{[](void *){}, nullptr},
			  failsafeTimer_{[](void *){}, nullptr},
			  queue_{STA_TACOS_STATEMACHINE_QUEUE_LENGTH},
			  threads_{}
		{
			STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES);
		}

		void Statemachine::init()
		{
			startThreads(getCurrentState());
		}

		void Statemachine::func()
		{
			// Wait for a message to be added to the queue.
			StateTransition transition;
			queue_.get(&transition, osWaitForever);

			// Check if the transition isn't blocked and is legal.
			if (!lockoutTimer_.isRunning() && transition.from == currentState_)
			{
				STA_ASSERT(transition.to < STA_TACOS_NUM_STATES);

#ifdef STA_TACOS_CAN_BUS_ENABLED
				if (transition.publish)
				{
					// Publish the state via CAN bus.
					tacos::publishState(transition.from, transition.to, 0);
				}
#endif // STA_TACOS_CAN_BUS_ENABLED

				// Perform the transition and notify the threads. The event flags are set
				// here in order to allow threads to react immediately.
				currentState_ = transition.to;

				// Send a system-wide notification for the state transition.
				Statemachine::stateChangeEvent.set(transition.event);
				Statemachine::stateChangeEvent.clear(EventFlags::ALL);

				if (failsafeTimer_.isRunning())
				{
					failsafeTimer_.stop();
				}

				// Start the lockout timer if requested.
				if (transition.lockout != 0)
				{
					setLockoutTimer(transition.lockout);
				}

				// get heap stats at the end of the state transition
				HeapStats_t stats;
				vPortGetHeapStats(&stats);

				// Execute the user-defined callback.
				sta::tacos::onStateTransition(transition.from, transition.to, transition.lockout);

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

		uint16_t Statemachine::getCurrentState() const
		{
			return currentState_;
		}

		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool publish /* = true */)
		{
			StateTransition transition;
			transition.from = from;
			transition.to = to;
			transition.event = EventFlags::NORMAL;
			transition.lockout = lockout;
			transition.publish = publish;

			// Try to add a state transition request to the queue. Don't wait if another
			// thread is already requesting a state change.
			queue_.put(transition, 0);
		}

		void Statemachine::forceStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool publish /* = true */)
		{
			// Force the transition, but only if the requested state is different from the current one.
			if (to != currentState_){
				// Perform the transition and notify the threads. The event flags are set
				// here in order to allow threads to react immediately.
				currentState_ = to;

#ifdef STA_TACOS_CAN_BUS_ENABLED
				if (publish)
				{
					// Publish the state via CAN bus.
					tacos::publishState(from, to, 0);
				}
#endif // STA_TACOS_CAN_BUS_ENABLED

				Statemachine::stateChangeEvent.set(EventFlags::NORMAL);
				Statemachine::stateChangeEvent.clear(EventFlags::ALL);

				if (failsafeTimer_.isRunning())
				{
					failsafeTimer_.stop();
				}

				// Start the lockout timer if requested.
				if (lockout != 0)
				{
					setLockoutTimer(lockout);
				}
			}
		}

		void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = true */)
		{
			STA_ASSERT(to < STA_TACOS_NUM_STATES);

			failsafeTimer_.setCallback([from, to, lockout, publish](void* arg) {
				Statemachine::instance()->requestStateTransition(from, to, lockout, publish);
			}, NULL);

			failsafeTimer_.start(millis);
		}

		void Statemachine::setLockoutTimer(uint32_t millis)
		{
			STA_ASSERT(!lockoutTimer_.isRunning());

			lockoutTimer_.setCallback([](void *) {  }, nullptr);
			lockoutTimer_.start(millis);
		}

		void Statemachine::registerThread(std::shared_ptr<TacosThread> thread, std::set<uint16_t> states)
		{
			for (uint16_t state : states)
			{
				STA_ASSERT(state < STA_TACOS_NUM_STATES);

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

		std::vector<std::shared_ptr<TacosThread>> Statemachine::getActiveThreads()
		{
			return threads_[tacos::getState()];
		}

		void Statemachine::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 Statemachine::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 && std::count(threads_[state].begin(), threads_[state].end(), thread) == 0)
					{
						// ...politely request termination.
						thread->requestTermination();
						terminated.emplace(thread);
					}
				}
			}
		}

		void Statemachine::updateThreads()
		{
			uint16_t state = getCurrentState();

			stopThreads(state);
			startThreads(state);
		}

		Statemachine* Statemachine::_instance = nullptr;

		RtosEvent Statemachine::stateChangeEvent;
	}  // namespace tacos
}  // namespace sta