/*
 * thread.cpp
 *
 *  Created on: Sep 14, 2023
 *      Author: Dario
 */


#include <sta/tacos/thread.hpp>
#include <sta/tacos/system/events.hpp>

#include <sta/debug/assert.hpp>
#include <sta/debug/debug.hpp>

#include <functional>
#include <cstring>
#include <cmsis_os2.h>

namespace sta
{
	namespace tacos
	{
		TacosThread::TacosThread(const char* name, osPriority_t prio, uint32_t stack_size /* = 0 */, uint32_t cb_size /* = 0 */)
				: RtosThread(name, prio, stack_size, cb_size),
				  instance_{ NULL },
				  attribs_{ .name=name, .cb_size=cb_size, .stack_size=stack_size, .priority=prio },
				  running_{false},
#ifdef STA_TACOS_WATCHDOG_ENABLED
				  status_{ThreadStatus::STOPPED},
#endif // STA_TACOS_WATCHDOG_ENABLED
#ifdef STA_CAN_BUS_FWD_ENABLE
				  CAN_queue_{STA_RTOS_CAN_BUS_QUEUE_LENGTH},
				  canID_{0},
#endif // STA_CAN_BUS_FWD_ENABLE
				  terminate_{false},
				  previous_tick_{0}
		{
			STA_ASSERT(stack_size >= 0);
			STA_ASSERT(cb_size >= 0);
		}

		bool TacosThread::isRunning()
		{
			return running_;
		}

		void TacosThread::start()
		{
			if (getInstance() == NULL)
			{
				RtosThread::start();
			}
			else
			{
				sysNotify(STA_RTOS_THREAD_FLAG_START);
			}
		}

		void TacosThread::init() {
#ifdef STA_TACOS_DIAGNOSTICS_ENABLED
			STA_DEBUG_PRINTF("Started %s", getName());
#endif //STA_TACOS_DIAGNOSTICS_ENABLED
		}

		void TacosThread::loop()
		{
			// Infinite loop allowing quick restarts of this thread after termination.
			while (true)
			{
				// The thread has to wait until the system startup has been completed.
				sta::tacos::waitForStartupEvent();

				// Wait for a thread start flag.
				wait(STA_RTOS_THREAD_FLAG_START);

				// Call user-defined initialization code.
				running_ = true;
				init();

				// Run the thread until the termination flag is set. Reset this
				while (!isTerminationRequested())
				{
#ifdef STA_TACOS_WATCHDOG_ENABLED
					if (status_ == ThreadStatus::UNKNOWN)
					{
						// Send a fresh heartbeat signal.
						heartbeat();
					}
#endif // STA_TACOS_WATCHDOG_ENABLED

					// Execute user-space implementation.
					func();
				}
#ifdef STA_TACOS_WATCHDOG_ENABLED
				status_ = ThreadStatus::STOPPED;
#endif // STA_TACOS_WATCHDOG_ENABLED

				// Clear the termination request flag for this thread.
				deleteTerminationRequest();

				// Call user-defined cleanup code.
				cleanup();
				running_ = false;
			}

			osThreadExit();
		}

		void TacosThread::cleanup() {
#ifdef STA_TACOS_DIAGNOSTICS_ENABLED
			STA_DEBUG_PRINTF("Stopped %s", getName());
#endif
		}

		void TacosThread::sleep(uint32_t ticks)
		{
#ifdef STA_TACOS_WATCHDOG_ENABLED
			waiting();
#endif // STA_TACOS_WATCHDOG_ENABLED

			osDelay(ticks);

#ifdef STA_TACOS_WATCHDOG_ENABLED
			heartbeat();
#endif // STA_TACOS_WATCHDOG_ENABLED
		}

		void TacosThread::periodicDelay(uint32_t frequency){
			if (previous_tick_ == 0)
			{
				previous_tick_ = osKernelGetTickCount();
			}

			previous_tick_ += osKernelGetTickFreq()/frequency; 

#ifdef STA_TACOS_WATCHDOG_ENABLED
			waiting();
#endif // STA_TACOS_WATCHDOG_ENABLED

			osDelayUntil(previous_tick_); 	

#ifdef STA_TACOS_WATCHDOG_ENABLED
			heartbeat();
#endif // STA_TACOS_WATCHDOG_ENABLED
		}

#ifdef STA_TACOS_WATCHDOG_ENABLED
		void TacosThread::heartbeat()
		{
			status_ = ThreadStatus::RUNNING;
		}

		void TacosThread::waiting()
		{
			status_ = ThreadStatus::WAITING;
		}

		ThreadStatus TacosThread::getStatus()
		{
			return status_;
		}

		void TacosThread::resetStatus()
		{
			status_ = ThreadStatus::UNKNOWN;
		}

		void TacosThread::watchdogIgnore()
		{
			status_ = ThreadStatus::IGNORED;
		}
#endif // STA_TACOS_WATCHDOG_ENABLED

		void TacosThread::requestTermination()
		{
			terminate_ = true;
		}

		void TacosThread::deleteTerminationRequest()
		{
			terminate_ = false;
		}

		bool TacosThread::isTerminationRequested()
		{
			return terminate_;
		}

		bool TacosThread::operator==(const TacosThread& other) const
		{
			return std::strcmp(this->getName(), other.getName()) == 0;
		}

		bool TacosThread::operator<(const TacosThread& other) const
		{
			return std::strcmp(this->getName(), other.getName()) < 0;
		}

		TacosThread::~TacosThread(){}

#ifdef STA_CAN_BUS_FWD_ENABLE
		void TacosThread::setCanID(uint32_t canID){
			canID_ = canID;
		}

		uint32_t TacosThread::getCanID(){
			return canID_;
		}
#endif // STA_CAN_BUS_FWD_ENABLE

	}  // namespace tacos
}  // namespace sta