#include "bsp/bsp.h"
#include "bsp/delay.h"
#include "os/os_types.h"

void task_ticks_enable(void)
{
	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}

uint32_t task_ticks_abs(void)
{
	return DWT->CYCCNT;
}

uint32_t task_ticks_rel(uint32_t start)
{
	uint32_t ticks = DWT->CYCCNT;

	if (ticks >= start) {
		return ticks - start;
	}

	return 0xFFFFFFFF - start + ticks + 1;
}

void task_ticks_delay(uint32_t ticks)
{
	uint32_t start;

	start = task_ticks_abs();
	while (task_ticks_rel(start) < ticks);
}

void task_udelay(uint32_t delay)
{
	task_ticks_delay(delay * (SystemCoreClock / 1000000));
}


uint32_t task_get_usecond(void) {
	return task_ticks_abs() / (SYSTEM_CLOCK / 1000000);
}

uint32_t task_get_delta_us(uint32_t us) {
	u32 count = us * (SYSTEM_CLOCK / 1000000);
	return task_delta_ticks(count) / (SYSTEM_CLOCK / 1000000);
}


uint32_t task_delta_ticks(u32 count) {
	u32 now = task_ticks_abs();
	u32 delta = now - count;
	if (now < count) { //wrap
		delta = 0xFFFFFFFF - count + now + 1;
	}
	return (delta);
}

void delay_us(uint16_t cnt)
{
	task_udelay(cnt);
}

/*!
    \brief      delay a time in milliseconds
    \param[in]  count: count in milliseconds
    \param[out] none
    \retval     none
*/
void delay_ms(uint32_t count)
{
	task_udelay(1000 * count);
}