/**@brief Function for updating the Capture Compare register.
 */
static void compare_reg_update(app_timer_id_t timer_id_head_old)
{
    // Setup the timeout for timers on the head of the list 
    if (m_timer_id_head != TIMER_NULL)
    {
        uint32_t ticks_to_expire = mp_nodes[m_timer_id_head].ticks_to_expire;
        uint32_t counter         = rtc1_counter_get();
        uint32_t cc              = m_ticks_latest;
        uint32_t ticks_elapsed   = ticks_diff_get(counter, cc) + RTC_COMPARE_OFFSET_MIN;

        if (timer_id_head_old == TIMER_NULL)
        {
            // No timers were already running, start RTC
            rtc1_start();
        }

        cc += (ticks_elapsed < ticks_to_expire) ? ticks_to_expire : ticks_elapsed;
        cc &= MAX_RTC_CNT;
        
        rtc1_compare0_set(cc);

        if (ticks_diff_get(rtc1_counter_get(), counter) > ticks_diff_get(cc, counter))
        {
            timer_timeouts_check_sched();
        }
    }
    else
    {
        // No timers are running, stop RTC
        rtc1_stop();
    }
}

/**@brief Function for scheduling a Timer Start operation.
 *
 * @param[in]  user_id           Id of user calling this function.
 * @param[in]  timer_id          Id of timer to start.
 * @param[in]  timeout_initial   Time (in ticks) to first timer expiry.
 * @param[in]  timeout_periodic  Time (in ticks) between periodic expiries.
 * @param[in]  p_context         General purpose pointer. Will be passed to the timeout handler when
 *                               the timer expires.
 * @return     NRF_SUCCESS on success, otherwise an error code.
 */
static uint32_t timer_start_op_schedule(timer_user_id_t user_id,
                                        timer_node_t * p_node,
                                        uint32_t        timeout_initial,
                                        uint32_t        timeout_periodic,
                                        void *          p_context)
{
    uint8_t last_index;
    
    timer_user_op_t * p_user_op = user_op_alloc(&mp_users[user_id], &last_index);
    if (p_user_op == NULL)
    {
        return NRF_ERROR_NO_MEM;
    }
    
    p_user_op->op_type                              = TIMER_USER_OP_TYPE_START;
    p_user_op->p_node                               = p_node;
    p_user_op->params.start.ticks_at_start          = rtc1_counter_get();
    p_user_op->params.start.ticks_first_interval    = timeout_initial;
    p_user_op->params.start.ticks_periodic_interval = timeout_periodic;
    p_user_op->params.start.p_context               = p_context;
    
    user_op_enque(&mp_users[user_id], last_index);    

    timer_list_handler_sched();

    return NRF_SUCCESS;
}

static uint32_t timer_start_op_schedule(timer_node_t * p_node,
                                        uint32_t        timeout_initial,
                                        uint32_t        timeout_periodic,
                                        void *          p_context)
{
    uint8_t last_index;
    uint32_t err_code = NRF_SUCCESS;

    CRITICAL_REGION_ENTER();
    timer_user_op_t * p_user_op = user_op_alloc(&last_index);
    if (p_user_op == NULL)
    {
        err_code = NRF_ERROR_NO_MEM;
    }
    else
    {

        p_user_op->op_type                              = TIMER_USER_OP_TYPE_START;
        p_user_op->p_node                               = p_node;
        p_user_op->params.start.ticks_at_start          = rtc1_counter_get();
        p_user_op->params.start.ticks_first_interval    = timeout_initial;
        p_user_op->params.start.ticks_periodic_interval = timeout_periodic;
        p_user_op->params.start.p_context               = p_context;

        user_op_enque(last_index);
    }
    CRITICAL_REGION_EXIT();

    if (err_code == NRF_SUCCESS)
    {
        timer_list_handler_sched();
    }

    return err_code;
}

/**@brief Function for updating the Capture Compare register.
 */
static void compare_reg_update(timer_node_t * p_timer_id_head_old)
{
    // Setup the timeout for timers on the head of the list
    if (mp_timer_id_head != NULL)
    {
        uint32_t ticks_to_expire = mp_timer_id_head->ticks_to_expire;
        uint32_t pre_counter_val = rtc1_counter_get();
        uint32_t cc              = m_ticks_latest;
        uint32_t ticks_elapsed   = ticks_diff_get(pre_counter_val, cc) + RTC_COMPARE_OFFSET_MIN;

        if (!m_rtc1_running)
        {
            // No timers were already running, start RTC
            rtc1_start();
        }

        cc += (ticks_elapsed < ticks_to_expire) ? ticks_to_expire : ticks_elapsed;
        cc &= MAX_RTC_COUNTER_VAL;

        rtc1_compare0_set(cc);

        uint32_t post_counter_val = rtc1_counter_get();

        if (
            (ticks_diff_get(post_counter_val, pre_counter_val) + RTC_COMPARE_OFFSET_MIN)
            >
            ticks_diff_get(cc, pre_counter_val)
           )
        {
            // When this happens the COMPARE event may not be triggered by the RTC.
            // The nRF51 Series User Specification states that if the COUNTER value is N
            // (i.e post_counter_val = N), writing N or N + 1 to a CC register may not trigger a
            // COMPARE event. Hence the RTC interrupt is forcefully pended by calling the following
            // function.
            rtc1_compare0_set(rtc1_counter_get());  // this should prevent CC to fire again in the background while the code is in RTC-ISR
            nrf_delay_us(MAX_RTC_TASKS_DELAY);
            timer_timeouts_check_sched();
        }
    }
    else
    {
#if (APP_TIMER_KEEPS_RTC_ACTIVE == 0)
        // No timers are running, stop RTC
        rtc1_stop();
#endif //(APP_TIMER_KEEPS_RTC_ACTIVE == 0)
    }
}

uint32_t app_timer_init(uint32_t                      prescaler,
                        uint8_t                       op_queues_size,
                        void *                        p_buffer,
                        app_timer_evt_schedule_func_t evt_schedule_func)
{
    int i;

    // Check that buffer is correctly aligned
    if (!is_word_aligned(p_buffer))
    {
        return NRF_ERROR_INVALID_PARAM;
    }
    // Check for NULL buffer
    if (p_buffer == NULL)
    {
        mp_users = NULL;
        return NRF_ERROR_INVALID_PARAM;
    }
    
    // Stop RTC to prevent any running timers from expiring (in case of reinitialization)
    rtc1_stop();
    
    m_evt_schedule_func = evt_schedule_func;
    
    // Initialize users array
    m_user_array_size = APP_TIMER_INT_LEVELS;
    mp_users          = p_buffer;
    
    // Skip user array
    p_buffer = &((uint8_t *)p_buffer)[APP_TIMER_INT_LEVELS * sizeof(timer_user_t)];

    // Initialize operation queues
    for (i = 0; i < APP_TIMER_INT_LEVELS; i++)
    {
        timer_user_t * p_user = &mp_users[i];
        
        p_user->first              = 0;
        p_user->last               = 0;
        p_user->user_op_queue_size = op_queues_size;
        p_user->p_user_op_queue    = p_buffer;
    
        // Skip operation queue
        p_buffer = &((uint8_t *)p_buffer)[op_queues_size * sizeof(timer_user_op_t)];
    }

    mp_timer_id_head             = NULL;
    m_ticks_elapsed_q_read_ind  = 0;
    m_ticks_elapsed_q_write_ind = 0;

    NVIC_ClearPendingIRQ(SWI_IRQn);
    NVIC_SetPriority(SWI_IRQn, SWI_IRQ_PRI);
    NVIC_EnableIRQ(SWI_IRQn);

    rtc1_init(prescaler);

    m_ticks_latest = rtc1_counter_get();
    
    return NRF_SUCCESS;
}

/**@brief Function for updating the Capture Compare register.
 */
static void compare_reg_update(app_timer_id_t timer_id_head_old)
{
    // Setup the timeout for timers on the head of the list 
    if (m_timer_id_head != TIMER_NULL)
    {
        uint32_t ticks_to_expire = mp_nodes[m_timer_id_head].ticks_to_expire;
        uint32_t pre_counter_val = rtc1_counter_get();
        uint32_t cc              = m_ticks_latest;
        uint32_t ticks_elapsed   = ticks_diff_get(pre_counter_val, cc) + RTC_COMPARE_OFFSET_MIN;

        if (timer_id_head_old == TIMER_NULL)
        {
            // No timers were already running, start RTC
            rtc1_start();
        }

        cc += (ticks_elapsed < ticks_to_expire) ? ticks_to_expire : ticks_elapsed;
        cc &= MAX_RTC_COUNTER_VAL;
        
        rtc1_compare0_set(cc);

        uint32_t post_counter_val = rtc1_counter_get();

        if (
            (ticks_diff_get(post_counter_val, pre_counter_val) + RTC_COMPARE_OFFSET_MIN)
            >
            ticks_diff_get(cc, pre_counter_val)
           )
        {
            // When this happens the COMPARE event may not be triggered by the RTC.
            // The nRF51 Series User Specification states that if the COUNTER value is N
            // (i.e post_counter_val = N), writing N or N+1 to a CC register may not trigger a
            // COMPARE event. Hence the RTC interrupt is forcefully pended by calling the following
            // function.
            timer_timeouts_check_sched();
        }
    }
    else
    {
        // No timers are running, stop RTC
        rtc1_stop();
    }
}

uint32_t app_timer_init(uint32_t                      prescaler,
                        uint8_t                       op_queue_size,
                        void *                        p_buffer,
                        app_timer_evt_schedule_func_t evt_schedule_func)
{
    // Check that buffer is correctly aligned
    if (!is_word_aligned(p_buffer))
    {
        return NRF_ERROR_INVALID_PARAM;
    }
    // Check for NULL buffer
    if (p_buffer == NULL)
    {
        return NRF_ERROR_INVALID_PARAM;
    }

    // Stop RTC to prevent any running timers from expiring (in case of reinitialization)
    rtc1_stop();

    m_evt_schedule_func = evt_schedule_func;

    // Initialize operation queue
    m_op_queue.first           = 0;
    m_op_queue.last            = 0;
    m_op_queue.size            = op_queue_size;
    m_op_queue.p_user_op_queue = p_buffer;

    mp_timer_id_head            = NULL;
    m_ticks_elapsed_q_read_ind  = 0;
    m_ticks_elapsed_q_write_ind = 0;

#if APP_TIMER_WITH_PROFILER
    m_max_user_op_queue_utilization   = 0;
#endif

    NVIC_ClearPendingIRQ(SWI_IRQn);
    NVIC_SetPriority(SWI_IRQn, SWI_IRQ_PRI);
    NVIC_EnableIRQ(SWI_IRQn);

    rtc1_init(prescaler);

    m_ticks_latest = rtc1_counter_get();

    return NRF_SUCCESS;
}

/**@brief Function for checking for expired timers.
 */
static void timer_timeouts_check(void)
{
    // Handle expired of timer 
    if (mp_timer_id_head != NULL)
    {
        timer_node_t *  p_timer;
        timer_node_t *  p_previous_timer;
        uint32_t        ticks_elapsed;
        uint32_t        ticks_expired;

        // Initialize actual elapsed ticks being consumed to 0.
        ticks_expired = 0;

        // ticks_elapsed is collected here, job will use it.
        ticks_elapsed = ticks_diff_get(rtc1_counter_get(), m_ticks_latest);

        // Auto variable containing the head of timers expiring.
        p_timer = mp_timer_id_head;

        // Expire all timers within ticks_elapsed and collect ticks_expired.
        while (p_timer != NULL)
        {
            // Do nothing if timer did not expire.
            if (ticks_elapsed < p_timer->ticks_to_expire)
            {
                break;
            }

            // Decrement ticks_elapsed and collect expired ticks.
            ticks_elapsed -= p_timer->ticks_to_expire;
            ticks_expired += p_timer->ticks_to_expire;

            // Move to next timer.
            p_previous_timer = p_timer;
            p_timer = p_timer->next;

            // Execute Task.
            timeout_handler_exec(p_previous_timer);
        }

        // Prepare to queue the ticks expired in the m_ticks_elapsed queue.
        if (m_ticks_elapsed_q_read_ind == m_ticks_elapsed_q_write_ind)
        {
            // The read index of the queue is equal to the write index. This means the new
            // value of ticks_expired should be stored at a new location in the m_ticks_elapsed
            // queue (which is implemented as a double buffer).

            // Check if there will be a queue overflow.
            if (++m_ticks_elapsed_q_write_ind == CONTEXT_QUEUE_SIZE_MAX)
            {
                // There will be a queue overflow. Hence the write index should point to the start
                // of the queue.
                m_ticks_elapsed_q_write_ind = 0;
            }
        }

        // Queue the ticks expired.
        m_ticks_elapsed[m_ticks_elapsed_q_write_ind] = ticks_expired;

        timer_list_handler_sched();
    }
}

uint32_t app_timer_cnt_get(uint32_t * p_ticks)
{
    *p_ticks = rtc1_counter_get();
    return NRF_SUCCESS;
}

uint32_t app_timer_cnt_get(uint64_t * p_ticks)
{
    *p_ticks = overflowBits | rtc1_counter_get();
    return NRF_SUCCESS;
}

uint32_t app_timer_cnt_get(void)
{
    return rtc1_counter_get();
}

/**@brief Function for checking for expired timers.
 */
static void timer_timeouts_check(void)
{
    // Handle expired of timer 
    if (m_timer_id_head != TIMER_NULL)
    {
        uint32_t ticks_expired;
        uint8_t  ticks_elapsed_last;

        // Initialize actual elapsed ticks being consumed to 0 
        ticks_expired = 0;

        // Queue the ticks elapsed (to make the value context safe)
        ticks_elapsed_last = m_ticks_elapsed_last + 1;
        if (ticks_elapsed_last == CONTEXT_QUEUE_SIZE_MAX)
        {
            ticks_elapsed_last = 0;
        }

        if (ticks_elapsed_last != m_ticks_elapsed_first)
        {
            app_timer_id_t timer_id;
            uint32_t       ticks_elapsed;

            // Ticks_elapsed is collected here, job will use it
            ticks_elapsed = ticks_diff_get(rtc1_counter_get(), m_ticks_latest);

            // Auto variable containing the head of timers expiring 
            timer_id = m_timer_id_head;

            // Expire all timers within ticks_elapsed and collect ticks_expired 
            while (timer_id != TIMER_NULL)
            {
                timer_node_t * p_timer;

                // Auto variable for current timer node 
                p_timer = &mp_nodes[timer_id];

                // Do nothing if timer did not expire 
                if (ticks_elapsed < p_timer->ticks_to_expire)
                {
                    break;
                }

                // Decrement ticks_elapsed and collect expired ticks 
                ticks_elapsed -= p_timer->ticks_to_expire;
                ticks_expired += p_timer->ticks_to_expire;

                // Move to next timer 
                timer_id = p_timer->next;

                // Execute Task 
                timeout_handler_exec(p_timer);
            }
        }

        // Queue the elapsed value 
        m_ticks_elapsed[m_ticks_elapsed_last] = ticks_expired;
        m_ticks_elapsed_last                  = ticks_elapsed_last;

        timer_list_handler_sched();
    }
}

uint32_t app_timer_init(uint32_t                      prescaler,
                        uint8_t                       max_timers,
                        uint8_t                       op_queues_size,
                        void *                        p_buffer,
                        app_timer_evt_schedule_func_t evt_schedule_func)
{
    int i;

    // Check that buffer is correctly aligned
    if (!is_word_aligned(p_buffer))
    {
        return NRF_ERROR_INVALID_PARAM;
    }
    // Check for NULL buffer
    if (p_buffer == NULL)
    {
        return NRF_ERROR_INVALID_PARAM;
    }
    
    // Stop RTC to prevent any running timers from expiring (in case of reinitialization)
    rtc1_stop();
    
    m_evt_schedule_func = evt_schedule_func;

    // Initialize timer node array
    m_node_array_size = max_timers;
    mp_nodes          = p_buffer;
    
    for (i = 0; i < max_timers; i++)
    {
        mp_nodes[i].state      = STATE_FREE;
        mp_nodes[i].is_running = false;
    }
    
    // Skip timer node array
    p_buffer = &((uint8_t *)p_buffer)[max_timers * sizeof(timer_node_t)];
    
    // Initialize users array
    m_user_array_size = APP_TIMER_INT_LEVELS;
    mp_users          = p_buffer;
    
    // Skip user array
    p_buffer = &((uint8_t *)p_buffer)[APP_TIMER_INT_LEVELS * sizeof(timer_user_t)];

    // Initialize operation queues
    for (i = 0; i < APP_TIMER_INT_LEVELS; i++)
    {
        timer_user_t * p_user = &mp_users[i];
        
        p_user->first              = 0;
        p_user->last               = 0;
        p_user->user_op_queue_size = op_queues_size;
        p_user->p_user_op_queue    = p_buffer;
    
        // Skip operation queue
        p_buffer = &((uint8_t *)p_buffer)[op_queues_size * sizeof(timer_user_op_t)];
    }

    m_timer_id_head             = TIMER_NULL;
    m_ticks_elapsed_q_read_ind  = 0;
    m_ticks_elapsed_q_write_ind = 0;

		/*
    NVIC_ClearPendingIRQ(SWI0_IRQn);
    NVIC_SetPriority(SWI0_IRQn, SWI0_IRQ_PRI);
    NVIC_EnableIRQ(SWI0_IRQn);
		*/
		
		
		/*--khai-- : temporarily use SWI1 for app_timer so that conflict with event_handler won't occur*/
    NVIC_ClearPendingIRQ(SWI1_IRQn);
    NVIC_SetPriority(SWI1_IRQn, SWI0_IRQ_PRI);
    NVIC_EnableIRQ(SWI1_IRQn);
		
    rtc1_init(prescaler);

    m_ticks_latest = rtc1_counter_get();
    
    return NRF_SUCCESS;
}