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

static float __inline adc_to_current(u32 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/4095.0f * 3.3f / 0.001f);
}

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

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_4 || cs->sector == SECTOR_5) {
		/* Current on Phase C is not accessible */
      	/* Ia = PhaseAOffset - ADC converted value) */
		cs->ia = adc_to_current(phase_current1 - cs->adc_offset_a);
		cs->ib = adc_to_current(phase_current2 - cs->adc_offset_b);
		cs->ic = -(cs->ia + cs->ib);
	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_6) {
		/* Current on Phase A is not accessible 	*/
		/* Ib = PhaseBOffset - ADC converted value) */
		cs->ib = adc_to_current(phase_current1 - cs->adc_offset_b);
		cs->ic = adc_to_current(phase_current2 - cs->adc_offset_c);
		cs->ia = -(cs->ib + cs->ic);
	}else if (cs->sector == SECTOR_2 || cs->sector == SECTOR_3) {
		/* Current on Phase B is not accessible 	*/
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->ia = adc_to_current(phase_current1 - cs->adc_offset_a);
		cs->ic = adc_to_current(phase_current2 - cs->adc_offset_c);
		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_5;
		return FOC_PWM_Half_Period - 1;
	}else {
		u32 low_side_mid_duty = FOC_PWM_period/2 - 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;
}