#include "bsp/bsp.h"
#include "timer_count32.h"
#include "os/co_task.h"
static void init_master_timer(void);
static void init_slave_timer(void);

#define MASTER TIMER1
#define MASTER_CK RCU_TIMER1

#define SLAVE  TIMER2
#define SLAVE_CK RCU_TIMER2

void timer_count32_init(void) {

	init_master_timer();
	init_slave_timer();

	timer_enable(MASTER);
	timer_enable(SLAVE);
}

static void init_master_timer(void){
	timer_parameter_struct timer_initpara;
	
	rcu_periph_clock_enable(MASTER_CK);

	timer_deinit(MASTER);

	memset(&timer_initpara, 0, sizeof(timer_initpara));
	timer_initpara.prescaler		= TIM_CLOCK/COUNT_CLK - 1; //clk 10MHz, 100ns/0.1us
	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
	timer_initpara.counterdirection = TIMER_COUNTER_UP;
	timer_initpara.period		   = 0xFFFF;
	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
	timer_initpara.repetitioncounter = 0;
	timer_init(MASTER,&timer_initpara);
	timer_counter_value_config(MASTER, 0);
	timer_autoreload_value_config(MASTER, 0xFFFF);
	timer_counter_up_direction(MASTER);
	timer_auto_reload_shadow_enable(MASTER);
	timer_master_slave_mode_config(MASTER, TIMER_MASTER_SLAVE_MODE_ENABLE);
	timer_master_output_trigger_source_select(MASTER, TIMER_TRI_OUT_SRC_UPDATE);
}

static void init_slave_timer(void) {
	timer_parameter_struct timer_initpara;
	
	rcu_periph_clock_enable(SLAVE_CK);

	timer_deinit(SLAVE);

	memset(&timer_initpara, 0, sizeof(timer_initpara));
	timer_initpara.prescaler		= 0; //clk 10MHz/65535
	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
	timer_initpara.counterdirection = TIMER_COUNTER_UP;
	timer_initpara.period		   = 0xFFFF;
	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
	timer_initpara.repetitioncounter = 0;
	timer_init(SLAVE,&timer_initpara);
	timer_counter_value_config(SLAVE, 0);
	timer_autoreload_value_config(SLAVE, 0xFFFF);
	timer_counter_up_direction(SLAVE);
	timer_auto_reload_shadow_enable(SLAVE);
	timer_master_slave_mode_config(SLAVE, TIMER_MASTER_SLAVE_MODE_ENABLE);
	timer_slave_mode_select(SLAVE, TIMER_SLAVE_MODE_EXTERNAL0);
	timer_input_trigger_source_select(SLAVE, TIMER_SMCFG_TRGSEL_ITI1); //select timer1 output as the trigger input

}

static __inline__ u32 _timer_count32_get(void) {
#if 0
	u16 high;
	u16 low;
	do {
		high = TIMER_CNT(SLAVE);
		low = TIMER_CNT(MASTER);
	}while((high != TIMER_CNT(SLAVE)) /*|| (low != TIMER_CNT(MASTER))*/);
	return (high<<16) | low;
#else
	return cpu_counts_abs();
#endif
}


u32 timer_count32_get(void) {
	return _timer_count32_get();
}


u32 timer_count32_delta_us(u32 count, u32 *p_update) {
	u32 now = timer_count32_get();
	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 = timer_count32_delta_us(m->intval_count, &m->intval_count);
	m->exec_count = timer_count32_get();
}
void time_measure_end(measure_time_t *m) {
	m->exec_time = timer_count32_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;
	}
}