#include "bsp/bsp_driver.h"
#include "libs/logger.h"

static void _io_init(void) {
	rcu_periph_clock_enable(ENC_A_RCU);
	rcu_periph_clock_enable(ENC_B_RCU);
	rcu_periph_clock_enable(ENC_PWM_RCU);
	rcu_periph_clock_enable(ENC_I_RCU);
#ifdef TIMER2_PB4_PB5_REMAP
	gpio_pin_remap_config(TIMER2_PB4_PB5_REMAP, ENABLE);
#endif
#ifdef TIMER1_PA15_REMAP
	gpio_pin_remap_config(TIMER1_PA15_REMAP, ENABLE);
#endif
	gpio_init(ENC_A_GROUP, ENC_A_MODE, GPIO_OSPEED_50MHZ, ENC_A_PIN);
	gpio_init(ENC_B_GROUP, ENC_B_MODE, GPIO_OSPEED_50MHZ, ENC_B_PIN);
	gpio_init(ENC_PWM_GROUP, ENC_PWM_MODE, GPIO_OSPEED_50MHZ, ENC_PWM_PIN);
	gpio_init(ENC_I_GROUP, ENC_I_MODE, GPIO_OSPEED_50MHZ, ENC_I_PIN);
}

static void z_io_init_irq(void) {
#ifdef ENC_I_IRQ
	gpio_exti_source_select(ENC_I_EXIT_SRC_GROUP, ENC_I_EXIT_SRC_PIN);
	exti_init(ENC_I_EXTI, EXTI_INTERRUPT, EXTI_TRIG_RISING);
	exti_interrupt_flag_clear(ENC_I_EXTI);
	exti_interrupt_enable(ENC_I_EXTI);
	nvic_irq_enable(ENC_I_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0U);
#endif
}

void enc_intf_init(u32 rate) {
	_io_init();
	enc_intf_pwm_counter();
	enc_intf_quadrature_init(rate);
	z_io_init_irq();
	enc_intf_z_counter();
}

void enc_intf_quadrature_init(u32 rate) {
	timer_parameter_struct timer_initpara;
	timer_ic_parameter_struct timer_icinitpara;

	u32 timer = ENC_TIMER;

	rcu_periph_clock_enable(ENC_TIMER_RCU);

	timer_deinit(timer);

	/* TIMER configuration */
	timer_struct_para_init(&timer_initpara);
	timer_initpara.prescaler		= 0;//TIM_CLOCK/PWM_TIME_CLK - 1;
	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
	timer_initpara.period		   = rate-1;
	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
	timer_initpara.repetitioncounter = 0;
	timer_init(timer,&timer_initpara);

    /* initialize TIMER channel input parameter struct */
    timer_channel_input_struct_para_init(&timer_icinitpara);
    /* TIMER2 CH0 input capture configuration */
    timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
    timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
    timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
    timer_icinitpara.icfilter    = CONFIG_ENC_FILTER_NR;//采样频率120M, 这个必须要加入滤波，否则AB的计数可能不对
    timer_input_capture_config(timer,TIMER_CH_0,&timer_icinitpara);
	
	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
	timer_input_capture_config(timer,TIMER_CH_1,&timer_icinitpara);

	timer_quadrature_decoder_mode_config(timer ,TIMER_ENCODER_MODE2,TIMER_IC_POLARITY_FALLING, TIMER_IC_POLARITY_RISING);

	/* TIMER CH2 PWM input capture configuration */
	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
	timer_icinitpara.icselection = TIMER_IC_SELECTION_ITS;
	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
	timer_icinitpara.icfilter	 = 0;
	timer_input_capture_config(timer, TIMER_CH_2, &timer_icinitpara);
	timer_input_trigger_source_select(timer, TIMER_SMCFG_TRGSEL_ITI3);//come from trigger out of time3 which captrue the Z singnal and output to me

	/* auto-reload preload enable */
	timer_auto_reload_shadow_enable(timer);	

	timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_UP);

    /* clear channel 0 interrupt bit */
    timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_CH2);
    /* channel 0 interrupt enable */
    timer_interrupt_enable(timer, TIMER_INT_CH2);
	nvic_irq_enable(ENC_TIMER_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0);

	/* TIMER2 counter enable */
	timer_enable(timer);
}

void enc_intf_pwm_counter(void) {
	timer_ic_parameter_struct timer_icinitpara;
	timer_parameter_struct timer_initpara;
	u32 timer = ENC_PWM_TIMER;

	rcu_periph_clock_enable(ENC_PWM_TIMER_RCU);

	timer_deinit(timer);

	/* TIMER configuration */
	timer_struct_para_init(&timer_initpara);
	timer_initpara.prescaler		= TIM_CLOCK/PWM_TIME_CLK - 1;
	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
	timer_initpara.period		   = 65535;
	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
	timer_initpara.repetitioncounter = 0;
	timer_init(timer,&timer_initpara);

	timer_channel_input_struct_para_init(&timer_icinitpara);
	/* TIMER CH0 PWM input capture configuration */
	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
	timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
	timer_icinitpara.icfilter	 = CONFIG_PWM_FILTER_NR;
	timer_input_pwm_capture_config(timer, ENC_PWM_TIMER_CHAN, &timer_icinitpara);

	/* slave mode selection: TIMER */
	timer_input_trigger_source_select(timer, TIMER_SMCFG_TRGSEL_CI0FE0);
	timer_slave_mode_select(timer, TIMER_SLAVE_MODE_RESTART);

	/* select the master slave mode */
	timer_master_slave_mode_config(timer, TIMER_MASTER_SLAVE_MODE_ENABLE);

    /* auto-reload preload enable */
    timer_auto_reload_shadow_enable(timer);
    /* clear channel 0 interrupt bit */
    timer_interrupt_flag_clear(timer, ENC_PWM_TIMER_INT_FLG);
    /* channel 0 interrupt enable */
    timer_interrupt_enable(timer, ENC_PWM_TIMER_IRQ_CH);
	nvic_irq_enable(ENC_PWM_TIMER_IRQ, ENC_PWM_IRQ_PRIORITY, 0);
    /* TIMER2 counter enable */
    timer_enable(timer);
}


void enc_intf_z_counter(void) {
#ifdef ENC_I_TIMER
	timer_ic_parameter_struct timer_icinitpara;
	timer_parameter_struct timer_initpara;
	u32 timer = ENC_I_TIMER;

	rcu_periph_clock_enable(ENC_I_TIMER_RCU);

	timer_deinit(timer);

	/* TIMER configuration */
	timer_struct_para_init(&timer_initpara);
	timer_initpara.prescaler		= 0;
	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
	timer_initpara.period		   = 65535;
	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
	timer_initpara.repetitioncounter = 0;
	timer_init(timer,&timer_initpara);

	timer_channel_input_struct_para_init(&timer_icinitpara);
	/* TIMER CH0 PWM input capture configuration */
	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
	timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
	timer_icinitpara.icfilter	 = CONFIG_Z_IDX_FILTER_NR;
	timer_input_capture_config(timer, ENC_I_TIMER_CHAN, &timer_icinitpara);

	/* select the master slave mode */
	timer_master_slave_mode_config(timer,TIMER_MASTER_SLAVE_MODE_ENABLE);
	/* TIMER1 update event is used as trigger output */
	timer_master_output_trigger_source_select(timer,TIMER_TRI_OUT_SRC_CH0); //ch0 as tirgger out to encoder timer

    /* auto-reload preload enable */
    timer_auto_reload_shadow_enable(timer);
#if 0
	/* clear channel 0 interrupt bit */
    timer_interrupt_flag_clear(timer, ENC_I_TIMER_INT_FLG);
    /* channel 0 interrupt enable */
    timer_interrupt_enable(timer, ENC_I_TIMER_IRQ_CH);
	nvic_irq_enable(ENC_I_TIMER_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0);
#endif
    /* TIMER2 counter enable */
    timer_enable(timer);
#endif
}

__weak void ENC_TIMER_Overflow(void) {

}

__weak void ENC_ABI_IRQHandler(u32 count) {

}


void ENC_TIMER_IRQHandler(void) {
	if (SET == timer_interrupt_flag_get(ENC_TIMER, TIMER_INT_FLAG_UP)) {
		timer_interrupt_flag_clear(ENC_TIMER, TIMER_INT_FLAG_UP);
		ENC_TIMER_Overflow();
	}
	if (SET == timer_interrupt_flag_get(ENC_TIMER, TIMER_INT_CH2)) {
		timer_interrupt_flag_clear(ENC_TIMER, TIMER_INT_FLAG_CH2);
		ENC_ABI_IRQHandler(TIMER_CH2CV(ENC_TIMER));
	}
}


void ABI_I_IRQHandler(void) {
#ifdef ENC_I_IRQ
	ENC_ABI_IRQHandler(TIMER_CNT(ENC_TIMER));
#endif
}

void ENC_I_IRQHandler(void) {
#ifdef ENC_I_TIMER
    if(SET == timer_interrupt_flag_get(ENC_I_TIMER, ENC_I_TIMER_INT_FLG)){
        /* clear channel 0 interrupt bit */
        timer_interrupt_flag_clear(ENC_I_TIMER, ENC_I_TIMER_INT_FLG);
#if 0
		ENC_ABI_IRQHandler(TIMER_CNT(ENC_TIMER));
#endif
    }
#endif
}

__weak void ENC_PWM_Duty_Handler(float t, float d) {

}

static float pwm_freq = 0.0f;
void ENC_PWM_IRQHandler(void) {
    if(SET == timer_interrupt_flag_get(ENC_PWM_TIMER, ENC_PWM_TIMER_INT_FLG)){
        /* clear channel 0 interrupt bit */
        timer_interrupt_flag_clear(ENC_PWM_TIMER, ENC_PWM_TIMER_INT_FLG);
        /* read channel 0 capture value */
        u32 ic0value = TIMER_CH0CV(ENC_PWM_TIMER)+1;
		u32 ic1value = TIMER_CH1CV(ENC_PWM_TIMER)+1;
		float p_calc = ENC_PWM_Calc_P(ic0value);
		float d_calc = ENC_PWM_Calc_P(ic1value);
		ENC_PWM_Duty_Handler(p_calc, d_calc);
		pwm_freq = (float)PWM_TIME_CLK/((float)ic0value);
    }
}

float enc_get_pwm_freq(void) {
	return pwm_freq;
}