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

#include <sta/tacos/statemachine.hpp>

#include <sta/debug/debug.hpp>


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}
		{
			STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES);
		}

		void Statemachine::init()
		{

		}

		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);

				// Perform the transition and notify the threads. The event flags are set
				// here in order to allow threads to react immediately.
				currentState_ = transition.to;
				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);
				}
			}
		}

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

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

			// Force the transition if requested, but only if the requested state is different from the current one.
			if (force && transition.to != currentState_){
				// Perform the transition and notify the threads. The event flags are set
				// here in order to allow threads to react immediately.
				currentState_ = transition.to;
				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);
				}
			}else{
				// 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::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
		{
			STA_ASSERT(to < STA_TACOS_NUM_STATES);

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

			failsafeTimer_.start(millis);
		}

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

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

		Statemachine* Statemachine::_instance = nullptr;

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