#include "bsp/adc.h"
#include "foc_type.h"
#include "libs/utils.h"
#include "libs/logger.h"
#define MOS_Rds_Calibrate 1

#define NB_OFFSET_SAMPLES 32

#define Rvbus  0.0005f
#define Gvbus (13.1f) //母线电流的运放 
#define Rds_Defualt  0.00263f//欧
#define Gmos  (1.7f)//mos 电流的运放
#define Sample_R Rds_Defualt
#define Lower_Pass_p 0.2f

#define VBUS_VOL(adc) ((adc) * 3.3f / 4096.0f / Gvbus)
#define MOSds_VOL(adc) ((adc) * 3.3f / 4096.0f / Gmos)

#define current_i(v, r) ((v)/(r))

void phase_current_init(current_samp_t *cs) {
	cs->offset_sample_count = NB_OFFSET_SAMPLES;
	cs->adc_offset_a = 0;
	cs->adc_offset_b = 0;
	cs->adc_offset_c = 0;
}


void phase_current_offset(current_samp_t *cs) {
	s32 phase_current1, phase_current2;

	adc_disable_ext_trigger();

	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
	if (cs->offset_sample_count > 0) {
		cs->offset_sample_count--;
		if (cs->sector == SECTOR_1 && cs->offset_sample_count >= 0) {
			cs->adc_offset_a += phase_current1;
			cs->adc_offset_c += phase_current2;
			if (cs->offset_sample_count == 0) {
				cs->adc_offset_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
				cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
			}
		}
		if (cs->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
			cs->adc_offset_b += phase_current1;
			if (cs->offset_sample_count == 0) {
				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
			}
		}
	}
}

void phase_current_sample(current_samp_t *cs){
	s32 phase_current1, phase_current2;

	adc_disable_ext_trigger();
	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
	if (cs->sector == SECTOR_3 || cs->sector == SECTOR_6) {
		/* Current on Phase C is not accessible */
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Sample_R);
		cs->Ia = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_a), Sample_R);
		cs->Ic = -(cs->Ia + cs->Ib);
	}else if (cs->sector == SECTOR_2 || cs->sector == SECTOR_5) {
		/* Current on Phase A is not accessible 	*/
		/* Ib = PhaseBOffset - ADC converted value) */
		cs->Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Sample_R);
		cs->Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Sample_R);
		cs->Ia = -(cs->Ib + cs->Ic);
	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_4) {
		/* Current on Phase B is not accessible 	*/
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->Ia = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_a), Sample_R);
		cs->Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Sample_R);
		cs->Ib = -(cs->Ia + cs->Ic);
	}
}


u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector){
	u32 low_side_low_duty = FOC_PWM_Half_Period - time->low;	
	cs->sector = sector;
	//duty > deadtime + max(Rise time, Noise time)
	if (low_side_low_duty > (TDead + MAX(TRise, TNoise))) {
		cs->sector = SECTOR_1;
		return FOC_PWM_Half_Period - 1;
	}else {
		u32 low_side_mid_duty = FOC_PWM_Half_Period - time->midle;
		u32 delta_duty = low_side_mid_duty - low_side_low_duty;
		if (delta_duty > low_side_low_duty * 2) {
			return time->low - TADC;
		}else {
			u32 sample_point = time->low + (TDead + MAX(TRise, TNoise));
			if (sample_point >= FOC_PWM_Half_Period) {
				//这里需要修改触发方式，GD不支持adc设置上升或下降沿触发，考虑切换pwm模式？？？
				sample_point = ( 2u * FOC_PWM_Half_Period ) - sample_point - (uint16_t) 1;		
			}
			return sample_point;
		}
	}
}

u32 get_vbus_sample_point(current_samp_t *cs, phase_time_t *time){
	return 0;
}

void phase_current_adc_triger(current_samp_t *cs){
	adc_phase_inserted_config(cs->sector);
	adc_enable_ext_trigger();
}