#include <libopencm3/stm32/adc.h>
#include <libopencm3/stm32/dma.h>

#include <fxp.h>

#include "pinout.h"

#include "measurement.h"

#define ADC_NUM_CHANNELS 6
volatile int16_t adc_values[ADC_NUM_CHANNELS];


/* Temperature sensor calibration value address */
#define TEMP110_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7C2))
#define TEMP30_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7B8))
#define VDD_CALIB ((uint16_t) (330)) /* calibration voltage = 3,30V - DO NOT CHANGE */
#define VDD_APPLI ((uint16_t) (330)) /* actual supply voltage */

/* function for temperature conversion */
static fxp_t calc_temperature(uint16_t adc_val)
{
	fxp_t temperature = FXP_FROM_INT(
			((int32_t)adc_val * VDD_APPLI / VDD_CALIB) -
			 (int32_t)*TEMP30_CAL_ADDR);

	temperature = fxp_mult(temperature, FXP_FROM_INT(110 - 30));
	temperature = fxp_div(temperature, FXP_FROM_INT(*TEMP110_CAL_ADDR - *TEMP30_CAL_ADDR));

	return fxp_add(temperature, FXP_FROM_INT(30));
}


static fxp_t adc_val_to_pin_voltage(uint16_t adc_val)
{
	return fxp_div(
	           fxp_mult(FXP_FROM_INT(adc_val), fxp_div(FXP_FROM_INT(33), FXP_FROM_INT(10))),
	           FXP_FROM_INT(4096));
}


static fxp_t convert_voltage_divider(uint16_t adc_val, fxp_t r1, fxp_t r2)
{
	fxp_t pin_voltage = adc_val_to_pin_voltage(adc_val);

	return fxp_mult(pin_voltage, fxp_div(fxp_add(r1, r2), r2));
}


static fxp_t convert_ina139(uint16_t adc_val, fxp_t rshunt, fxp_t vgain)
{
	fxp_t pin_voltage = adc_val_to_pin_voltage(adc_val);

	fxp_t shunt_voltage = fxp_div(pin_voltage, vgain);
	fxp_t current = fxp_div(shunt_voltage, rshunt);

	return current;
}


void measurement_init(void)
{
	uint8_t channels[ADC_NUM_CHANNELS] = {
		ANALOG_INPUT_U_BAT,   // U_Bat
		ANALOG_INPUT_U_SOLAR, // U_Solar
		ANALOG_INPUT_U_SW,    // U_SW
		ANALOG_INPUT_I_SOLAR, // I_Solar
		ANALOG_INPUT_I_LOAD,  // I_Load
		16                    // Temperature sensor
	};

	adc_power_off(ADC1);

	// enable the temperature sensor
	adc_enable_temperature_sensor();

	// configure ADC
	//adc_enable_scan_mode(ADC1);
	adc_set_single_conversion_mode(ADC1);
	adc_set_resolution(ADC1, ADC_RESOLUTION_12BIT);
	adc_set_sample_time_on_all_channels(ADC1, ADC_SMPR_SMP_239DOT5);
	adc_disable_external_trigger_regular(ADC1);
	adc_set_right_aligned(ADC1);
	adc_set_regular_sequence(ADC1, ADC_NUM_CHANNELS, channels);

	// configure DMA for ADC

	//nvic_enable_irq(NVIC_DMA1_STREAM5_IRQ);
	dma_channel_reset(DMA1, DMA_CHANNEL1);
	dma_set_priority(DMA1, DMA_CHANNEL1, DMA_CCR_PL_LOW);
	dma_set_memory_size(DMA1, DMA_CHANNEL1, DMA_CCR_MSIZE_16BIT);
	dma_set_peripheral_size(DMA1, DMA_CHANNEL1, DMA_CCR_PSIZE_16BIT);
	dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL1);
	dma_enable_circular_mode(DMA1, DMA_CHANNEL1);
	dma_set_read_from_peripheral(DMA1, DMA_CHANNEL1);
	dma_set_peripheral_address(DMA1, DMA_CHANNEL1, (uint32_t) &ADC1_DR);
	/* The array adc_values[] is filled with the waveform data to be output */
	dma_set_memory_address(DMA1, DMA_CHANNEL1, (uint32_t) adc_values);
	dma_set_number_of_data(DMA1, DMA_CHANNEL1, ADC_NUM_CHANNELS);
	//dma_enable_transfer_complete_interrupt(DMA1, DMA_CHANNEL1);
	dma_enable_channel(DMA1, DMA_CHANNEL1);

	adc_enable_dma(ADC1);

	adc_power_on(ADC1);
}


void measurement_start(void)
{
	// start the ADC conversion sequency. The result will be transferred to RAM
	// by the DMA.
	adc_start_conversion_regular(ADC1);
}


void measurement_wait_for_completion(void)
{
	// wait for DMA transfer to complete
	while(!dma_get_interrupt_flag(DMA1, DMA_CHANNEL1, DMA_TCIF));
	dma_clear_interrupt_flags(DMA1, DMA_CHANNEL1, DMA_TCIF);
}


void measurement_finalize(struct MeasurementResult *result)
{
	result->u_bat = convert_voltage_divider(adc_values[ANALOG_INPUT_U_BAT],
	                                        FXP_FROM_INT(220),
	                                        FXP_FROM_INT(22));

	result->u_solar = convert_voltage_divider(adc_values[ANALOG_INPUT_U_SOLAR],
	                                        FXP_FROM_INT(330),
	                                        FXP_FROM_INT(22));

	result->u_sw = convert_voltage_divider(adc_values[ANALOG_INPUT_U_SW],
	                                        FXP_FROM_INT(1000),
	                                        FXP_FROM_INT(47));

	result->i_solar = convert_ina139(adc_values[ANALOG_INPUT_I_SOLAR],
	                                 fxp_div(FXP_FROM_INT(2), FXP_FROM_INT(1000)),
	                                 FXP_FROM_INT(56));

	result->i_load = convert_ina139(adc_values[ANALOG_INPUT_I_LOAD],
	                                fxp_div(FXP_FROM_INT(5), FXP_FROM_INT(1000)),
	                                FXP_FROM_INT(56));

	result->temperature = calc_temperature(adc_values[5]);
}