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


namespace sta {
    RtosTimer::RtosTimer(std::function<void(void*)> callback, void *arg)
    	: timer_id_{NULL},
		  timer_attr_{.name="Timer", .attr_bits=osTimerOnce},
		  callback_{callback},
		  callbackArg_{arg},
		  running_{false}
    {
    	// Pass an anonymous function as the callback which will invoke the currently registered callback.
        timer_id_ = osTimerNew(timeoutHandler, osTimerOnce, this, &timer_attr_);

        STA_ASSERT_MSG(timer_id_ != NULL, "Failed to create timer!");
    }

    RtosTimer::RtosTimer()
    	: RtosTimer([](void *){}, NULL)
    {}

    RtosTimer::~RtosTimer()
    {
    	if (isRunning()) stop();

        osTimerDelete(timer_id_);
    }

    void RtosTimer::setCallback(std::function<void(void*)> callback, void *arg)
    {
    	callback_ = callback;
    	callbackArg_ = arg;
    }

    void RtosTimer::start(uint32_t millis)
    {
        osStatus_t status = osTimerStart(timer_id_, millis);

        if (status != osOK)
        {
        	STA_DEBUG_PRINTLN("Timer start failed");
        	return;
        }

        running_ = true;
    }

    void RtosTimer::stop()
    {
        osStatus_t status = osTimerStop(timer_id_);
        
        if (status != osOK)
		{
        	STA_DEBUG_PRINTLN("Timer stop failed");
        	return;
		}

        running_ = false;
    }

    bool RtosTimer::isRunning()
    {
		return osTimerIsRunning(timer_id_);
	}

    void RtosTimer::timeoutHandler(void* arg)
    {
		RtosTimer* timer = static_cast<RtosTimer*>(arg);
		timer->running_ = false;
		timer->callback_(timer->callbackArg_);
	}

} // namespace sta