#include "bsp/bsp_driver.h"
#include "os/os_task.h"
#include "libs/time_measure.h"

#define COUNT_2_US(c) (c/(SYSTEM_CLOCK/1000000))

u32 time_delta_us(u32 count, u32 *p_update) {
	u32 now = task_ticks_abs();
	u32 delta = now - count;
	if (now < count) { //wrap
		delta = 0xFFFFFFFF - count + now + 1;
	}
	if (p_update) {
		*p_update = now;
	}
	return COUNT_2_US(delta);
}


void time_measure_start(measure_time_t *m){
	m->intval_time = time_delta_us(m->intval_count, &m->intval_count);
	m->exec_count = task_ticks_abs();
	if (m->first) {
		m->first = false;
		return;
	}
	if (m->intval_time > m->intval_max_time+5) {
		m->intval_time_h_error ++;
		m->intval_hi_err = m->intval_time;
	}
	if (m->intval_time < m->intval_max_time-5) {
		m->intval_time_l_error ++;
		m->intval_low_err = m->intval_time;
	}
}
void time_measure_end(measure_time_t *m) {
	if (m->first) {
		return;
	}
	m->exec_time = time_delta_us(m->exec_count, NULL);
	if (m->exec_time > m->exec_max_time) {
		m->exec_time_error ++;
		m->exec_max_error_time = m->exec_time;
	}
}