/*
 * statemachine.hpp
 *
 *  Created on: Sep 14, 2023
 *      Author: Dario
 */

#ifndef INCLUDE_TACOS_STATEMACHINE_HPP_
#define INCLUDE_TACOS_STATEMACHINE_HPP_

#include <sta/config.hpp>

#if !defined(STA_TACOS_NUM_STATES)  && !defined(DOXYGEN)
#	error "Number of states wasn't defined in config.hpp"
#else

#if !defined(STA_TACOS_INITIAL_STATE)  && !defined(DOXYGEN)
#	define STA_TACOS_INITIAL_STATE 0
#endif

/**
 * @defgroup tacos_statemachine Statemachine Task
 * @ingroup tacos
 * @brief Statemachine task for TACOS.
 */

/**
 * @brief The maximum number of state transitions that can be queued.
 * 
 * @ingroup tacos_statemachine
 */
#define STA_TACOS_STATEMACHINE_QUEUE_LENGTH		4

/**
 * @ingroup tacos_statemachine
 * @{
 */

// State transition happened because of
/**
 * @brief State change due to requested Transition.
*/
#define STA_TACOS_STATE_CHANGE_NORMAL_FLAG		( 0x1U )
/**
 * @brief State change due to forced Transition.
*/
#define STA_TACOS_STATE_CHANGE_FORCED_FLAG		( 0x1U << 1)
/**
 * @brief State change due to timeout.
*/
#define STA_TACOS_STATE_CHANGE_TIMEOUT			( 0x1U << 2)
/**
 * @brief State change due to startup.
*/
#define STA_TACOS_STATE_CHANGE_STARTUP_FLAG			( 0x1U << 3)
/**
 * @brief State change due to any reason.
*/
#define STA_TACOS_STATE_CHANGE_ALL_FLAG			( 0x15U )

/** @} */

#include <sta/tacos/thread.hpp>
#include <sta/rtos/queue.hpp>
#include <sta/rtos/timer.hpp>
#include <sta/rtos/event.hpp>
#include <sta/event.hpp>
#include <sta/debug/assert.hpp>
#include <sta/lang.hpp>

#include <functional>
#include <tuple>
#include <list>
#include <vector>
#include <memory>
#include <set>
#include <algorithm>


/**
 * @brief Addresses all states from 0 to STA_TACOS_NUM_STATES-1.
 *
 * @ingroup tacos_statemachine
 */
#define ALL_STATES std::set<uint16_t>{[]{ std::set<uint16_t> states; for (uint16_t i = 0; i < STA_TACOS_NUM_STATES; ++i) states.insert(i); return states; }() }

/**
 * @brief Shorthand to make own sets of states. Primarily for exlcusion via "-" operator.
 *
 * @ingroup tacos_statemachine
 */
#define state_set std::set<uint16_t>

namespace sta
{
	namespace tacos
	{
		/**
		 * @brief Event flags for state transitions.
		 *
		 * @ingroup tacos_statemachine
		 */
		enum EventFlags
		{
			NORMAL 				= 0x1U,
			FORCED				= 0x1U << 1,
			TIMEOUT				= 0x1U << 2,
			STARTUP				= 0x1U << 3,
			ALL					= NORMAL | FORCED | TIMEOUT | STARTUP
		};

		/**
		 * @brief State transition structure.
		 *
		 * @ingroup tacos_statemachine
		 */
		struct StateTransition
		{
			/// Origin of transition
			uint16_t from; 		
			/// Destination of transition	
			uint16_t to; 			
			/// Event that triggered the transition
			EventFlags event; 	
			/// Lockout time after transition	
			uint32_t lockout;
			/// Whether to publish the transition via CAN.
			bool publish = false;
		};

		/**
		 * @brief Statemachine implementation for Tacos.
		 *
		 * @ingroup tacos_statemachine
		 */
		class Statemachine : public TacosThread
		{
		public:
			/**
			 * @brief The global event signaling a state change.
			 * 
			 * @ingroup tacos_statemachine
			 */
			static RtosEvent stateChangeEvent;

			/**
			 * @brief Getter function for the singleton instance.
			 * 
			 * @ingroup tacos_statemachine
			 */
			static Statemachine* instance()
			{
				static CGuard g;

				if (!_instance)
				{
					// Create the statemachine singleton instance.
					Statemachine::_instance = new Statemachine();
				}

				return _instance;
			}

			/**
			 * @brief Returns the statemachine's current state.
			 * 
			 * @ingroup tacos_statemachine
			 */
			uint16_t getCurrentState() const;

			/**
			 * @brief Request a state transition from a state to another.
			 *
			 * @param from The state which we want to leave. This is used to filter out obsolete transitions.
			 * @param to The state to transition to.
			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
			 * @param publish If true, the state transition will be published via CAN.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool publish = true);

			/**
			 * @brief Request a state transition from a state to another.
			 *
			 * @param from The state which we want to leave. This is used to filter out obsolete transitions.
			 * @param to The state to transition to.
			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
			 * @param publish If true, the state transition will be published via CAN.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void forceStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool publish = true);

			/**
			 * @brief Request a state transition after a given time has passed.
			 *
			 * @param from The state which we want to leave. This is used to filter out obsolete transitions.
			 * @param to The state to transition to.
			 * @param millis the number of milliseconds to wait before triggering the transition.
			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
			 * @param publish If true, the state transition will be published via CAN.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = true);

			/**
			 * @brief Register a thread to be managed by the statemachine.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void registerThread(std::shared_ptr<TacosThread> thread, std::set<uint16_t> states);

			/**
			 * @brief Get the Active Threads object
			 * 
			 * @return std::vector<std::shared_ptr<TacosThread>> 
			 * 
			 * @ingroup tacos_statemachine
			 */
			std::vector<std::shared_ptr<TacosThread>> getActiveThreads();

			void init() override;
			void func() override;

		private:
			static Statemachine * _instance;

			class CGuard
			{
			public:
			   ~CGuard()
			   {
				  if( NULL != Statemachine::_instance )
				  {
					 delete Statemachine::_instance;
					 Statemachine::_instance = NULL;
				  }
			   }
			};

			Statemachine();

			Statemachine(const Statemachine&);

			~Statemachine() {}

		private:
			/**
			 * @brief Starts the lockoutTimer for the desired duration.
			 * 
			 * @param millis The duration of the timer in milliseconds.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void setLockoutTimer(uint32_t millis);

			/**
			 * @brief Forces only threads of current state to run.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void updateThreads();

			/**
			 * @brief Starts all threads which should be running in the given state. Does nothing if the state is already running.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void startThreads(uint16_t state);

			/**
			 * @brief Stops all threads which should not be running in the given state.
			 * 
			 * @ingroup tacos_statemachine
			 */
			void stopThreads(uint16_t state);

		private:
			uint16_t currentState_;

			RtosTimer lockoutTimer_;
			RtosTimer failsafeTimer_;

			RtosQueue<StateTransition> queue_;

			/**
			 * @brief Pointers to all threads which are managed by the statemachine.
			 *
			 * @ingroup tacos_statemachine
			 */
			std::vector<std::shared_ptr<TacosThread>> threads_[STA_TACOS_NUM_STATES];
		};

		/**
		 * @brief Callback that is called when a state transition occurs.
		 * 
		 * @param from The state we transitioned from.
		 * @param to The state we transitioned to.
		 * @param lockout The lockout time after the transition.
		 * 
		 * @ingroup tacos_statemachine
		 */
		STA_WEAK
		void onStateTransition(uint16_t from, uint16_t to, uint32_t lockout){}
	}  // namespace tacos
}  // namespace sta

#endif // STA_TACOS_NUM_STATES

/// Overwrite "-" operator for set.
template <typename T>
std::set<T> operator-(const std::set<T>& set1, const std::set<T>& set2) {
    std::set<T> result;
    std::set_difference(set1.begin(), set1.end(), set2.begin(), set2.end(), std::inserter(result, result.begin()));
    return result;
}

#endif /* INCLUDE_TACOS_STATEMACHINE_HPP_ */