#include "hal/adc.h"
#include "foc_type.h"

#define NB_OFFSET_SAMPLES 32
static float __inline adc_to_current(int adc){
	int i_adc = (int)adc;
	if (i_adc > INT16_MAX){
		i_adc = INT16_MAX;
	}else if (i_adc < -INT16_MAX) {
		i_adc = - INT16_MAX;
	}
	return (i_adc/65535.0f * 3.3f / 1.53f / 0.33f);
}

void phase_current_init(current_samp_t *cs) {
	cs->adc_inject_flags = LL_ADC_INJ_TRIG_EXT_RISING;
	cs->offset_sample_count = NB_OFFSET_SAMPLES;
}


void phase_current_offset(current_samp_t *cs) {
	u32 phase_current1, phase_current2;
	HAL_ADC1_Inject_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;
			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;
	HAL_ADC1_Inject_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 = adc_to_current((int)phase_current1 - (int)cs->adc_offset_b);
		cs->Ia = adc_to_current((int)phase_current2 - (int)cs->adc_offset_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 = adc_to_current((int)phase_current1 - (int)cs->adc_offset_c);
		cs->Ib = adc_to_current((int)phase_current2 - (int)cs->adc_offset_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 = adc_to_current((int)phase_current1 - (int)cs->adc_offset_a);
		cs->Ic = adc_to_current((int)phase_current2 - (int)cs->adc_offset_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	
}


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) {
				/* ADC trigger edge must be changed from positive to negative */
				cs->adc_inject_flags= (uint16_t) LL_ADC_INJ_TRIG_EXT_FALLING;
				sample_point = ( 2u * FOC_PWM_Half_Period ) - sample_point - (uint16_t) 1;		
			}
			return sample_point;
		}
	}
}

void phase_current_adc_triger(current_samp_t *cs){
	HAL_ADC1_Inject_Config(cs->sector, cs->adc_inject_flags);
	cs->adc_inject_flags = LL_ADC_INJ_TRIG_EXT_RISING;
}