#include "sdadc.h"
#include "main.h"
#include "math.h"

extern SDADC_HandleTypeDef SDADC;
int16_t sdadc_samples[NSADC_CH][NSADC_SAMPLES];
uint16_t ptr_sadc_int, ptr_sadc_sample[NSADC_CH];
float sdadc_result[NSADC_CH];
uint8_t sdadc_init_stack = 0;
__IO uint32_t sdInjChannel = 0;

void sdadc_init() {
	SDADC_ConfParamTypeDef confParam = {0};

	/* -1- Initialise the SDADC */
	if (HAL_SDADC_Init(&SDADC) != HAL_OK)
	{
	;
	}

	/* -2- Prepare the channel configuration */
	confParam.CommonMode = SDADC_COMMON_MODE_VSSA;
	confParam.Gain = SDADC_GAIN_1;
	confParam.InputMode = SDADC_INPUT_MODE_SE_ZERO_REFERENCE;
	confParam.Offset = 0x00000000;
	if (HAL_SDADC_PrepareChannelConfig(&SDADC, SDADC_CONF_INDEX_0, &confParam) != HAL_OK)
	{
	;
	}

	/* associate POT_SDADC_CHANNEL to the configuration 0 */
	if (HAL_SDADC_AssociateChannelConfig(&SDADC, SDADC_CHANNEL_4|SDADC_CHANNEL_5|SDADC_CHANNEL_6, SDADC_CONF_INDEX_0) != HAL_OK)
	{
	;
	}


	/* select POT_SDADC_CHANNEL for injected conversion and continuous mode */
	if (HAL_SDADC_InjectedConfigChannel(&SDADC,  SDADC_CHANNEL_4|SDADC_CHANNEL_5|SDADC_CHANNEL_6, SDADC_CONTINUOUS_CONV_OFF) != HAL_OK)
	{
	;
	}

	/* Select external trigger for injected conversion */
	if (HAL_SDADC_SelectInjectedTrigger(&SDADC, SDADC_SOFTWARE_TRIGGER) != HAL_OK)
	{
	;
	}

	HAL_SDADC_SelectInjectedDelay(&SDADC, SDADC_INJECTED_DELAY);

//	/* Start Calibration in polling mode */
//	if (HAL_SDADC_CalibrationStart(&SDADC, SDADC_CALIBRATION_SEQ_1) != HAL_OK)
//	{
//	;
//	}
//
//	/* Pool for the end of calibration */
//	if (HAL_SDADC_PollForCalibEvent(&SDADC, HAL_MAX_DELAY) != HAL_OK)
//	{
//	;
//	}

	/* Start injected conversion in interrupt mode */
//	if (HAL_SDADC_InjectedStart_IT(&SDADC) != HAL_OK)
//	{
//	;
//	}
}

void sd_adc_tasks() {

	if (((ptr_sadc_int % NSADC_CH) == 0 )||SDADC.InjConvRemaining == 3){
		HAL_SDADC_InjectedStop_IT(&SDADC);

		/* Start injected conversion in interrupt mode */
		if (HAL_SDADC_InjectedStart_IT(&SDADC) != HAL_OK)
		{
		;
		}
	}

	if (ptr_sadc_int >=(NSADC_CH*NSADC_SAMPLES)) sdadc_init_stack = 1;
	if (sdadc_init_stack)  {
		ptr_sadc_int = 0;
		int i = 0;
		for (i = 0; i < NSADC_CH; i++) {
			float sample_avg = 0;

			int j = 0;
			for (j = 0; j < NSADC_SAMPLES; j++) {
				sample_avg += sdadc_samples[i][j];
			}
			sample_avg = sample_avg / NSADC_SAMPLES;
			sdadc_result[i] = (sample_avg + 32768) * 1.225 / 65536;

//			if (i == 1) {
//				sdadc_result[i] = sdadc_result[i] * 0.9725 - 0.01472;
//			}

			if (i == 2) {
				sdadc_result[i] = sdadc_result[i] * 0.9626 - 0.0128;
			}

			if (i == 3) {
				sdadc_result[i] = sdadc_result[i] * 0.9725 - 0.01472;
			}
			sdadc_result[i] = roundf(sdadc_result[i] * 1000) / 1000; //only 3 decimal values
		}
	}
}

void HAL_SDADC_InjectedConvCpltCallback(SDADC_HandleTypeDef *hsdadc)
{

	int16_t value = (int16_t) HAL_SDADC_InjectedGetValue(hsdadc, (uint32_t *) &sdInjChannel);
	uint8_t ch = sdInjChannel - 4;
  /* Get the converted value */
  	sdadc_samples[ch][ptr_sadc_sample[ch]] = value;
  	ptr_sadc_sample[ch]++;
  	if (ptr_sadc_sample[ch] >=NSADC_SAMPLES) ptr_sadc_sample[ch] = 0;
  	ptr_sadc_int++;

}