#include "bsp/adc.h"
#include "foc_type.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 Lower_Pass_p 0.2f

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

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

/* Ids 通过母线采样电阻测量 */
static __inline__ void _calc_mos_rds(u32 Vds, u32 Ids, float *dest) {
	*dest = (*dest) * (1.0f - Lower_Pass_p) + Vds/Ids * Lower_Pass_p;
}

void phase_current_init(current_samp_t *cs) {
	cs->offset_sample_count = NB_OFFSET_SAMPLES;
	cs->vbus_i_invert = INVERT_NO;
	cs->Rds_a = Rds_Defualt;
	cs->Rds_b = Rds_Defualt;
	cs->Rds_c = Rds_Defualt;
}


void phase_current_offset(current_samp_t *cs) {
	u32 phase_current1, phase_current2;
	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);

	if (cs->offset_sample_count > 0) {
		cs->offset_sample_count--;
		if (cs->sector == SECTOR_5 && 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_1 && cs->offset_sample_count >= 0) {
			cs->adc_offset_b += phase_current1;
			cs->adc_offset_ivbus += 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){
		u32 phase_current1, phase_current2;

		adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
		if (cs->sector == SECTOR_1 || cs->sector == SECTOR_2) {
			/* Current on Phase C is not accessible */
			/* Ia = PhaseAOffset - ADC converted value) */
			cs->Ib = current_i(MOSds_VOL((int)phase_current1 - (int)cs->adc_offset_b), cs->Rds_b);
			cs->Ia = current_i(MOSds_VOL((int)phase_current2 - (int)cs->adc_offset_a), cs->Rds_a);
			cs->Ic = -(cs->Ia + cs->Ib);
		}else if (cs->sector == SECTOR_3 || cs->sector == SECTOR_4) {
			/* Current on Phase A is not accessible 	*/
			/* Ib = PhaseBOffset - ADC converted value) */
			cs->Ic = current_i(MOSds_VOL((int)phase_current1 - (int)cs->adc_offset_c), cs->Rds_c);
			cs->Ib = current_i(MOSds_VOL((int)phase_current2 - (int)cs->adc_offset_b), cs->Rds_b);
			cs->Ia = -(cs->Ib + cs->Ic);		
		}else if (cs->sector == SECTOR_5 || cs->sector == SECTOR_6) {
			/* Current on Phase B is not accessible 	*/
			/* Ia = PhaseAOffset - ADC converted value) */
			cs->Ia = current_i(MOSds_VOL((int)phase_current1 - (int)cs->adc_offset_a), cs->Rds_a);
			cs->Ic = current_i(MOSds_VOL((int)phase_current2 - (int)cs->adc_offset_c), cs->Rds_c);
			cs->Ib = -(cs->Ia + cs->Ic);
		}
#if 0
		static int tet_p = 0;
		if (tet_p++ % 5 == 0) {
			printf("$%d %d %d;", (int)(cs->Ia * 1000), (int)(cs->Ib*1000), (int)(cs->Ic*1000));
		}
#endif	

}

/* 校准mos的 drain-source 内阻 */
void phase_Rds_calibrate(current_samp_t *cs) {	
	if (cs->vbus_i_invert != INVERT_NO) {
		u32 adcm;
		u32 adcv;
		adc_cali_current_read(&adcm, &adcv);
		float vbus_i = VBUS_VOL(adcv)/Rvbus;
		if (cs->vbus_i_invert == INVERT_A) {
			_calc_mos_rds(adcm, vbus_i, &cs->Rds_a);
		}else if (cs->vbus_i_invert == INVERT_B) {
			_calc_mos_rds(adcm, vbus_i, &cs->Rds_b);
		}else if (cs->vbus_i_invert == INVERT_C) {
			_calc_mos_rds(adcm, vbus_i, &cs->Rds_c);
		}
		cs->vbus_i_invert = INVERT_NO;		
	}
	adc_config_trigger(ADC_TRIGGER_PHASE);
	adc_phase_inserted_config(cs->sector);
}

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) {
				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){
	if (cs->vbus_i_invert != INVERT_NO) {
		adc_config_trigger(ADC_TRIGGER_VBUS);
		adc_cali_inserted_config(cs->vbus_i_invert-1);
	}else {
		adc_config_trigger(ADC_TRIGGER_PHASE);
		adc_phase_inserted_config(cs->sector);
	}
}