/* 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 <stdlib.h>
#include <float.h>
#include <reflow-controller/safety/safety-controller.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 *offset)
{
	if (!sens_deviation || !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;

	*offset = (high_setpoint * low_measured - low_setpoint * high_measured) / (high_setpoint - low_setpoint);
}

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 float calculate_mean(float *values, uint32_t count)
{
	uint32_t loop_cnt = (count + 7) / 8;
	uint32_t remainder = count % 8;
	float sum = 0;

	switch (remainder) {
	case 0: do { sum += *values++; /* FALLTHRU */
	case 7:      sum += *values++; /* FALLTHRU */
	case 6:      sum += *values++; /* FALLTHRU */
	case 5:      sum += *values++; /* FALLTHRU */
	case 4:      sum += *values++; /* FALLTHRU */
	case 3:      sum += *values++; /* FALLTHRU */
	case 2:      sum += *values++; /* FALLTHRU */
	case 1:      sum += *values++;
		} while (--loop_cnt > 0);
	}

	return sum / (float)count;
}

static float calculate_standard_deviation(float *values, uint32_t count, float mean)
{
	uint32_t loop_cnt = (count + 7) / 8;
	uint32_t remainder = count % 8;
	float sum = 0;
	float res;

	switch (remainder) {
	case 0:	do {
		sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
	case 7:	sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
	case 6: sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
	case 5:	sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
	case 4: sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
	case 3: sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
	case 2: sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
	case 1: sum += (*values - mean) * (*values - mean);
		values++;
		/* FALLTHRU */
		} while (--loop_cnt > 0);
	}

	sum /= (float)count;
	/* Compute the square root using the FPU.
	 * The constraint 't' tells GCC to use a floating point register
	 */
	__asm__ __volatile__("vsqrt.f32 %0, %1" : "=t"(res) : "t"(sum));

	return res;
}

static int calibration_poll_data_acquisition(float *mem_array, uint32_t count, volatile int *flag, float *mu, float *std_dev)
{
	int ret_val = 0;

	if (!flag || !mem_array || !mu || !std_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 */
	*mu = calculate_mean(mem_array, count);
	*std_dev = calculate_standard_deviation(mem_array, count, *mu);

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;
	bool error_occured;
	const enum safety_flag meas_adc_err_mask = ERR_FLAG_MEAS_ADC_OFF | ERR_FLAG_MEAS_ADC_WATCHDOG;

	/* 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;
	bool cal_active;

	switch (cal_state) {
	case CAL_START:
		/* Clear errors of PT1000 reading */
		safety_controller_ack_flag(ERR_FLAG_MEAS_ADC_WATCHDOG);
		safety_controller_ack_flag(ERR_FLAG_OVERTEMP);
		adc_pt1000_get_resistance_calibration(&offset, &sens_dev, &cal_active);
		if (cal_active) {
			shellmatta_printf(shell, "Already calibrated.\r\n\tOffset: %f\r\n\tSens: %f\r\n", offset, sens_dev);
			shellmatta_printf(shell, "Press CTRL-C to abort new calibration.\r\n");
		}
		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");
					safety_controller_ack_flag(ERR_FLAG_MEAS_ADC_WATCHDOG);
					safety_controller_ack_flag(ERR_FLAG_OVERTEMP);
					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, Std-Dev: %.2f\r\n", mu, dev);
			error_occured = safety_controller_get_flags_by_mask(meas_adc_err_mask);
			if (error_occured) {
				shellmatta_printf(shell, "Error in resistance measurement");
				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");
			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");
					safety_controller_ack_flag(ERR_FLAG_MEAS_ADC_WATCHDOG);
					safety_controller_ack_flag(ERR_FLAG_OVERTEMP);
					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, Std-Dev: %.2f\r\n", mu2, dev2);
			error_occured = safety_controller_get_flags_by_mask(meas_adc_err_mask);
			if (error_occured) {
				shellmatta_printf(shell, "Error in resistance measurement");
				ret_val = SHELLMATTA_OK;
				cal_state = CAL_START;
			} else {
				ret_val = SHELLMATTA_OK;
				cal_state = CAL_START;

				if (dev > CALIBRATION_MAX_NOISE_OHM ||
				    dev2 > CALIBRATION_MAX_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");
			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;
}