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

static void _io_init(void) {
	rcu_apb2_periph_clock_enable(ENC_A_RCU);
	rcu_apb2_periph_clock_enable(ENC_B_RCU);
	rcu_apb2_periph_clock_enable(ENC_PWM_RCU);
	rcu_apb2_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 _io_init_irq(void) {
	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);
}

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

void enc_intf_quadrature_init(u32 rate) {
	TIM_TimeBaseInitType TIM_TimeBaseStructure;
	TIM_ICInitType TIM_ICInitStructure;

	rcu_apb1_periph_clock_enable(ENC_TIMER_RCU);
	
	TIM_Module* TIMx = ENC_TIMER;
	
	//initial tim for encode
	TIM_DeInit(TIMx);
	TIM_InitTimBaseStruct(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.Period = rate - 1;
	TIM_TimeBaseStructure.Prescaler = 0;
	TIM_TimeBaseStructure.ClkDiv = TIM_CLK_DIV1;
	TIM_TimeBaseStructure.CntMode = TIM_CNT_MODE_UP;
	TIM_InitTimeBase(TIMx,&TIM_TimeBaseStructure);

	TIM_InitIcStruct(&TIM_ICInitStructure);
	TIM_ICInitStructure.IcPolarity = TIM_IC_POLARITY_RISING;
	TIM_ICInitStructure.IcFilter = ENC_FILTER_NR;
	TIM_ICInitStructure.Channel = TIM_CH_1;
	TIM_ICInit(TIMx,&TIM_ICInitStructure);
	TIM_ICInitStructure.Channel = TIM_CH_2;
	TIM_ICInit(TIMx,&TIM_ICInitStructure);

	//qr encode set
	TIM_ConfigEncoderInterface(TIMx,TIM_ENCODE_MODE_TI12,TIM_IC_POLARITY_FALLING,TIM_IC_POLARITY_RISING);
		
	TIM_ConfigArPreload(TIMx,ENABLE);
	TIM_SetCnt(TIMx,0);
	TIM_Enable(TIMx,ENABLE);
	
	TIM_ClearFlag(TIMx,TIM_FLAG_UPDATE);
	TIM_ConfigInt(TIMx,TIM_INT_UPDATE,ENABLE);
}

void enc_intf_pwm_counter(void) {
	TIM_TimeBaseInitType TIM_TimeBaseStructure;
	TIM_ICInitType TIM_ICInitStructure;

	rcu_apb1_periph_clock_enable(ENC_PWM_TIMER_RCU);

	TIM_Module* TIMx = ENC_PWM_TIMER;

	TIM_DeInit(TIMx);
	TIM_InitTimBaseStruct(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.Period = 65535;
	TIM_TimeBaseStructure.Prescaler = TIM_PWM_CLK/PWM_TIME_CLK - 1;
	TIM_TimeBaseStructure.ClkDiv = TIM_CLK_DIV1;
	TIM_TimeBaseStructure.CntMode = TIM_CNT_MODE_UP;
	TIM_InitTimeBase(TIMx,&TIM_TimeBaseStructure);

    TIM_ICInitStructure.Channel     = ENC_PWM_TIMER_CHAN;
    TIM_ICInitStructure.IcPolarity  = TIM_IC_POLARITY_RISING;
    TIM_ICInitStructure.IcSelection = TIM_IC_SELECTION_DIRECTTI;
    TIM_ICInitStructure.IcPrescaler = TIM_IC_PSC_DIV1;
    TIM_ICInitStructure.IcFilter    = ENC_FILTER_NR;

    TIM_ConfigPwmIc(TIMx, &TIM_ICInitStructure);

    /* Select the TIM3 Input Trigger: TI2FP2 */
    TIM_SelectInputTrig(TIMx, TIM_TRIG_SEL_TI1FP1);

    /* Select the slave Mode: Reset Mode */
    TIM_SelectSlaveMode(TIMx, TIM_SLAVE_MODE_RESET);

    /* Enable the Master/Slave Mode */
    TIM_SelectMasterSlaveMode(TIMx, TIM_MASTER_SLAVE_MODE_ENABLE);

    /* TIM enable counter */
    TIM_Enable(TIMx, ENABLE);

	TIM_ClearFlag(TIMx,ENC_PWM_TIMER_INT_FLG);
    /* Enable the CC2 Interrupt Request */
    TIM_ConfigInt(TIMx, ENC_PWM_TIMER_IRQ_CH, ENABLE);

	nvic_irq_enable(ENC_PWM_TIMER_IRQ, ENC_PWM_IRQ_PRIORITY, 0);

}

__weak void ENC_TIMER_Overflow(void) {

}

void ENC_TIMER_IRQHandler(void) {
	if (SET == TIM_GetIntStatus(ENC_TIMER, TIM_INT_UPDATE)) {
		TIM_ClrIntPendingBit(ENC_TIMER, TIM_INT_UPDATE);
		ENC_TIMER_Overflow();
	}
}


__weak void ENC_ABI_IRQHandler(void) {

}

void ABI_I_IRQHandler(void) {
	ENC_ABI_IRQHandler();
}

__weak void ENC_PWM_Duty_Handler(float t, float d) {

}

static float pwm_freq = 0.0f;
void ENC_PWM_IRQHandler(void) {
    if(SET == TIM_GetIntStatus(ENC_PWM_TIMER, ENC_PWM_TIMER_INT_FLG)){
        /* clear channel 0 interrupt bit */
        TIM_ClrIntPendingBit(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;
}