#include "wsf_queue.h"
#include "wsf_timer.h"
#include "mk_common.h"
#include "mk_misc.h"
// #include "mk_trace.h"

/**************************************************************************************************
  Global Variables
**************************************************************************************************/

static wsfQueue_t wsfTimerTimerQueue; /*!< Timer queue */

/*************************************************************************************************/
/*!
 *  \brief  Remove a timer from queue.  Note this function does not lock task scheduling.
 *
 *  \param  pTimer  Pointer to timer.
 */
/*************************************************************************************************/
static void wsfTimerRemove(wsfTimer_t *pTimer)
{
    wsfTimer_t *pElem;
    wsfTimer_t *pPrev = NULL;

    pElem = (wsfTimer_t *)wsfTimerTimerQueue.pHead;

    /* find timer in queue */
    while (pElem != NULL)
    {
        if (pElem == pTimer)
        {
            break;
        }
        pPrev = pElem;
        pElem = pElem->pNext;
    }

    /* if timer found, remove from queue */
    if (pElem != NULL)
    {
        WsfQueueRemove(&wsfTimerTimerQueue, pTimer, pPrev);

        pTimer->isStarted = FALSE;
    }
}

/*************************************************************************************************/
/*!
 *  \brief  Insert a timer into the queue sorted by the timer expiration.
 *
 *  \param  pTimer  Pointer to timer.
 *  \param  ticks   Timer ticks until expiration.
 *  \param  mode    Timer work mode.
 */
/*************************************************************************************************/
static void wsfTimerInsert(wsfTimer_t *pTimer, uint32_t ticks, uint8_t mode)
{
    wsfTimer_t *pElem;
    wsfTimer_t *pPrev = NULL;

    uint32_t lock = int_lock();

    /* if timer is already running stop it first */
    if (pTimer->isStarted)
    {
        wsfTimerRemove(pTimer);
    }

    pTimer->isStarted = TRUE;
    pTimer->ticks = ticks;
    pTimer->count = ticks;
    pTimer->mode = mode;

    pElem = (wsfTimer_t *)wsfTimerTimerQueue.pHead;

    /* find insertion point in queue */
    while (pElem != NULL)
    {
        if (pTimer->ticks < pElem->ticks)
        {
            break;
        }
        pPrev = pElem;
        pElem = pElem->pNext;
    }

    WsfQueueInsert(&wsfTimerTimerQueue, pTimer, pPrev);
    int_unlock(lock);
}

/*************************************************************************************************/
/*!
 *  \brief  Return the number of ticks until the next timer expiration.  Note that this
 *          function can return zero even if a timer is running, indicating a timer
 *          has expired but has not yet been serviced.
 *
 *  \return The number of ticks until the next timer expiration.
 */
/*************************************************************************************************/
uint32_t WsfTimerNextExpiration(void)
{
    uint32_t ticks;
    uint32_t lock = int_lock();

    if (wsfTimerTimerQueue.pHead == NULL)
    {
        ticks = UINT32_MAX;
    }
    else
    {
        ticks = ((wsfTimer_t *)wsfTimerTimerQueue.pHead)->ticks;
    }

    int_unlock(lock);
    return ticks;
}

/*************************************************************************************************/
/*!
 *  \brief  Initialize the timer service.  This function should only be called once
 *          upon system initialization.
 */
/*************************************************************************************************/
void WsfTimerInit(void)
{
    WSF_QUEUE_INIT(&wsfTimerTimerQueue);
}

/*************************************************************************************************/
/*!
 *  \brief  Start a timer in units of 32768Hz ticks.
 *
 *  \param  pTimer  Pointer to timer.
 *  \param  ticks   32768Hz ticks until expiration.
 *  \param  mode    Timer work mode.
 */
/*************************************************************************************************/
void WsfTimerStartTick(wsfTimer_t *pTimer, uint32_t ticks, uint8_t mode)
{
    wsfTimerInsert(pTimer, ticks, mode);
}

/*************************************************************************************************/
/*!
 *  \brief  Start a timer in units of milliseconds.
 *
 *  \param  pTimer  Pointer to timer.
 *  \param  ms     Milliseconds until expiration.
 *  \param  mode    Timer work mode.
 */
/*************************************************************************************************/
void WsfTimerStartMs(wsfTimer_t *pTimer, uint32_t ms, uint8_t mode)
{
    wsfTimerInsert(pTimer, __MS_TO_32K_CNT(ms), mode);
}

/*************************************************************************************************/
/*!
 *  \brief  Stop a timer.
 *
 *  \param  pTimer  Pointer to timer.
 */
/*************************************************************************************************/
void WsfTimerStop(wsfTimer_t *pTimer)
{
    uint32_t lock = int_lock();

    wsfTimerRemove(pTimer);

    int_unlock(lock);
}

/*************************************************************************************************/
/*!
 *  \brief  Update the timer service with the number of elapsed ticks.
 *
 *  \param  ticks  Number of ticks since last update.
 */
/*************************************************************************************************/
void WsfTimerUpdate(uint32_t ticks)
{
    wsfTimer_t *pElem;

    uint32_t lock = int_lock();

    pElem = (wsfTimer_t *)wsfTimerTimerQueue.pHead;

    /* iterate over timer queue */
    while (pElem != NULL)
    {
        /* decrement ticks while preventing underflow */
        if (pElem->ticks > ticks)
        {
            pElem->ticks -= ticks;
        }
        else
        {
            pElem->ticks = 0;
        }

        if (pElem->ticks < POWER_DOWN_TIME_TICK_MIN)
        {
            pElem->ticks = 0;
            /* timer expired; set task for this timer as ready */
            WsfTaskSetReady(pElem->handlerId, WSF_TIMER_EVENT);
        }

        // LOG_INFO(TRACE_MODULE_OS, "WsfTimerUpdate  %u %u %u %u\r\n", pElem->handlerId, pElem->msg.event, pElem->ticks, ticks);

        pElem = pElem->pNext;
    }

    int_unlock(lock);
}

/*************************************************************************************************/
/*!
 *  \brief  Service expired timers for the given task.
 *
 *  \return Pointer to timer or NULL.
 */
/*************************************************************************************************/
wsfTimer_t *WsfTimerServiceExpired(void)
{
    wsfTimer_t *pElem;
    wsfTimer_t *pPrev = NULL;

    uint32_t lock = int_lock();

    /* find expired timers in queue */
    pElem = (wsfTimer_t *)wsfTimerTimerQueue.pHead;
    if ((pElem != NULL) && (pElem->ticks == 0))
    {
        if (pElem->mode == WSF_TIMER_PERIODIC)
        {
            wsfTimerInsert(pElem, pElem->count, WSF_TIMER_PERIODIC);
        }
        else
        {
            WsfQueueRemove(&wsfTimerTimerQueue, pElem, pPrev);
            pElem->isStarted = FALSE;
        }

        int_unlock(lock);
        return pElem;
    }

    int_unlock(lock);
    return NULL;
}