/* 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/>.
 */


#include <reflow-controller/calibration.h>
#include <reflow-controller/adc-meas.h>
#include <stm-periph/uart.h>
#include <helper-macros/helper-macros.h>
#include <arm_math.h>
#include <stdlib.h>
#include <float.h>

enum calibration_shell_state {CAL_START = 0, CAL_WAIT_RES1, CAL_MEAS_RES1, CAL_WAIT_RES2, CAL_MEAS_RES2};

void calibration_calculate(float low_measured, float low_setpoint, float high_measured, float high_setpoint,
			   float *sens_deviation, float *sens_corrected_offset)
{
	if (!sens_deviation || !sens_corrected_offset)
		return;

	float delta_y;
	float delta_x;
	float sens_corr_mult;

	delta_y = high_measured - low_measured;
	delta_x = high_setpoint - low_setpoint;

	sens_corr_mult = delta_x / delta_y;
	*sens_deviation = sens_corr_mult - 1.0f;

	*sens_corrected_offset = low_setpoint - low_measured * sens_corr_mult;
}



float *calibration_acquire_data_start(uint32_t count, volatile int *flag)
{
	int status;
	float *stream_mem;

	if (!count)
		return NULL;

	stream_mem = (float *)calloc(count, sizeof(float));
	if (!stream_mem)
		return stream_mem;

	*flag = 0;
	status = adc_pt1000_stream_raw_value_to_memory(stream_mem, count, flag);
	if (status)
		goto free_mem;


	return stream_mem;
free_mem:
	free(stream_mem);
	return NULL;
}

static int calibration_poll_data_acquisition(float *mem_array, uint32_t count, volatile int *flag, float *mu, float *max_dev)
{
	int ret_val = 0;
	float min_val = FLT_MAX;
	float max_val = -FLT_MAX;
	uint32_t i;

	if (!flag || !mem_array || !mu || !max_dev)
		return -1000;

	if (*flag == 0) {
		/* Continue polling */
		return 1;
	}

	if (*flag != 1) {
		/* Error */
		ret_val = -1;
		goto ret_free_mem;
	}

	/* Convert the stream memory to Ohm readings */
	adc_pt1000_convert_raw_value_array_to_resistance(NULL, mem_array, count);

	/* Do not compute std-deviation. Too imprecise
	 * arm_std_f32(stream_mem, count, sigma);
	 */
	arm_mean_f32(mem_array, count, mu);

	/* Find min and max values of array */
	for (i = 0U; i < count; i++) {
		min_val = MIN(min_val, mem_array[i]);
		max_val = MAX(max_val, mem_array[i]);
	}

	/* Compute maximum deviation range */
	*max_dev = max_val - min_val;

ret_free_mem:
	free(mem_array);
	return ret_val;
}

shellmatta_retCode_t calibration_sequence_shell_cmd(shellmatta_handle_t shell, const char *arg, uint32_t len)
{
	(void)arg;
	(void)len;

	/* This stores the current state of the calibration process */
	static enum calibration_shell_state cal_state = CAL_START;
	shellmatta_retCode_t ret_val = SHELLMATTA_BUSY;
	uint32_t i;
	int res;

	static float mu = 0.0f, mu2 = 0.0f, dev = 0.0f, dev2 = 0.0f;
	float sens_dev, offset;
	static float *data_buffer = NULL;
	static volatile int flag;
	char *stdin_data;
	uint32_t stdin_len;


	switch (cal_state) {
	case CAL_START:
		/* Clear errors of PT1000 reading */
		adc_pt1000_clear_error();
		shellmatta_printf(shell, "Starting calibration: Insert 1000 Ohm calibration resistor and press ENTER\r\n");
		cal_state = CAL_WAIT_RES1;
		ret_val = SHELLMATTA_CONTINUE;
		break;
	case CAL_WAIT_RES1:
		cal_state = CAL_WAIT_RES1;
		ret_val = SHELLMATTA_CONTINUE;
		shellmatta_read(shell, &stdin_data, &stdin_len);
		if (stdin_len > 0) {
			for (i = 0; i < stdin_len; i++) {
				if (stdin_data[i] == '\r') {
					cal_state = CAL_MEAS_RES1;
					ret_val = SHELLMATTA_BUSY;
					shellmatta_printf(shell, "Measurement...\r\n");
					adc_pt1000_clear_error();
					data_buffer = calibration_acquire_data_start(512UL, &flag);
					break;
				} else if (stdin_data[i] == '\x03') {
					cal_state = CAL_START;
				}
			}
		}

		break;
	case CAL_MEAS_RES1:
		if (!data_buffer) {
			shellmatta_printf(shell, "Data acquisition failed!\r\n");
			ret_val = SHELLMATTA_OK;
			cal_state = CAL_START;
			break;
		}

		res = calibration_poll_data_acquisition(data_buffer, 512UL, &flag, &mu, &dev);
		/* Stay in this state until the measurements are finished */
		if (res == 1) {
			ret_val = SHELLMATTA_BUSY;
			cal_state = CAL_MEAS_RES1;
		} else if (res == 0) {
			shellmatta_printf(shell, "R=%.2f, Noise peak-peak: %.2f\r\n", mu, dev);
			if (adc_pt1000_check_error() != ADC_PT1000_NO_ERR) {
				shellmatta_printf(shell, "Error in resistance measurement: %d", adc_pt1000_check_error());
				ret_val = SHELLMATTA_OK;
				cal_state = CAL_START;
			} else {
				ret_val = SHELLMATTA_CONTINUE;
				shellmatta_printf(shell, "Insert 2000 Ohm calibration resistor and press ENTER\r\n");
				cal_state = CAL_WAIT_RES2;
			}
		} else {
			shellmatta_printf(shell, "Error in resistance measurement: %d", adc_pt1000_check_error());
			ret_val = SHELLMATTA_OK;
			cal_state = CAL_START;
		}
		break;
	case CAL_WAIT_RES2:
		cal_state = CAL_WAIT_RES2;
		ret_val = SHELLMATTA_CONTINUE;
		shellmatta_read(shell, &stdin_data, &stdin_len);
		if (stdin_len > 0) {
			for (i = 0; i < stdin_len; i++) {
				if (stdin_data[i] == '\r') {
					cal_state = CAL_MEAS_RES2;
					ret_val = SHELLMATTA_BUSY;
					shellmatta_printf(shell, "Measurement...\r\n");
					adc_pt1000_clear_error();
					data_buffer = calibration_acquire_data_start(512UL, &flag);
					break;
				} else if (stdin_data[i] == '\x03') {
					cal_state = CAL_START;
				}
			}
		}

		break;
	case CAL_MEAS_RES2:
		if (!data_buffer) {
			shellmatta_printf(shell, "Data acquisition failed!\r\n");
			ret_val = SHELLMATTA_OK;
			cal_state = CAL_START;
			break;
		}

		res = calibration_poll_data_acquisition(data_buffer, 512UL, &flag, &mu2, &dev2);
		/* Stay in this state until the measurements are finished */
		if (res == 1) {
			ret_val = SHELLMATTA_BUSY;
			cal_state = CAL_MEAS_RES2;
		} else if (res == 0) {
			shellmatta_printf(shell, "R=%.2f, Noise peak-peak: %.2f\r\n", mu2, dev2);
			if (adc_pt1000_check_error() != ADC_PT1000_NO_ERR) {
				shellmatta_printf(shell, "Error in resistance measurement: %d", adc_pt1000_check_error());
				ret_val = SHELLMATTA_OK;
				cal_state = CAL_START;
			} else {
				ret_val = SHELLMATTA_OK;
				cal_state = CAL_START;

				if (dev > CALIBRATION_MAX_PEAK_PEAK_NOISE_OHM ||
				    dev2 > CALIBRATION_MAX_PEAK_PEAK_NOISE_OHM) {
					shellmatta_printf(shell, "Calibration failed! Too much noise. Check your hardware.\r\n");
					break;
				}
				shellmatta_printf(shell, "Calibartion finished successfully!\r\n");
				/* Calculate calibration */
				calibration_calculate(mu, 1000.0f, mu2, 2000.0f, &sens_dev, &offset);

				shellmatta_printf(shell, "\r\n\tSENS_DEVIATION: %.4f\r\n\tOFFSET_CORR: %.2f\r\n", sens_dev, offset);
				adc_pt1000_set_resistance_calibration(offset, sens_dev, true);
			}
		} else {
			shellmatta_printf(shell, "Error in resistance measurement: %d", adc_pt1000_check_error());
			ret_val = SHELLMATTA_OK;
			cal_state = CAL_START;
		}
		break;
	default:
		shellmatta_printf(shell, "Undefined state reached in calibration. Aborting\r\n");
		cal_state = CAL_START;
		ret_val = SHELLMATTA_OK;
		break;
	}

	return ret_val;
}
