#include "bsp/bsp_driver.h"
#include "libs/utils.h"
#include "os/os_task.h"
#include "libs/logger.h"
#include "math/fast_math.h"


#ifdef CONFIG_BOARD_MCXXX
#if (CONFIG_MC105_HW_VERSION==2)
#define ADC01_NUM (8)
#define ADC2_NUM 4

#define MOS_TEMP_BUFF_IDX 0
#define VREF5v_BUFF_IDX 1
#define VBUS_I_BUFF_IDX 2
#define U_VOL_BUFF_IDX 3
#define V_VOL_BUFF_IDX 4
#define W_VOL_BUFF_IDX 5
#define VREF_BUFF_IDX 7

#define VBUS_V_BUFF_IDX 8
#define ACC_V_BUFF_IDX 9
#define THROTTLE_BUFF_IDX 10
#define MOTOR_TEMP_BUFF_IDX 11

#elif (CONFIG_MC105_HW_VERSION==3)
#define ADC1_NUM 9
#define ADC2_NUM 9

#define MOS_TEMP_BUFF_IDX 0
#define VBUS_V_BUFF_IDX 1

#define MOTOR_TEMP_BUFF_IDX 2
#define ACC_V_BUFF_IDX 3

#define THROTTLE_BUFF_IDX 4
#define VBUS_I_BUFF_IDX 5

#define THROTTLE2_BUFF_IDX 6
#define V_VOL_BUFF_IDX 7

//zero chan            8
#define W_VOL_BUFF_IDX 9

#define VREF_BUFF_IDX   10
//zero chan             11

#define THROTTLE2_5V_BUFF_IDX 12
#define THROTTLE_5V_BUFF_IDX 13

#define U_VOL_BUFF_IDX 14
//zero chan            15

//zero chan            16
#define VREF5v_BUFF_IDX 17

#endif
#endif
#define REG_CHAN_NUM (ADC1_NUM + ADC2_NUM)
u16 adc_buffer[REG_CHAN_NUM];
float vref_adc = 1408.0f;
float vref_5v_adc = 3095.0f;

static void analog_gpio_init(gpio_type *gpiox, u32 pin) {
	gpio_init_type gpio_init_struct = {0};
	/* gpio configuration */
	gpio_default_para_init(&gpio_init_struct);
	gpio_init_struct.gpio_mode = GPIO_MODE_ANALOG;
	gpio_init_struct.gpio_pins = pin;
	gpio_init(gpiox, &gpio_init_struct);
}

static void adc01_dma_init(void)
{
    dma_init_type dma_init_struct;
	/* dma clock configuration */
	crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE);

	/* dma configuration */
	dma_reset(DMA1_CHANNEL1);
	dma_default_para_init(&dma_init_struct);
	dma_init_struct.buffer_size = REG_CHAN_NUM/2;
	dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY;
	dma_init_struct.memory_base_addr = (uint32_t)adc_buffer;
	dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_WORD;
	dma_init_struct.memory_inc_enable = TRUE;
	dma_init_struct.peripheral_base_addr = (uint32_t)&(ADC1->odt);
	dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_WORD;
	dma_init_struct.peripheral_inc_enable = FALSE;
	dma_init_struct.priority = DMA_PRIORITY_HIGH;
	dma_init_struct.loop_mode_enable = TRUE;
	dma_init(DMA1_CHANNEL1, &dma_init_struct);
	
	dma_channel_enable(DMA1_CHANNEL1, TRUE);

}


static void adc0_init(void){
	adc_base_config_type adc_base_struct;
	
	/* adc clock configuration */
	crm_adc_clock_div_set(CRM_ADC_DIV_8);								  /* PCLK2 Max. CLK = 100M Hz, ADC_CLK = 100/4 = 25M Hz */
	crm_periph_clock_enable(CRM_ADC1_PERIPH_CLOCK, TRUE);
	crm_periph_clock_enable(CRM_ADC2_PERIPH_CLOCK, TRUE);

	adc_reset(ADC1);
	adc_reset(ADC2);
	
	adc_base_struct.sequence_mode = TRUE;
	adc_base_struct.repeat_mode = TRUE;
	adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT;
	adc_base_struct.ordinary_channel_length = ADC1_NUM;
	adc_base_config(ADC1, &adc_base_struct);
	adc_base_config(ADC2, &adc_base_struct);
	
	/* ordinary channel configuration */
	adc_ordinary_channel_set(ADC1, MOS_TEMP_ADC_CHAN, 1, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC1, MOTOR_TEMP_ADC_CHAN, 2, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC1, THROTTLE_CHAN, 3, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC1, THROTTLE2_CHAN, 4, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC1, ZERO_ADC_CHAN, 5, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
	adc_ordinary_channel_set(ADC1, ADC_CHANNEL_17, 6, ADC_REGCHAN_SAMPLE_TIME); //mcu内部vref
	adc_ordinary_channel_set(ADC1, THROTTLE2_5V_CHAN, 7, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC1, U_VOL_ADC_CHAN, 8, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC1, ZERO_ADC_CHAN, 9, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
	adc_ordinary_conversion_trigger_set(ADC1, ADC12_ORDINARY_TRIG_SOFTWARE, TRUE);

	adc_ordinary_channel_set(ADC2, VBUS_V_CHAN, 1, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, ACC_V_CHAN, 2, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, VBUS_I_CHAN, 3, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, V_VOL_ADC_CHAN, 4, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, W_VOL_ADC_CHAN, 5, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, ZERO_ADC_CHAN, 6, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
	adc_ordinary_channel_set(ADC2, THROTTLE_5V_CHAN, 7, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, ZERO_ADC_CHAN, 8, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
	adc_ordinary_channel_set(ADC2, DC5V_ADC_CHAN, 9, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_conversion_trigger_set(ADC2, ADC12_ORDINARY_TRIG_SOFTWARE, TRUE);


	adc_preempt_channel_length_set(ADC1, 3);
	adc_preempt_channel_set(ADC1, V_PHASE_I_CHAN, 1, ADC_INSERT_SAMPLE_TIME);
	adc_preempt_channel_set(ADC1, V_PHASE_I_CHAN, 2, ADC_INSERT_SAMPLE_TIME);
	adc_preempt_channel_set(ADC1, V_PHASE_I_CHAN, 3, ADC_INSERT_SAMPLE_TIME);
	//adc_preempt_channel_set(ADC1, V_PHASE_I_CHAN, 4, ADC_INSERT_SAMPLE_TIME);
	/* adc prempt trigger source */
	adc_preempt_conversion_trigger_set(ADC1, ADC12_PREEMPT_TRIG_TMR1CH4, TRUE);

	adc_preempt_channel_length_set(ADC2, 3);
	adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 1, ADC_INSERT_SAMPLE_TIME);
	adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 2, ADC_INSERT_SAMPLE_TIME);
	adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 3, ADC_INSERT_SAMPLE_TIME);
	//adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 4, ADC_INSERT_SAMPLE_TIME);
	/* adc prempt trigger source */
	adc_preempt_conversion_trigger_set(ADC2, ADC12_PREEMPT_TRIG_TMR1CH4, TRUE);

	/* select adc mster-slave mode */
	adc_combine_mode_select(ADC_ORDINARY_SMLT_PREEMPT_SMLT_MODE);

	adc_tempersensor_vintrv_enable(TRUE);

	adc_dma_mode_enable(ADC1, TRUE);
	adc_dma_mode_enable(ADC2, TRUE);

	/* ADC enable and calibration */
	adc_enable(ADC1, TRUE);
	adc_calibration_init(ADC1);
	while(adc_calibration_init_status_get(ADC1));
	adc_calibration_start(ADC1);
	while(adc_calibration_status_get(ADC1));

	adc_enable(ADC2, TRUE);
	adc_calibration_init(ADC2);
	while(adc_calibration_init_status_get(ADC2));
	adc_calibration_start(ADC2);
	while(adc_calibration_status_get(ADC2));

	nvic_irq_enable(ADC1_2_IRQn, ADC_IRQ_PRIORITY, 0);

	adc_disable_ext_trigger();

	adc_current_sample_config(0);
	
	adc_ordinary_software_trigger_enable(ADC1, TRUE);
}

static void adc1_init(void){

#if 0
	adc_base_config_type adc_base_struct;
	
	/* adc clock configuration */
	crm_periph_clock_enable(CRM_ADC2_PERIPH_CLOCK, TRUE);

	adc_reset(ADC2);
	adc_base_struct.sequence_mode = TRUE;
	adc_base_struct.repeat_mode = TRUE;
	adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT;
	adc_base_struct.ordinary_channel_length = ADC2_NUM;
	adc_base_config(ADC2, &adc_base_struct);

	/* ordinary channel configuration */
	adc_ordinary_channel_set(ADC2, VBUS_V_CHAN, 1, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, ACC_V_CHAN, 2, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, VBUS_I_CHAN, 3, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, V_VOL_ADC_CHAN, 4, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, W_VOL_ADC_CHAN, 5, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, ZERO_ADC_CHAN, 6, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
	adc_ordinary_channel_set(ADC2, THROTTLE_5V_CHAN, 7, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_channel_set(ADC2, ZERO_ADC_CHAN, 8, ADC_REGCHAN_SAMPLE_TIME); //insert zero vol
	adc_ordinary_channel_set(ADC2, DC5V_ADC_CHAN, 9, ADC_REGCHAN_SAMPLE_TIME);
	adc_ordinary_conversion_trigger_set(ADC2, ADC12_ORDINARY_TRIG_SOFTWARE, TRUE);

	adc_preempt_channel_length_set(ADC2, 3);
	adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 1, ADC_INSERT_SAMPLE_TIME);
	adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 2, ADC_INSERT_SAMPLE_TIME);
	adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 3, ADC_INSERT_SAMPLE_TIME);
	//adc_preempt_channel_set(ADC2, W_PHASE_I_CHAN, 4, ADC_INSERT_SAMPLE_TIME);
	/* adc prempt trigger source */
	adc_preempt_conversion_trigger_set(ADC2, ADC12_PREEMPT_TRIG_TMR1CH4, TRUE);

	adc_dma_mode_enable(ADC2, TRUE);
	/* ADC enable and calibration */
	adc_enable(ADC2, TRUE);
	adc_calibration_init(ADC2);
	while(adc_calibration_init_status_get(ADC2));
	adc_calibration_start(ADC2);
	while(adc_calibration_status_get(ADC2));
#endif
}


static void adc_gpio_init(void) {

	/* configure ADC pin, current sampling -- ADC_IN1(PA1) ADC_IN12(PC2) ADC_IN13(PC3) */
#ifdef U_PHASE_ADC_GROUP
	crm_periph_clock_enable(U_PHASE_ADC_RCU, TRUE);
	analog_gpio_init(U_PHASE_ADC_GROUP, U_PHASE_ADC_PIN);
#endif
#ifdef V_PHASE_ADC_GROUP
	crm_periph_clock_enable(V_PHASE_ADC_RCU, TRUE);
	analog_gpio_init(V_PHASE_ADC_GROUP, V_PHASE_ADC_PIN);
#endif
#ifdef W_PHASE_ADC_GROUP
	crm_periph_clock_enable(W_PHASE_ADC_RCU, TRUE);
	analog_gpio_init(W_PHASE_ADC_GROUP, W_PHASE_ADC_PIN);
#endif

#ifdef VBUS_V_ADC_GROUP
	crm_periph_clock_enable(VBUS_V_ADC_RCU, TRUE);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	analog_gpio_init(VBUS_V_ADC_GROUP, VBUS_V_ADC_PIN);
#endif

#ifdef VBUS_I_ADC_GROUP
	crm_periph_clock_enable(VBUS_I_ADC_RCU, TRUE);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	analog_gpio_init(VBUS_I_ADC_GROUP, VBUS_I_ADC_PIN);
#endif


#ifdef ACC_V_ADC_GROUP
	crm_periph_clock_enable(ACC_V_ADC_RCU, TRUE);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	analog_gpio_init(ACC_V_ADC_GROUP, ACC_V_ADC_PIN);
#endif

#ifdef THROTTLE_V_ADC_GROUP
	crm_periph_clock_enable(THROTTLE_V_ADC_RCU, TRUE);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	analog_gpio_init(THROTTLE_V_ADC_GROUP, THROTTLE_V_ADC_PIN);
#endif
#ifdef THROTTLE2_V_ADC_GROUP
	crm_periph_clock_enable(THROTTLE2_V_ADC_RCU, TRUE);
	analog_gpio_init(THROTTLE2_V_ADC_GROUP, THROTTLE2_V_ADC_PIN);
#endif

#ifdef THROTTLE_5V_ADC_GROUP
	crm_periph_clock_enable(THROTTLE_5V_ADC_RCU, TRUE);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	analog_gpio_init(THROTTLE_5V_ADC_GROUP, THROTTLE_5V_ADC_PIN);
#endif
#ifdef THROTTLE2_5V_ADC_GROUP
	crm_periph_clock_enable(THROTTLE2_5V_ADC_RCU, TRUE);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	analog_gpio_init(THROTTLE2_5V_ADC_GROUP, THROTTLE2_5V_ADC_PIN);
#endif

#ifdef U_VOL_ADC_GROUP
	crm_periph_clock_enable(U_VOL_ADC_RCU, TRUE);
	analog_gpio_init(U_VOL_ADC_GROUP,  U_VOL_ADC_PIN);
#endif
#ifdef V_VOL_ADC_GROUP
	crm_periph_clock_enable(V_VOL_ADC_RCU, TRUE);
	analog_gpio_init(V_VOL_ADC_GROUP, V_VOL_ADC_PIN);
#endif
#ifdef W_VOL_ADC_GROUP
	crm_periph_clock_enable(W_VOL_ADC_RCU, TRUE);
	analog_gpio_init(W_VOL_ADC_GROUP, W_VOL_ADC_PIN);
#endif
#ifdef MOS_TEMP_ADC_GROUP
	crm_periph_clock_enable(MOS_TEMP_ADC_RCU, TRUE);
	analog_gpio_init(MOS_TEMP_ADC_GROUP, MOS_TEMP_ADC_PIN);
#endif
#ifdef MOS_TEMP1_ADC_GROUP
	crm_periph_clock_enable(MOS_TEMP1_ADC_RCU, TRUE);
	analog_gpio_init(MOS_TEMP1_ADC_GROUP, MOS_TEMP1_ADC_PIN);
#endif

#ifdef MOTOR_TEMP_ADC_GROUP
	crm_periph_clock_enable(MOTOR_TEMP_ADC_RCU, TRUE);
	analog_gpio_init(MOTOR_TEMP_ADC_GROUP, MOTOR_TEMP_ADC_PIN);
#endif

#ifdef ZERO_ADC_GROUP
	crm_periph_clock_enable(ZERO_ADC_RCU, TRUE);
	analog_gpio_init(ZERO_ADC_GROUP, ZERO_ADC_PIN);
#endif

}

void adc_init(void) {
	adc_gpio_init();
	adc01_dma_init();
	adc0_init();
	adc1_init();
}

void adc_set_vref_calc(float v) {
	vref_adc = v;
}

void adc_set_5vref_calc(float v) {
	vref_5v_adc = v;
}

#define VREF_COMP_LFP_CEOF (0.0001F)
static float vref_compestion_filter = 1.0f;
#define VREF_3V3_COMPESTION() (vref_adc/(float)adc_buffer[VREF_BUFF_IDX])
void adc_3v3ref_filter(void) {
	float value = VREF_3V3_COMPESTION();
	LowPass_Filter(vref_compestion_filter, value, VREF_COMP_LFP_CEOF);
}

float adc_vref_compesion(void) {
	return vref_compestion_filter;
}

static float vref_5v_compestion_filter = 1.0f;
#define VREF_5V_COMPESTION() (vref_5v_adc/(float)adc_buffer[VREF5v_BUFF_IDX])
void adc_5vref_filter(void) {
	float value = VREF_5V_COMPESTION();
	LowPass_Filter(vref_5v_compestion_filter, value, VREF_COMP_LFP_CEOF);
}

float adc_5vref_compesion(void) {
	return vref_5v_compestion_filter;
}

void adc_vref_filter(void) {
	adc_3v3ref_filter();
	adc_5vref_filter();
}

u16 adc_get_vbus(void) {
	return (float)adc_buffer[VBUS_V_BUFF_IDX] * VREF_3V3_COMPESTION();
}

u16 adc_get_acc(void) {
#ifdef CONFIG_BOARD_MCXXX
	return (float)adc_buffer[ACC_V_BUFF_IDX] * VREF_3V3_COMPESTION();
#else
	return adc_get_vbus();
#endif
}

u16 adc_get_ibus(void) {
#ifdef CONFIG_BOARD_MCXXX
	return (float)adc_buffer[VBUS_I_BUFF_IDX] * VREF_5V_COMPESTION();
#else
	return 0;
#endif
}

u16 adc_get_throttle(void) {
	return adc_buffer[THROTTLE_BUFF_IDX] * VREF_3V3_COMPESTION();
}

u16 adc_get_throttle2(void) {
#ifdef THROTTLE2_BUFF_IDX
	return adc_buffer[THROTTLE2_BUFF_IDX] * VREF_3V3_COMPESTION();
#else
	return adc_get_throttle();
#endif
}

u16 adc_get_thro_5v(void) {
#ifdef THROTTLE_5V_BUFF_IDX
	return adc_buffer[THROTTLE_5V_BUFF_IDX] * VREF_3V3_COMPESTION();
#else
	return 0;
#endif
}

u16 adc_get_thro2_5v(void) {
#ifdef THROTTLE2_5V_BUFF_IDX
	return adc_buffer[THROTTLE2_5V_BUFF_IDX] * VREF_3V3_COMPESTION();
#else
	return 0;
#endif
}

void adc_get_uvw_phaseV(u16 *uvw) {
	uvw[0] = adc_buffer[U_VOL_BUFF_IDX];
	uvw[1] = adc_buffer[V_VOL_BUFF_IDX];
	uvw[2] = adc_buffer[W_VOL_BUFF_IDX];
}

u16 adc_get_mos_temp(void) {
	return adc_buffer[MOS_TEMP_BUFF_IDX];
}

u16 adc_get_motor_temp(void) {
	return adc_buffer[MOTOR_TEMP_BUFF_IDX];
}

u16 adc_get_vref(void) {
#ifdef CONFIG_BOARD_MCXXX
	return adc_buffer[VREF_BUFF_IDX];
#else
	return 0;
#endif
}

u16 adc_get_5v_ref(void) {
#ifdef CONFIG_BOARD_MCXXX
	return adc_buffer[VREF5v_BUFF_IDX];
#else
	return 0;
#endif
}

void adc_start_convert(void) {
	int drop = 16;
    /* clear the ADC flag */
    adc_flag_clear(ADC1, ADC_PCCE_FLAG);
    adc_flag_clear(ADC2, ADC_PCCE_FLAG);

	adc_enable_ext_trigger();
	while(drop-- > 0) {
		while (adc_flag_get(ADC1, ADC_PCCE_FLAG) == RESET);
		adc_flag_clear(ADC1, ADC_PCCE_FLAG);
	}
    /* enable ADC interrupt */

	adc_interrupt_enable(ADC1, ADC_PCCE_INT, TRUE);

	adc_update_ext_trigger(ADC_TRIGGER_PHASE);
}

void adc_stop_convert(void) {
	adc_disable_ext_trigger();
    /* disable ADC interrupt */

	adc_interrupt_enable(ADC1, ADC_PCCE_INT, FALSE);

    /* clear the ADC flag */
    adc_flag_clear(ADC1, ADC_PCCE_FLAG);
	adc_flag_clear(ADC2, ADC_PCCE_FLAG);
}