/**** Includes ****/
#include "hal/udccd_hal.h"
#include "analog.h"

/**** Private variables ****/
static const uint8_t ICOIL_MUL = 215;
static const uint8_t ICOIL_DIV = 22;
static const int16_t ICOIL_OFF = 0;
static const uint8_t ICOIL_CNT = 1;

static const uint8_t UCOIL_MUL = 20;
static const uint8_t UCOIL_DIV = 1;
static const int16_t UCOIL_OFF = 0;
static const uint8_t UCOIL_CNT = 1;

static const uint8_t UBAT_MUL = 20;
static const uint8_t UBAT_DIV = 1;
static const int16_t UBAT_OFF = 0;
static const uint8_t UBAT_CNT = 1;

static const uint8_t IBAT_MUL = 235;
static const uint8_t IBAT_DIV = 6;
static const int16_t IBAT_OFF = 0;
static const uint8_t IBAT_CNT = 1;

static const uint8_t POT_MUL = 215;
static const uint8_t POT_DIV = 44;
static const int16_t POT_OFF = 0;
static const uint8_t POT_CNT = 1;

static const uint8_t MODE_MUL = 215;
static const uint8_t MODE_DIV = 44;
static const int16_t MODE_OFF = 0;
static const uint8_t MODE_CNT = 1;


/**** Private function declarations ****/
static uint16_t ConvertSIntShort(int32_t in);
static uint16_t ConvertUintShort(uint32_t in);
static uint16_t read_ch(adcCh_t ch, uint16_t gnd_raw, uint8_t mul, uint8_t div, int16_t offset, uint8_t samples);

/**** Public function definitions ****/
void Analog_UpdateAll(analog_t* meas)
{
	uint16_t gnd_lvl = HAL_ADC_Read(ADC_GND);
	
	meas->hb_currnet     = read_ch(ADC_ICOIL, 0, ICOIL_MUL, ICOIL_DIV, ICOIL_OFF, ICOIL_CNT ); //mA
	meas->hb_voltage     = read_ch(ADC_UCOIL, 0, UCOIL_MUL, UCOIL_DIV, UCOIL_OFF, UCOIL_CNT ); //mV
	meas->supply_current = read_ch(ADC_IBAT , 0, IBAT_MUL , IBAT_DIV , IBAT_OFF , IBAT_CNT  ); //mA
	meas->supply_voltage = read_ch(ADC_UBAT , 0, UBAT_MUL , UBAT_DIV , UBAT_OFF , UBAT_CNT  ); //mV
	meas->pot_voltage    = read_ch(ADC_POT  , 0, POT_MUL  , POT_DIV  , POT_OFF  , POT_CNT   ); //mV
	meas->mode_voltage   = read_ch(ADC_MODE , 0, MODE_MUL , MODE_DIV , MODE_OFF , MODE_CNT  ); //mV
	
	// Calculate derived measurements
	// Halfbridge output power
	uint32_t temp = (uint32_t)meas->hb_currnet * (uint32_t)meas->hb_voltage;
	temp /= 1000;
	meas->hb_power = ConvertUintShort(temp);
	
	// Halfbridge output resistance
	if(meas->hb_currnet == 0) meas->hb_resistance = 0xFFFF;
	else if(meas->hb_voltage == 0) meas->hb_resistance = 0;
	else
	{
		temp = (uint32_t)meas->hb_voltage * 1000;
		temp /= (uint32_t)meas->hb_currnet;
		meas->hb_resistance = ConvertUintShort(temp);
	}
	
	// Supply power
	temp = (uint32_t)meas->supply_current * (uint32_t)meas->supply_voltage;
	temp /= 1000;
	meas->supply_power = ConvertUintShort(temp);
}

static uint16_t read_ch(adcCh_t ch, uint16_t gnd_raw, uint8_t mul, uint8_t div, int16_t offset, uint8_t samples)
{
	//Sanity check
	if(div==0) return 0xFFFF;
	
	// Do at least one sample
	if(samples<1) samples = 1;
	
	// Read ADC value
	int32_t raw = 0;
	for(uint8_t i=0; i<samples; i++)
	{
		raw += (int32_t)HAL_ADC_Read(ch);
	}
	
	// Do average
	if(samples != 1)
	{
		raw /= samples;
	};
	
	// Remove GND level
	if(gnd_raw)	raw = raw - (int32_t)gnd_raw;
	
	// Convert to target units	
	raw = raw * mul;
	if(div!=1) raw /= div;
	raw += offset;
	
	return ConvertSIntShort(raw);
}

static uint16_t ConvertSIntShort(int32_t in)
{
	if(in <= 0) return 0;
	else if(in >= 0x0000FFFF) return 0xFFFF;
	else return (uint16_t)in;
}

static uint16_t ConvertUintShort(uint32_t in)
{
	if(in == 0) return 0;
	else if(in >= 0x0000FFFF) return 0xFFFF;
	else return (uint16_t)in;
}