#include "os/os_type.h"
#include "bsp/bsp.h"

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

u32 cpu_counts_abs(void)
{
	return DWT->CYCCNT;
}

static __inline__ u32 cpu_counts_rel(u32 start)
{
	u32 ticks = DWT->CYCCNT;

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

	return 0xFFFFFFFF - start + ticks + 1;
}

static void cpu_counts_delay(u32 ticks)
{
	volatile u32 start = cpu_counts_abs();
	while (cpu_counts_rel(start) < ticks);
}

void cpu_udelay(u32 delay)
{
	cpu_counts_delay(delay * (SystemCoreClock / 1000000));
}


float cpu_count_times(u32 prev) {
	u32 delta = cpu_counts_rel(prev);
	return (float)delta / (float)SystemCoreClock;
}

float cpu_times_bew_counts(u32 now, u32 prev) {
	u32 delta = now - prev;
	if (now < prev) {
		delta = 0xFFFFFFFF - prev + now + 1;
	}
	return (float)delta / (float)SystemCoreClock;
}

u32 cpu_count_delta_us(u32 count) {
	u32 cpu_c = cpu_counts_rel(count);
	float us = (float)cpu_c/120;
	
	return (u32)us;
}