#include <sta/tacos/watchdog.hpp>

#ifdef STA_TACOS_WATCHDOG_ENABLED

#include <sta/tacos/statemachine.hpp>

namespace sta
{
    namespace tacos
    {
		void Watchdog::func()
		{
			for (std::shared_ptr<TacosThread> thread : Statemachine::instance()->getActiveThreads())
			{
				switch (thread->getStatus())
				{
				case ThreadStatus::UNKNOWN:
					// Restart the thread.
					thread->kill();
					thread->start();
					
					restarts_++;
					break;
				case ThreadStatus::RUNNING:
					// Set the thread's status back to UNKNOWN.
					thread->resetStatus();
					break;
				default:
					break;
				}
			}
			sleep(STA_TACOS_WATCHDOG_FREQUENCY);
		}

		uint16_t Watchdog::getNumRestarts()
		{
			return restarts_;
		}

		Watchdog::Watchdog()
			: TacosThread{"Watchdog", STA_TACOS_WATCHDOG_PRIORITY},
			  restarts_{ 0 }
		{}

		Watchdog* Watchdog::_instance = nullptr;
    } // namespace tacos
} // namespace sta

#endif // STA_TACOS_WATCHDOG_ENABLED