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

static void enc_gpio_init(gpio_type *gpiox, gpio_mode_type mode, u32 pin) {
	gpio_init_type gpio_init_struct = {0};
	gpio_default_para_init(&gpio_init_struct);
	gpio_init_struct.gpio_mode = mode;
	gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
	gpio_init_struct.gpio_pull = GPIO_PULL_NONE;
	gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
	
	/* High-side, Phase A,B,C Config */
	gpio_init_struct.gpio_pins = pin;
	gpio_init(gpiox, &gpio_init_struct);
}


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

static void _io_init_irq(void) {
	exint_init_type exint_init_struct;

	gpio_exint_line_config(ENC_I_EXIT_SRC_GROUP, ENC_I_EXIT_SRC_PIN);
	
	exint_default_para_init(&exint_init_struct);
	exint_init_struct.line_enable = TRUE;
	exint_init_struct.line_mode = EXINT_LINE_INTERRUPUT;
	exint_init_struct.line_select = ENC_I_EXTI;
	exint_init_struct.line_polarity = EXINT_TRIGGER_RISING_EDGE;
	exint_init(&exint_init_struct);

	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) {

	tmr_type* timer = ENC_TIMER;

	crm_periph_clock_enable(ENC_TIMER_RCU, TRUE);
	tmr_reset(timer);

	tmr_base_init(timer, rate -1 , 0);
	tmr_cnt_dir_set(timer, TMR_COUNT_UP);
	tmr_input_channel_filter_set(timer, TMR_SELECT_CHANNEL_1, ENC_FILTER_NR);
	tmr_input_channel_filter_set(timer, TMR_SELECT_CHANNEL_2, ENC_FILTER_NR);
	/* config encoder mode */
	tmr_encoder_mode_config(timer, TMR_ENCODER_MODE_C, TMR_INPUT_FALLING_EDGE, TMR_INPUT_RISING_EDGE);
	
	/* enable tmr2 */
	tmr_counter_enable(timer, TRUE);
}

void enc_intf_pwm_counter(void) {
	tmr_input_config_type tmr_ic_init_structure;

	tmr_type * timer = ENC_PWM_TIMER;

	crm_periph_clock_enable(ENC_PWM_TIMER_RCU, TRUE);
	tmr_reset(timer);

	tmr_input_default_para_init(&tmr_ic_init_structure);
	tmr_ic_init_structure.input_filter_value = ENC_FILTER_NR;
	tmr_ic_init_structure.input_channel_select = ENC_PWM_TIMER_CHAN;
	tmr_ic_init_structure.input_mapped_select = TMR_CC_CHANNEL_MAPPED_DIRECT;
	tmr_ic_init_structure.input_polarity_select = TMR_INPUT_RISING_EDGE;
	
	tmr_pwm_input_config(timer, &tmr_ic_init_structure, TMR_CHANNEL_INPUT_DIV_1);
	
	/* select the tmr3 input trigger: c2df2 */
	tmr_trigger_input_select(timer, TMR_SUB_INPUT_SEL_C1DF1);
	
	/* select the sub mode: reset mode */
	tmr_sub_mode_select(timer, TMR_SUB_RESET_MODE);
	
	/* enable the sub sync mode */
	tmr_sub_sync_mode_set(timer, TRUE);
	
	/* tmr enable counter */
	tmr_counter_enable(timer, TRUE);
	
	/* enable the c2 interrupt request */
	tmr_interrupt_enable(timer, ENC_PWM_TIMER_IRQ_CH, TRUE);

	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 == tmr_flag_get(ENC_TIMER, TMR_OVF_FLAG)) {
		tmr_flag_clear(ENC_TIMER, TMR_OVF_FLAG);
		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 == tmr_flag_get(ENC_PWM_TIMER, ENC_PWM_TIMER_INT_FLG)){
        /* clear channel 0 interrupt bit */
        tmr_flag_clear(ENC_PWM_TIMER, ENC_PWM_TIMER_INT_FLG);
        /* read channel 0 capture value */
        u32 ic0value = tmr_channel_value_get(ENC_PWM_TIMER, TMR_SELECT_CHANNEL_1) + 1;
		u32 ic1value = tmr_channel_value_get(ENC_PWM_TIMER, TMR_SELECT_CHANNEL_2) + 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;
}