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


#if (CONFIG_MC105_HW_VERSION==2)
#define ADC01_NUM 6

#define MOS_TEMP_BUFF_IDX 0
#define VBUS_V_BUFF_IDX 1
#define VREF5v_BUFF_IDX 2  //需要跳线修改mos的第二路温感采集模拟5v
#define ACC_V_BUFF_IDX 3
#define U_VOL_BUFF_IDX 4
#define VBUS_I_BUFF_IDX 5
#define V_VOL_BUFF_IDX 6
#define THROTTLE_BUFF_IDX 7
#define W_VOL_BUFF_IDX 8
#define VREF_BUFF_IDX 10
#define MOTOR_TEMP_BUFF_IDX 11
#define REG_CHAN_NUM (ADC01_NUM + ADC01_NUM)
#define ADC_DUAL_MODE ADC_DAUL_REGULAL_PARALLEL_INSERTED_PARALLEL
#elif (CONFIG_MC105_HW_VERSION==3)
#define ADC01_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 14
#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 6
//zero chan            15

//zero chan            16
#define VREF5v_BUFF_IDX 17
#define REG_CHAN_NUM (ADC01_NUM + ADC01_NUM)
#define ADC_DUAL_MODE ADC_DAUL_REGULAL_PARALLEL_INSERTED_PARALLEL

#endif

s16 adc_buffer[REG_CHAN_NUM];
float vref_adc = 1408.0f;
float vref_5v_adc = 2047.0f;

#define VREF_ADC_DATA 1509.0F //1498, 1.21/3.3*4095


static void adc01_dma_init(void)
{
    dma_parameter_struct dma_init_struct;
    rcu_periph_clock_enable(RCU_DMA0);
    dma_deinit(DMA0, DMA_CH0);
    dma_init_struct.direction    = DMA_PERIPHERAL_TO_MEMORY;
    dma_init_struct.memory_addr  = (uint32_t)adc_buffer;
    dma_init_struct.memory_inc   = DMA_MEMORY_INCREASE_ENABLE;
    dma_init_struct.memory_width = DMA_MEMORY_WIDTH_32BIT;
    dma_init_struct.number       = REG_CHAN_NUM/2;
    dma_init_struct.periph_addr  = (uint32_t)(&ADC_RDATA(ADC0));
    dma_init_struct.periph_inc   = DMA_PERIPH_INCREASE_DISABLE;
    dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_32BIT;
    dma_init_struct.priority     = DMA_PRIORITY_ULTRA_HIGH;
    dma_init(DMA0, DMA_CH0, &dma_init_struct);
    dma_circulation_enable(DMA0, DMA_CH0);
    dma_memory_to_memory_disable(DMA0, DMA_CH0);

    dma_channel_enable(DMA0, DMA_CH0);
}


static void adc01_init(u32 reg_sampletime) {
	/* config work mode */
    adc_special_function_config(ADC0, ADC_CONTINUOUS_MODE, ENABLE);
    adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE);
    adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, ENABLE);
    adc_special_function_config(ADC1, ADC_SCAN_MODE, ENABLE);

    /* configure ADC data alignment */
    adc_data_alignment_config(ADC0, ADC_DATAALIGN_RIGHT);
	adc_data_alignment_config(ADC1, ADC_DATAALIGN_RIGHT);

	/* configure ADC regular channel */
	
#if (CONFIG_MC105_HW_VERSION==2)
	adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, ADC01_NUM);
	adc_regular_channel_config(ADC0, 0, MOS_TEMP_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 1, MOS_TEMP1_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 2, U_VOL_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 3, V_VOL_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 4, W_VOL_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_17, reg_sampletime);//3.3vref

	adc_tempsensor_vrefint_enable();

	adc_channel_length_config(ADC1, ADC_REGULAR_CHANNEL, ADC01_NUM);
	adc_regular_channel_config(ADC1, 0, VBUS_V_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 1, ACC_V_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 2, VBUS_I_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 3, THROTTLE_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 4, ADC_CHANNEL_10, reg_sampletime); //dumy
	adc_regular_channel_config(ADC1, 5, MOTOR_TEMP_ADC_CHAN, reg_sampletime);
#else
	adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, ADC01_NUM);
	adc_regular_channel_config(ADC0, 0, MOS_TEMP_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 1, MOTOR_TEMP_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 2, THROTTLE_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 3, U_VOL_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 4, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol
	adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_17, reg_sampletime); //mcu内部vref
	adc_regular_channel_config(ADC0, 6, THROTTLE2_5V_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 7, THROTTLE2_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC0, 8, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol

	adc_tempsensor_vrefint_enable();

	adc_channel_length_config(ADC1, ADC_REGULAR_CHANNEL, ADC01_NUM);
	adc_regular_channel_config(ADC1, 0, VBUS_V_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 1, ACC_V_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 2, VBUS_I_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 3, V_VOL_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 4, W_VOL_ADC_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 5, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol
	adc_regular_channel_config(ADC1, 6, THROTTLE_5V_CHAN, reg_sampletime);
	adc_regular_channel_config(ADC1, 7, ZERO_ADC_CHAN, reg_sampletime); //insert zero vol
	adc_regular_channel_config(ADC1, 8, DC5V_ADC_CHAN, reg_sampletime);
#endif
	adc_buffer[VREF_BUFF_IDX] = VREF_ADC_DATA; //1.21/3.3*4095

    /* configure ADC regular channel trigger */
    adc_external_trigger_source_config(ADC0, ADC_REGULAR_CHANNEL, ADC0_1_2_EXTTRIG_REGULAR_NONE);
	adc_external_trigger_source_config(ADC1, ADC_REGULAR_CHANNEL, ADC0_1_2_EXTTRIG_REGULAR_NONE);
    adc_external_trigger_config(ADC0, ADC_REGULAR_CHANNEL, ENABLE);
	adc_external_trigger_config(ADC1, ADC_REGULAR_CHANNEL, ENABLE);

	/* configure ADC inserted channel length */
	adc_channel_length_config(ADC0, ADC_INSERTED_CHANNEL, INJ_CHAN_NUM);
	adc_inserted_channel_config(ADC0, 0, V_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
#if (INJ_CHAN_NUM==4)
	adc_inserted_channel_config(ADC0, 1, W_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
	adc_inserted_channel_config(ADC0, 2, V_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
	adc_inserted_channel_config(ADC0, 3, W_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
#endif

	adc_channel_length_config(ADC1, ADC_INSERTED_CHANNEL, INJ_CHAN_NUM);
	adc_inserted_channel_config(ADC1, 0, W_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
#if (INJ_CHAN_NUM==4)
	adc_inserted_channel_config(ADC1, 1, V_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
	adc_inserted_channel_config(ADC1, 2, W_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
	adc_inserted_channel_config(ADC1, 3, V_PHASE_I_CHAN, ADC_INSERT_SAMPLE_TIME);
#endif
	adc_mode_config(ADC_DUAL_MODE);

    /* configure ADC inserted channel trigger */
    adc_external_trigger_source_config(ADC0, ADC_INSERTED_CHANNEL, ADC_TRIGGER_PHASE);
    /* ADC external trigger enable */
    adc_external_trigger_config(ADC0, ADC_INSERTED_CHANNEL, ENABLE);
    /* ADC1 external trigger disable */
	adc_external_trigger_source_config(ADC1, ADC_INSERTED_CHANNEL, ADC_TRIGGER_NONE);
    adc_external_trigger_config(ADC1, ADC_INSERTED_CHANNEL, ENABLE);

    /* enable ADC interface */
    adc_enable(ADC0);
	delay_ms(1);
    /* ADC calibration and reset calibration */
    adc_calibration_enable(ADC0);
    adc_enable(ADC1);
	delay_ms(1);
    /* ADC calibration and reset calibration */
    adc_calibration_enable(ADC1);

	adc_dma_mode_enable(ADC0);

	adc_disable_ext_trigger();

	nvic_irq_enable(ADC0_1_IRQn, ADC_IRQ_PRIORITY, 0);

	//start regular channels
	adc_software_trigger_enable(ADC0, ADC_REGULAR_CHANNEL);
}


static void adc_gpio_init(void) {

	rcu_periph_clock_enable(RCU_AF);
		/* configure ADC pin, current sampling -- ADC_IN1(PA1) ADC_IN12(PC2) ADC_IN13(PC3) */
#ifdef U_PHASE_ADC_GROUP
	rcu_periph_clock_enable(U_PHASE_ADC_RCU);
	gpio_init(U_PHASE_ADC_GROUP, U_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, U_PHASE_ADC_PIN);
#endif
#ifdef V_PHASE_ADC_GROUP
	rcu_periph_clock_enable(V_PHASE_ADC_RCU);
	gpio_init(V_PHASE_ADC_GROUP, V_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, V_PHASE_ADC_PIN);
#endif
#ifdef W_PHASE_ADC_GROUP
	rcu_periph_clock_enable(W_PHASE_ADC_RCU);
	gpio_init(W_PHASE_ADC_GROUP, W_PHASE_ADC_MODE, GPIO_OSPEED_50MHZ, W_PHASE_ADC_PIN);
#endif

#ifdef VBUS_V_ADC_GROUP
	rcu_periph_clock_enable(VBUS_V_ADC_RCU);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	gpio_init(VBUS_V_ADC_GROUP, VBUS_V_ADC_MODE, GPIO_OSPEED_50MHZ, VBUS_V_ADC_PIN);
#endif

#ifdef VBUS_I_ADC_GROUP
	rcu_periph_clock_enable(VBUS_I_ADC_RCU);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	gpio_init(VBUS_I_ADC_GROUP, VBUS_I_ADC_MODE, GPIO_OSPEED_50MHZ, VBUS_I_ADC_PIN);
#endif


#ifdef ACC_V_ADC_GROUP
	rcu_periph_clock_enable(ACC_V_ADC_RCU);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	gpio_init(ACC_V_ADC_GROUP, ACC_V_ADC_MODE, GPIO_OSPEED_50MHZ, ACC_V_ADC_PIN);
#endif

#ifdef THROTTLE_V_ADC_GROUP
	rcu_periph_clock_enable(THROTTLE_V_ADC_RCU);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	gpio_init(THROTTLE_V_ADC_GROUP, THROTTLE_V_ADC_MODE, GPIO_OSPEED_50MHZ, THROTTLE_V_ADC_PIN);
#endif
#ifdef THROTTLE2_V_ADC_GROUP
	rcu_periph_clock_enable(THROTTLE2_V_ADC_RCU);
	gpio_init(THROTTLE2_V_ADC_GROUP, THROTTLE2_V_ADC_MODE, GPIO_OSPEED_50MHZ, THROTTLE2_V_ADC_PIN);
#endif

#ifdef THROTTLE_5V_ADC_GROUP
	rcu_periph_clock_enable(THROTTLE_5V_ADC_RCU);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	gpio_init(THROTTLE_5V_ADC_GROUP, THROTTLE_5V_ADC_MODE, GPIO_OSPEED_50MHZ, THROTTLE_5V_ADC_PIN);
#endif
#ifdef THROTTLE2_5V_ADC_GROUP
	rcu_periph_clock_enable(THROTTLE2_5V_ADC_RCU);
	/* configure ADC pin, bus voltage sampling -- ADC_IN0(PA0) */
	gpio_init(THROTTLE2_5V_ADC_GROUP, THROTTLE2_5V_ADC_MODE, GPIO_OSPEED_50MHZ, THROTTLE2_5V_ADC_PIN);
#endif

#ifdef U_VOL_ADC_GROUP
	rcu_periph_clock_enable(U_VOL_ADC_RCU);
	gpio_init(U_VOL_ADC_GROUP, U_VOL_ADC_MODE, GPIO_OSPEED_50MHZ, U_VOL_ADC_PIN);
#endif
#ifdef V_VOL_ADC_GROUP
	rcu_periph_clock_enable(V_VOL_ADC_RCU);
	gpio_init(V_VOL_ADC_GROUP, V_VOL_ADC_MODE, GPIO_OSPEED_50MHZ, V_VOL_ADC_PIN);
#endif
#ifdef W_VOL_ADC_GROUP
	rcu_periph_clock_enable(W_VOL_ADC_RCU);
	gpio_init(W_VOL_ADC_GROUP, W_VOL_ADC_MODE, GPIO_OSPEED_50MHZ, W_VOL_ADC_PIN);
#endif
#ifdef MOS_TEMP_ADC_GROUP
	rcu_periph_clock_enable(MOS_TEMP_ADC_RCU);
	gpio_init(MOS_TEMP_ADC_GROUP, MOS_TEMP_ADC_MODE, GPIO_OSPEED_50MHZ, MOS_TEMP_ADC_PIN);
#endif
#ifdef MOS_TEMP1_ADC_GROUP
	rcu_periph_clock_enable(MOS_TEMP1_ADC_RCU);
	gpio_init(MOS_TEMP1_ADC_GROUP, MOS_TEMP1_ADC_MODE, GPIO_OSPEED_50MHZ, MOS_TEMP1_ADC_PIN);
#endif

#ifdef MOTOR_TEMP_ADC_GROUP
	rcu_periph_clock_enable(MOTOR_TEMP_ADC_RCU);
	gpio_init(MOTOR_TEMP_ADC_GROUP, MOTOR_TEMP_ADC_MODE, GPIO_OSPEED_50MHZ, MOTOR_TEMP_ADC_PIN);
#endif

#ifdef ZERO_ADC_GROUP
	rcu_periph_clock_enable(ZERO_ADC_RCU);
	gpio_init(ZERO_ADC_GROUP, ZERO_ADC_MODE, GPIO_OSPEED_50MHZ, ZERO_ADC_PIN);
#endif

#ifdef DC5V_ADC_GROUP
	rcu_periph_clock_enable(DC5V_ADC_RCU);
	gpio_init(DC5V_ADC_GROUP, DC5V_ADC_MODE, GPIO_OSPEED_50MHZ, DC5V_ADC_PIN);
#endif
}

void adc_init(bool mot_ind) {
	/* config ADC clock */
    rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV4); //APB2 clk 120M, adc clk 30M
	rcu_periph_clock_enable(RCU_ADC0);
	rcu_periph_clock_enable(RCU_ADC1);
	adc_deinit(ADC0);
	adc_deinit(ADC1);

	adc_gpio_init();
	adc01_dma_init();
	if (!mot_ind) {
		adc01_init(ADC_REGCHAN_SAMPLE_TIME);
	}else {
		adc01_init(ADC_REGCHAN_MOT_IND_SAMPLE_TIME);
	}
	adc_current_sample_config(0);
}

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) {
	return (float)adc_buffer[ACC_V_BUFF_IDX] * VREF_3V3_COMPESTION();
}

u16 adc_get_ibus(void) {
	return (float)adc_buffer[VBUS_I_BUFF_IDX] * VREF_5V_COMPESTION();
}

u16 adc_get_throttle(void) {
#if CONFIG_MC105_HW_VERSION==3
	if (gpio_board_id() == CONFIG_MC105_VER3_ID2) { //v3 和 V4 adc 通道交换了一下
		return adc_buffer[THROTTLE_BUFF_IDX] * VREF_3V3_COMPESTION();
	}else {
		return adc_buffer[MOTOR_TEMP_BUFF_IDX] * VREF_3V3_COMPESTION();
	}
#else
	return adc_buffer[THROTTLE_BUFF_IDX] * VREF_3V3_COMPESTION();
#endif
}

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 0xFFFF;
#endif
}

u16 adc_get_thro2_5v(void) {
#ifdef THROTTLE2_5V_BUFF_IDX
	return adc_buffer[THROTTLE2_5V_BUFF_IDX] * VREF_3V3_COMPESTION();
#else
	return 0xFFFF;
#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) {
#if CONFIG_MC105_HW_VERSION==3
	if (gpio_board_id() == CONFIG_MC105_VER3_ID2) {
		return adc_buffer[MOTOR_TEMP_BUFF_IDX];
	}else {
		return adc_buffer[THROTTLE_BUFF_IDX];
	}
#else
	return adc_buffer[MOTOR_TEMP_BUFF_IDX];
#endif
}

u16 adc_get_vref(void) {
	return adc_buffer[VREF_BUFF_IDX];
}

u16 adc_get_5v_ref(void) {
	return adc_buffer[VREF5v_BUFF_IDX];
}

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

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

	adc_interrupt_enable(ADC0, ADC_INT_EOIC);

	adc_update_ext_trigger(ADC_TRIGGER_PHASE);
}

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

	adc_interrupt_disable(ADC0, ADC_INT_EOIC);

    /* clear the ADC flag */
    adc_flag_clear(ADC0, ADC_FLAG_EOIC);
	adc_flag_clear(ADC1, ADC_FLAG_EOIC);
}


s32 adc_sample_regular_channel(int channel, int times) {
#ifndef REG_CHAN_DMA
	u32 adc_device = ADC0;
	int value = 0;
	int count = 0;
	int min = 0xFFFFF;
	int max = -0xFFFFF;
	u64 start_time;
	adc_channel_length_config(adc_device, ADC_REGULAR_CHANNEL, 1);
	adc_regular_channel_config(adc_device, 0, channel, ADC_REGCHAN_SAMPLE_TIME);
	while(count < times){
restart:		
		start_time = shark_get_mseconds();
		adc_software_trigger_enable(adc_device, ADC_REGULAR_CHANNEL);
    	while(SET != adc_flag_get(adc_device, ADC_FLAG_EOC)){
			if (shark_get_mseconds() - start_time >= 2){
				goto restart;
			}
		};
    	int one = adc_regular_data_read(adc_device);
		adc_flag_clear(adc_device, ADC_FLAG_EOC);		
		value += (one & 0xFFF);
		count ++;
		if (one > max){
			max = one;
		}
		if (one < min) {
			min = one;
		}
	}
	if (times <= 2) {
		return value/times;
	}
	return (value - min - max)/(times-2);
#else
	return 0;
#endif
}