/* Reflow Oven Controller
 *
 * Copyright (C) 2020  Mario Hüttel <mario.huettel@gmx.net>
 *
 * This file is part of the Reflow Oven Controller Project.
 *
 * The reflow oven controller is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * The Reflow Oven Control Firmware is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with the reflow oven controller project.
 * If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file adc-meas.c
 * @brief Implementation of the PT1000 measurement ADC and filtering functions
 */

#include <reflow-controller/adc-meas.h>
#include <stm32/stm32f4xx.h>
#include <cmsis/core_cm4.h>
#include <stm-periph/stm32-gpio-macros.h>
#include <stdlib.h>
#include <helper-macros/helper-macros.h>
#include <stm-periph/rcc-manager.h>
#include <reflow-controller/safety/safety-controller.h>

static float IN_SECTION(.ccm.bss) pt1000_offset;
static float IN_SECTION(.ccm.bss) pt1000_sens_dev;
static bool IN_SECTION(.ccm.bss) calibration_active;
static float IN_SECTION(.ccm.bss) filter_alpha;

/**
 * @brief Filtered PT1000 resistance value.
 * @note This value is not yet calibrated.
 * Use @ref adc_pt1000_get_current_resistance to get this value with calibration.
 */
static volatile float IN_SECTION(.ccm.bss) pt1000_res_raw_lf;

static volatile int * volatile streaming_flag_ptr;
static volatile float IN_SECTION(.ccm.bss) adc_pt1000_raw_reading_hf;
static volatile uint16_t dma_sample_buffer[ADC_PT1000_DMA_AVG_SAMPLES];
static volatile uint32_t IN_SECTION(.ccm.bss) adc_watchdog_counter;

volatile float * volatile stream_buffer;
volatile uint32_t stream_count;
volatile uint32_t stream_pos;

static inline void adc_pt1000_stop_sample_frequency_timer(void)
{
	TIM2->CR1 &= ~TIM_CR1_CEN;
	rcc_manager_disable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(RCC_APB1ENR_TIM2EN));
}

static inline void adc_pt1000_setup_sample_frequency_timer(void)
{
	rcc_manager_enable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(RCC_APB1ENR_TIM2EN));

	/* Divide 2*42 MHz peripheral clock by 42 */
	TIM2->PSC = (84UL-1UL);

	/* Reload value */
	TIM2->ARR = ADC_PT1000_SAMPLE_CNT_DELAY;

	/* Trigger output at update event */
	TIM2->CR2 = TIM_CR2_MMS_1;

	/* Start Timer in downcounting mode with autoreload */
	TIM2->CR1 = TIM_CR1_DIR | TIM_CR1_CEN;

}

static inline void adc_pt1000_disable_adc(void)
{
	ADC_PT1000_PERIPH->CR2 &= ~ADC_CR2_ADON;
	DMA2_Stream0->CR = 0;

	safety_controller_set_crc_monitor(ERR_CRC_MON_MEAS_ADC, SAFETY_CRC_MON_MEAS_ADC_PW);

	safety_controller_report_error_with_key(ERR_FLAG_MEAS_ADC_OFF, MEAS_ADC_SAFETY_FLAG_KEY);
	safety_controller_enable_timing_mon(ERR_TIMING_MEAS_ADC, false);
	rcc_manager_disable_clock(&RCC->APB2ENR, BITMASK_TO_BITNO(RCC_APB2ENR_ADC3EN));
	rcc_manager_disable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(ADC_PT1000_PORT_RCC_MASK));
}

/**
 * @brief Enable DMA Stream for ADC
 *
 * DMA2 Stream 0 is used. It will capture @ref ADC_PT1000_DMA_AVG_SAMPLES measurement values,
 * delete the two most extreme values
 * and calculate the avereage over the remaining values. This ensures, that one time errors are
 * not included in the measurement.
 *
 * After that, the moving average filter is fed with the values.
 *
 */
static inline void adc_pt1000_enable_dma_stream(void)
{
	/* Enable peripheral clock for DMA2 */
	rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(RCC_AHB1ENR_DMA2EN));

	/* Destination is the DMA sample buffer */
	DMA2_Stream0->M0AR = (uint32_t)dma_sample_buffer;

	/* Source is the ADC data register */
	DMA2_Stream0->PAR = (uint32_t)&ADC_PT1000_PERIPH->DR;

	/* Transfer size is ADC_PT1000_DMA_AVG_SAMPLES */
	DMA2_Stream0->NDTR = ADC_PT1000_DMA_AVG_SAMPLES;

	NVIC_EnableIRQ(DMA2_Stream0_IRQn);

	/* Enable the stream in Peripheral-to-Memory mode with 16 bit data and a circular destination buffer
	 * Enable interrupt generation on transfer complete
	 *
	 * Todo: Maybe use twice as big of a buffer and also use half-fill interrupt in order to prevent overruns
	 */
	DMA2_Stream0->CR = DMA_SxCR_PL_1 | DMA_SxCR_MSIZE_0 | DMA_SxCR_PSIZE_0 | DMA_SxCR_MINC |
			DMA_SxCR_CIRC | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_EN | ((ADC_PT1000_CHANNEL  & 0x7)<<25);
}

static inline void adc_pt1000_disable_dma_stream(void)
{
	/* Disable the stream */
	DMA2_Stream0->CR = 0;

	/* Disable clock if necessary */
	rcc_manager_disable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(RCC_AHB1ENR_DMA2EN));

	/* Disable interrupt */
	NVIC_DisableIRQ(DMA2_Stream0_IRQn);
}

void adc_pt1000_setup_meas(void)
{
	rcc_manager_enable_clock(&RCC->APB2ENR, BITMASK_TO_BITNO(RCC_APB2ENR_ADC3EN));
	rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(ADC_PT1000_PORT_RCC_MASK));

	ADC_PT1000_PORT->MODER |= ANALOG(ADC_PT1000_PIN);

	/* Set S&H time for PT1000 ADC channel */
#if ADC_PT1000_CHANNEL < 10
	ADC_PT1000_PERIPH->SMPR2 |= (7U << (3*ADC_PT1000_CHANNEL));
#else
	ADC_PT1000_PERIPH->SMPR1 |= (7U << (3*(ADC_PT1000_CHANNEL-10)));
#endif

	ADC->CCR |= (0x3<<16);

	/* Set watchdog limits */
	ADC_PT1000_PERIPH->HTR = ADC_PT1000_UPPER_WATCHDOG;
	ADC_PT1000_PERIPH->LTR = ADC_PT1000_LOWER_WATCHDOG;

	/* Set length of sequence to 1 */
	ADC_PT1000_PERIPH->SQR1 = (0UL<<20);

	/* Set channel as 1st element in sequence */
	ADC_PT1000_PERIPH->SQR3 = (ADC_PT1000_CHANNEL<<0);

	ADC_PT1000_PERIPH->CR1 = ADC_CR1_OVRIE | ADC_CR1_AWDEN | ADC_CR1_AWDIE;
	ADC_PT1000_PERIPH->CR2 = ADC_CR2_EXTEN_0 | ADC_CR2_EXTSEL_2 | ADC_CR2_EXTSEL_1 |
			ADC_CR2_ADON | ADC_CR2_DMA | ADC_CR2_DDS;

	adc_pt1000_set_moving_average_filter_param(ADC_PT1000_FILTER_WEIGHT);
	adc_pt1000_set_resistance_calibration(0, 0, false);
	pt1000_res_raw_lf = 0.0f;

	NVIC_EnableIRQ(ADC_IRQn);

	adc_pt1000_enable_dma_stream();

	adc_pt1000_setup_sample_frequency_timer();

	safety_controller_ack_flag_with_key(ERR_FLAG_MEAS_ADC_OFF, MEAS_ADC_SAFETY_FLAG_KEY);

	streaming_flag_ptr = NULL;
	adc_watchdog_counter = 0UL;
	stream_buffer = NULL;

	safety_controller_set_crc_monitor(ERR_CRC_MON_MEAS_ADC, SAFETY_CRC_MON_MEAS_ADC_PW);
}

void adc_pt1000_set_moving_average_filter_param(float alpha)
{
	filter_alpha = alpha;
	safety_controller_report_error_with_key(ERR_FLAG_MEAS_ADC_UNSTABLE, MEAS_ADC_SAFETY_FLAG_KEY);
}

void adc_pt1000_set_resistance_calibration(float offset, float sensitivity_deviation, bool active)
{
	pt1000_offset = offset;
	pt1000_sens_dev = sensitivity_deviation;
	calibration_active = active;

	if (!calibration_active)
		safety_controller_report_error_with_key(ERR_FLAG_UNCAL, MEAS_ADC_SAFETY_FLAG_KEY);
	else
		safety_controller_ack_flag_with_key(ERR_FLAG_UNCAL, MEAS_ADC_SAFETY_FLAG_KEY);
}

void adc_pt1000_get_resistance_calibration(float *offset, float *sensitivity_deviation, bool *active)
{
	if (offset)
		*offset = pt1000_offset;
	if (sensitivity_deviation)
		*sensitivity_deviation = pt1000_sens_dev;
	if (active)
		*active = calibration_active;
}

static inline float adc_pt1000_apply_calibration(float raw_resistance)
{
	if (calibration_active)
		return (raw_resistance - pt1000_offset) * (1.0f + pt1000_sens_dev);
	else
		return raw_resistance;

}

int adc_pt1000_get_current_resistance(float *resistance)
{
	int ret_val = 0;
	bool flag = true;


	if (!resistance)
		return -1001;

	*resistance = adc_pt1000_apply_calibration(pt1000_res_raw_lf);

	if (safety_controller_get_flags_by_mask(ERR_FLAG_MEAS_ADC_OFF | ERR_FLAG_MEAS_ADC_OVERFLOW |
						ERR_FLAG_MEAS_ADC_WATCHDOG | ERR_FLAG_TIMING_MEAS_ADC)) {
		ret_val = -100;
		goto return_value;
	}

	(void)safety_controller_get_flag(ERR_FLAG_MEAS_ADC_UNSTABLE, &flag, false);

	if (flag) {
		ret_val = 2;
		goto return_value;
	}

return_value:
	return ret_val;
}

int adc_pt1000_stream_raw_value_to_memory(volatile float *adc_array, uint32_t length, volatile int *flag_to_set)
{
	int ret = 0;

	if (!flag_to_set)
		return -1;

	stream_buffer = adc_array;
	stream_count = length;
	stream_pos = 0U;

	if (adc_array) {
		*flag_to_set = 0;
		streaming_flag_ptr = flag_to_set;
	} else {
		ret = -2;
	}

	return ret;
}

void adc_pt1000_convert_raw_value_array_to_resistance(float *resistance_dest, float *raw_source, uint32_t count)
{
	uint32_t i;

	if (!resistance_dest)
		resistance_dest = raw_source;

	if (!raw_source || !count)
		return;

	for (i = 0; i < count; i++)
		resistance_dest[i] = ADC_TO_RES(raw_source[i]);
}

void adc_pt1000_disable(void)
{
	adc_pt1000_disable_adc();
	adc_pt1000_stop_sample_frequency_timer();
	adc_pt1000_disable_dma_stream();

	pt1000_res_raw_lf = 0.0f;
	safety_controller_report_error_with_key(ERR_FLAG_MEAS_ADC_OFF, MEAS_ADC_SAFETY_FLAG_KEY);
	safety_controller_report_error_with_key(ERR_FLAG_MEAS_ADC_UNSTABLE, MEAS_ADC_SAFETY_FLAG_KEY);

	if (streaming_flag_ptr) {
		*streaming_flag_ptr = -3;
		streaming_flag_ptr = NULL;
	}
}

static inline __attribute__((optimize("O3"))) void adc_pt1000_filter(float adc_prefiltered_value)
{
	float alpha;
	float res;
	static uint8_t old_state = 0;
	static uint32_t stable_sample_counter = 0;

	res = ADC_TO_RES(adc_prefiltered_value);
	if (ABS(res - pt1000_res_raw_lf) >= ADC_PT1000_FILTER_UNSTABLE_DIFF) {
		stable_sample_counter = 0;
		alpha = ADC_PT1000_FILTER_WEIGHT_FAST;
		if (old_state != 1) {
			safety_controller_report_error_with_key(ERR_FLAG_MEAS_ADC_UNSTABLE, MEAS_ADC_SAFETY_FLAG_KEY);
			old_state = 1;
		}
	} else {
		alpha = filter_alpha;
		if (old_state != 2) {
			stable_sample_counter++;
			if (stable_sample_counter >= ADC_PT1000_FILTER_STABLE_SAMPLE_COUNT) {
				safety_controller_ack_flag_with_key(ERR_FLAG_MEAS_ADC_UNSTABLE, MEAS_ADC_SAFETY_FLAG_KEY);
				old_state = 2;
			}
		}
	}

	pt1000_res_raw_lf = (1.0f - alpha) * pt1000_res_raw_lf +
			alpha * res;

	safety_controller_report_timing(ERR_TIMING_MEAS_ADC);
}

static inline __attribute__((optimize("O3"))) float adc_pt1000_dma_avg_pre_filter(void)
{
	unsigned int i;
	uint32_t sum = 0;
	uint16_t max_val = 0U;
	uint16_t min_val = 65535U;
	uint16_t sample;

	for (i = 0; i < ADC_PT1000_DMA_AVG_SAMPLES; i++) {
		sample = dma_sample_buffer[i];

		/* Update min and max trackers */
		max_val = (sample > max_val ? sample : max_val);
		min_val = (sample < min_val ? sample : min_val);

		/* Sum up all values (for average) */
		sum += sample;
	}

	/* Delete max and min vals from sum */
	sum = sum - (uint32_t)max_val - (uint32_t)min_val;

	/* Divide to get average and return */
	return (float)sum / (float)(ADC_PT1000_DMA_AVG_SAMPLES-2);
}

void ADC_IRQHandler(void)
{
	uint32_t adc1_sr;

	adc1_sr = ADC_PT1000_PERIPH->SR;

	if (adc1_sr & ADC_SR_OVR) {
		ADC_PT1000_PERIPH->SR &= ~ADC_SR_OVR;
		safety_controller_report_error(ERR_FLAG_MEAS_ADC_OVERFLOW);
		/* Disable ADC  in case of overrrun*/
		adc_pt1000_disable();
	}

	if (adc1_sr & ADC_SR_AWD) {
		ADC_PT1000_PERIPH->SR &= ~ADC_SR_AWD;
		adc_watchdog_counter++;
		if (adc_watchdog_counter >= ADC_PT1000_WATCHDOG_SAMPLE_COUNT)
			safety_controller_report_error(ERR_FLAG_MEAS_ADC_WATCHDOG);
	}
}

static void append_stream_buffer(float val)
{
	if (!stream_buffer || !streaming_flag_ptr)
		return;

	if (stream_pos < stream_count)
		stream_buffer[stream_pos++] = val;

	if (stream_pos >= stream_count) {
		*streaming_flag_ptr = 1;
		streaming_flag_ptr = NULL;
	}

}

void DMA2_Stream0_IRQHandler(void)
{
	uint32_t lisr;
	float adc_val;

	lisr = DMA2->LISR & (0x3F);
	DMA2->LIFCR = lisr;

	if (lisr & DMA_LISR_TCIF0) {
		/* Samples transferred */
		adc_val = adc_pt1000_dma_avg_pre_filter();
		adc_pt1000_raw_reading_hf = adc_val;

		if (streaming_flag_ptr)
			append_stream_buffer(adc_val);

		if (adc_watchdog_counter > 0UL)
			adc_watchdog_counter--;

		/* Call moving average filter */
		adc_pt1000_filter(adc_val);
	}

	if (lisr & DMA_LISR_TEIF0)
		adc_pt1000_disable();

}