#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==3)
#define ADC01_NUM (8)

#define VBUS_V_BUFF_IDX 0
#define ACC_V_BUFF_IDX 2
#define VBUS_I_BUFF_IDX 4
#define V_VOL_BUFF_IDX 6
#define THROTTLE_5V_BUFF_IDX 8
//zero chan             10
#define VREF_BUFF_IDX   12
#define VREF5v_BUFF_IDX 14


#define MOS_TEMP_BUFF_IDX 1
#define MOTOR_TEMP_BUFF_IDX 3
//zero                      5
#define THROTTLE_BUFF_IDX 7
#define THROTTLE2_BUFF_IDX 9
#define U_VOL_BUFF_IDX 11
#define W_VOL_BUFF_IDX 13
//zero                 15

#define REG_CHAN_NUM (ADC01_NUM + ADC01_NUM)
#define ADC_DUAL_MODE ADC_DAUL_REGULAL_PARALLEL_INSERTED_PARALLEL

#endif

u16 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_InitType DMA_InitStructure;
    rcu_ahb_periph_clock_enable(RCU_DMA1);

    DMA_DeInit(DMA1_CH1);
    DMA_InitStructure.PeriphAddr     = (uint32_t)&ADC1->DAT;
    DMA_InitStructure.MemAddr        = (uint32_t)adc_buffer;
    DMA_InitStructure.Direction      = DMA_DIR_PERIPH_SRC;
    DMA_InitStructure.BufSize        = REG_CHAN_NUM/2;
    DMA_InitStructure.PeriphInc      = DMA_PERIPH_INC_DISABLE;
    DMA_InitStructure.DMA_MemoryInc  = DMA_MEM_INC_ENABLE;
    DMA_InitStructure.PeriphDataSize = DMA_PERIPH_DATA_SIZE_WORD;
    DMA_InitStructure.MemDataSize    = DMA_MemoryDataSize_Word;
    DMA_InitStructure.CircularMode   = DMA_MODE_CIRCULAR;
    DMA_InitStructure.Priority       = DMA_PRIORITY_HIGH;
    DMA_InitStructure.Mem2Mem        = DMA_M2M_DISABLE;
    DMA_Init(DMA1_CH1, &DMA_InitStructure);
    /* Enable DMA1 Channel1 */
    DMA_EnableChannel(DMA1_CH1, ENABLE);
}

static void adc01_init(void) {
	ADC_InitType ADC_InitStructure;
    /* config ADC clock */
    RCC_ConfigAdcHclk(RCC_ADCHCLK_DIV4);

	rcu_ahb_periph_clock_enable(RCU_ADC1);
	rcu_ahb_periph_clock_enable(RCU_ADC2);

	ADC_DeInit(ADC1);
	ADC_DeInit(ADC2);

	//ADC1 and ADC2 configuration
	ADC_InitStruct(&ADC_InitStructure);
	ADC_InitStructure.WorkMode = ADC_WORKMODE_REG_INJECT_SIMULT;
	ADC_InitStructure.MultiChEn = ENABLE;
	ADC_InitStructure.ContinueConvEn = ENABLE;
	ADC_InitStructure.ExtTrigSelect = ADC_EXT_TRIG_INJ_CONV_T1_CC4 | ADC_EXT_TRIGCONV_NONE; //inject trigger T1CC4, regular trigger software
	ADC_InitStructure.DatAlign = ADC_DAT_ALIGN_R;
	ADC_InitStructure.ChsNumber = ADC01_NUM;
	ADC_Init(ADC1, &ADC_InitStructure);
	ADC_Init(ADC2, &ADC_InitStructure);

	//Config Sampling Time
	ADC_ConfigInjectedSequencerLength(ADC1,INJ_CHAN_NUM);
	ADC_ConfigInjectedChannel(ADC1, V_PHASE_I_CHAN, 1, ADC_INJ_SAMPLE_TIME);
#if (INJ_CHAN_NUM == 4)
	ADC_ConfigInjectedChannel(ADC1, V_PHASE_I_CHAN, 2, ADC_INJ_SAMPLE_TIME);
	ADC_ConfigInjectedChannel(ADC1, V_PHASE_I_CHAN, 3, ADC_INJ_SAMPLE_TIME);
	ADC_ConfigInjectedChannel(ADC1, V_PHASE_I_CHAN, 4, ADC_INJ_SAMPLE_TIME);
#endif
	ADC_ConfigInjectedSequencerLength(ADC2,INJ_CHAN_NUM);
	ADC_ConfigInjectedChannel(ADC2, W_PHASE_I_CHAN, 1, ADC_INJ_SAMPLE_TIME);
#if (INJ_CHAN_NUM == 4)
	ADC_ConfigInjectedChannel(ADC2, W_PHASE_I_CHAN, 2, ADC_INJ_SAMPLE_TIME);
	ADC_ConfigInjectedChannel(ADC2, W_PHASE_I_CHAN, 3, ADC_INJ_SAMPLE_TIME);
	ADC_ConfigInjectedChannel(ADC2, W_PHASE_I_CHAN, 4, ADC_INJ_SAMPLE_TIME);
#endif
	//ADC1 and ADC2 TrigInJectConv Enable
	ADC_EnableExternalTrigInjectedConv(ADC1,ENABLE);
	ADC_EnableExternalTrigInjectedConv(ADC2,ENABLE);

	ADC_ConfigRegularChannel(ADC1, VBUS_V_CHAN, 1, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC1, ACC_V_CHAN, 2, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC1, VBUS_I_CHAN, 3, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC1, V_VOL_ADC_CHAN, 4, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC1, THROTTLE_5V_CHAN, 5, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC1, ZERO_ADC_CHAN, 6, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC1, ADC_CH_INT_VREF, 7, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC1, DC5V_ADC_CHAN, 8, ADC_REGCHAN_SAMPLE_TIME); 
	ADC_EnableExternalTrigConv(ADC1, ENABLE);
	
    ADC_ConfigRegularChannel(ADC2, MOS_TEMP_ADC_CHAN, 1, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC2, MOTOR_TEMP_ADC_CHAN, 2, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC2, ZERO_ADC_CHAN, 3, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC2, THROTTLE_CHAN, 4, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC2, THROTTLE2_CHAN, 5, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC2, U_VOL_ADC_CHAN, 6, ADC_REGCHAN_SAMPLE_TIME);
	ADC_ConfigRegularChannel(ADC2, W_VOL_ADC_CHAN, 7, ADC_REGCHAN_SAMPLE_TIME); //adc12
	ADC_ConfigRegularChannel(ADC2, ZERO_ADC_CHAN, 8, ADC_REGCHAN_SAMPLE_TIME);	

	ADC_EnableExternalTrigConv(ADC2, ENABLE);

	/* Enable Vrefint channel17 */
	ADC_EnableTempSensorVrefint(ENABLE);

	ADC_EnableDMA(ADC1, ENABLE);

	/* Enable ADC1 */
	ADC_Enable(ADC1, ENABLE);
	/*Check ADC Ready*/
	while(ADC_GetFlagStatusNew(ADC1,ADC_FLAG_RDY) == RESET)
		 ;
	/* Start ADC1 calibration */
	ADC_StartCalibration(ADC1);
	/* Check the end of ADC1 calibration */
	while (ADC_GetCalibrationStatus(ADC1))
		 ;
	
	/* Enable ADC2 */
	ADC_Enable(ADC2, ENABLE);
	/*Check ADC Ready*/
	while(ADC_GetFlagStatusNew(ADC2,ADC_FLAG_RDY) == RESET)
		 ;
	/* Start ADC2 calibration */
	ADC_StartCalibration(ADC2);
	/* Check the end of ADC2 calibration */
	while (ADC_GetCalibrationStatus(ADC2))
		 ;
	
	adc_disable_ext_trigger();

	nvic_irq_enable(ADC1_2_IRQn, ADC_IRQ_PRIORITY, 0);

	/* Start ADC1 Software Conversion */
	ADC_EnableSoftwareStartConv(ADC1, ENABLE);
}


static void adc_gpio_init(void) {

	rcu_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_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_apb2_periph_clock_enable(ZERO_ADC_RCU);
	gpio_init(ZERO_ADC_GROUP, ZERO_ADC_MODE, GPIO_OSPEED_50MHZ, ZERO_ADC_PIN);
#endif

}

void adc_init(void) {
	adc_gpio_init();
	adc01_dma_init();
	adc01_init();
	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) {
#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_ClearFlag(ADC1, ADC_FLAG_JENDC);
	ADC_ClearFlag(ADC2, ADC_FLAG_JENDC);

	adc_enable_ext_trigger();
	while(drop-- > 0) {
		while (ADC_GetFlagStatus(ADC1, ADC_FLAG_JENDC) == RESET);
		ADC_ClearFlag(ADC1, ADC_FLAG_JENDC);
	}
    /* enable ADC interrupt */

	ADC_ConfigInt(ADC1, ADC_INT_JENDC, ENABLE);
	
	adc_update_ext_trigger(ADC_TRIGGER_PHASE);
}

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

	ADC_ConfigInt(ADC1, ADC_INT_JENDC, DISABLE);

    /* clear the ADC flag */
    ADC_ClearFlag(ADC1, ADC_FLAG_JENDC);
	ADC_ClearFlag(ADC2, ADC_FLAG_JENDC);
}