#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 get_phase_current(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){
	cs->sector = sector;
	return 0;
}