#include <sta/rtos/system/watchdog.hpp>
#ifdef STA_RTOS_WATCHDOG_ENABLE

#include <sta/assert.hpp>
#include <sta/lang.hpp>
#include <sta/rtos/defs.hpp>
#include <sta/rtos/system/system_events.hpp>

#include <cmsis_os2.h>
#include <FreeRTOS.h>


namespace
{
	StaticTask_t watchdogCB;
	StaticTimer_t watchdogTimerCB;

	osThreadId_t watchdogTaskHandle = nullptr;
	osTimerId_t  watchdogTimerHandle = nullptr;

	// Static stack memory
	uint32_t stackBuffer[256];
}


extern "C"
{
	void watchdogTask(void * arg);
	void watchdogTimerCallback(void *);
}


namespace sta
{
	namespace rtos
	{
		void initWatchdog()
		{
			// Create thread using static allocation
			const osThreadAttr_t taskAttributes = {
				.name = "sysWatchdog",
				.cb_mem = &watchdogCB,
				.cb_size = sizeof(watchdogCB),
				.stack_mem = &stackBuffer[0],
				.stack_size = sizeof(stackBuffer),
				.priority = (osPriority_t) osPriorityLow,
			};

			watchdogTaskHandle = osThreadNew(watchdogTask, NULL, &taskAttributes);
			STA_ASSERT_MSG(watchdogTaskHandle != nullptr, "System watchdog task initialization failed");


			// Create timer using static allocation
			const osTimerAttr_t timerAttributes = {
				.name = "sysWatchdogTimer",
				.attr_bits = 0, // Reserved, must be set to 0
				.cb_mem = &watchdogTimerCB,
				.cb_size = sizeof(watchdogTimerCB)
			};

			watchdogTimerHandle = osTimerNew(watchdogTimerCallback, osTimerPeriodic, nullptr, &timerAttributes);
			STA_ASSERT_MSG(watchdogTimerHandle != nullptr, "System watchdog timer initialization failed");
			osTimerStart(watchdogTimerHandle, STA_RTOS_WATCHDOG_TIMER_PERIOD);
		}


		void notifyWatchdog(uint32_t flags)
		{
			STA_ASSERT_MSG(watchdogTaskHandle != nullptr, "System watchdog not initialized");
			osThreadFlagsSet(watchdogTaskHandle, flags);
		}

		STA_WEAK void watchdogEventHandler(void * arg, uint32_t flags)
		{}
	} // namespace rtos
} // namespace sta



void watchdogTask(void * arg)
{
	sta::rtos::waitForStartupEvent();

	while (true)
	{
		// Wait for any flag to be set
		uint32_t flags = osThreadFlagsWait(STA_RTOS_THREAD_FLAGS_VALID_BITS, osFlagsWaitAny, osWaitForever);

		// Call event handler
		sta::rtos::watchdogEventHandler(arg, flags);
	}
}

void watchdogTimerCallback(void *)
{
	// Notify watchdog task to send heartbeat message
	// Required because blocking in a timer callback is not allowed
	osThreadFlagsSet(watchdogTaskHandle, STA_WATCHDOG_FLAG_HEARTBEAT);
}



#endif // STA_RTOS_WATCHDOG_ENABLE