/*
* @file	pid_auto.h
*
* PID Auto tune.
*
* @date Oct 08, 2019
* @author andreika, (c) 2019
*/

#pragma once

#include "global.h"
#include <fstream>
#include <stdarg.h>

#include "levenberg_marquardt_solver.h"
#include "pid_functions.h"
#include "pid_avg_buf.h"
#include "pid_controller.h"
#include "pid_open_loop_models.h"
#include "pid_sim.h"
#include "output_csv.h"


/// These settings should be obtained from the measured data
class PidAutoTuneSettings {
public:
	double minValue, maxValue;
	double stepPoint, maxPoint;
	double timeScale;
	double targetValue;
};

// PID auto-tune method using different tuning rules and FOPDT/SOPDT models
class PidAutoTune {
public:
	PidAutoTune() {
		measuredData.init();
	}

	void addData(float v) {
		measuredData.addDataPoint(v);
	}

	// minValue, maxValue - input step values
	// stepPoint - data index where the step was done
	// maxPoint - data index where the output was saturated
	// this is not thread-safe (because of internal static vars) but anyway it works...
	bool findPid(pid_sim_type_e simType, pid_tune_method_e method, const PidAutoTuneSettings & settings, const double *initialParams) {
		// save current settings & simType
		this->settings = settings;
		this->simType = simType;
		// without bias we cannot fit the analytic curve
		modelBias = findModelBias();
		
		// todo: where do we get them?
		pid.minValue = 0;
		pid.maxValue = 100;

#ifdef PID_DEBUG
		printf("* modelBias=%g avgMin=%g avgMax=%g\r\n", modelBias, avgMeasuredMin, avgMeasuredMax);
#endif
		StepFunction stepFunc(settings.minValue, settings.maxValue, settings.stepPoint, settings.timeScale);

		int methodOrder = getMethodOrder(method);

		// set initial params
		if (initialParams == nullptr) {
			// guess initial params
			if (methodOrder == 1) {
				findFirstOrderInitialParams2Points(params);
			} else {
				if (!findSecondOrderInitialParamsHarriott(params)) {
					if (!findSecondOrderInitialParams3Points(params)) {
						// this is a bad scenario, but we don't have a choice...
						// first, find FOPDT params (at least it doesn't fail)
						findFirstOrderInitialParams2Points(params);
						// then imitate FOPDT with SODPT, when T1+T2 = T, T1*T2 -> 0
						params[PARAM_T2] = 0.01;
						// and hope that our solver will do the rest...
					}
				}
			}
#ifdef PID_DEBUG
			printf("* Params0: K=%g T1=%g T2=%g L=%g\r\n", params[PARAM_K], params[PARAM_T], params[PARAM_T2], params[PARAM_L]);
#endif
		}
		else {
			// use provided initial params
			memcpy(params, initialParams, sizeof(params));
		}

		double merit0, merit;
#ifdef PID_DEBUG
		printf("* Solving...\r\n");
#endif
		// create and start solver
		if (methodOrder == 1) {
			// 1st order
			FirstOrderPlusDelayLineFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
			func.justifyParams(params);
			const int numParams1stOrder = 3;
			outputFunc<numParams1stOrder>("pid_func01.csv", func, stepFunc, params);
			LevenbergMarquardtSolver<numParams1stOrder> solver(&func, params);
			merit0 = func.calcMerit(params);
			iterationCount = solver.solve(minParamT);
			outputFunc<numParams1stOrder>("pid_func1.csv", func, stepFunc, params);
			merit = func.calcMerit(params);
		}
		else {
			// 2nd order or approximated 2nd->1st order
			SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
			func.justifyParams(params);
			const int numParams2ndOrder = 4;
			outputFunc<numParams2ndOrder>("pid_func02.csv", func, stepFunc, params);
			LevenbergMarquardtSolver<numParams2ndOrder> solver(&func, params);
			merit0 = func.calcMerit(params);
			iterationCount = solver.solve(minParamT);
			outputFunc<numParams2ndOrder>("pid_func2.csv", func, stepFunc, params);
			merit = func.calcMerit(params);
		}

#ifdef PID_DEBUG
		printSolverResult(iterationCount, merit0, merit);
#endif

		ModelOpenLoopPlant *model = nullptr;
		switch (method) {
		case PID_TUNE_CHR1:
		case PID_TUNE_AUTO1: {
			static ModelChienHronesReswickFirstOrder chr(params);
			model = &chr;
			break;
		}
		case PID_TUNE_IMC2_1: {
			ModelFopdtApproximatedFromSopdt fo(params);
			static ModelRiveraMorariFirstOrder imc(fo.getParams());
			model = &imc;
			break;
		}
		case PID_TUNE_CHR2_1: {
			ModelFopdtApproximatedFromSopdt fo(params);
			static ModelChienHronesReswickFirstOrder chr(fo.getParams());
			model = &chr;
			break;
		}
		case PID_TUNE_CHR2:
		case PID_TUNE_AUTO2: {
			static ModelChienHronesReswickSecondOrder chr(params);
			model = &chr;
			break;
		}
		case PID_TUNE_VDG2: {
			static ModelVanDerGrintenSecondOrder vdg(params);
			model = &vdg;
			break;
		}
		case PID_TUNE_HP2: {
			static ModelHaalmanPembertonSecondOrder hp(params);
			model = &hp;
			break;
		}
		default:
			return false;
		}
		pid.pFactor = model->getKp();
		pid.iFactor = model->getKi();
		pid.dFactor = model->getKd();
		pid.offset = getPidOffset(model);
		pid.periodMs = 1000.0 / settings.timeScale;

		pid0 = pid;

		// for "automatic" methods, we try to make the coeffs even better!
		if (method == PID_TUNE_AUTO1 || method == PID_TUNE_AUTO2) {
			ModelAutoSolver autoSolver(model);
			solveModel(method, autoSolver);
			pid.pFactor = autoSolver.getKp();
			pid.iFactor = autoSolver.getKi();
			pid.dFactor = autoSolver.getKd();
		}

		return true;
	}

	int getMethodOrder(pid_tune_method_e method) {
		switch (method) {
			// 1st order
		case PID_TUNE_CHR1:
		case PID_TUNE_AUTO1:
			return 1;
			// 2nd order
		default:
			return 2;
		}
	}

	double const *getParams() const {
		return params;
	}

	pid_s const &getPid() const {
		return pid;
	}

	pid_s const &getPid0() const {
		return pid0;
	}

	double getAvgMeasuredMin() const {
		return avgMeasuredMin;
	}

	double getAvgMeasuredMax() const {
		return avgMeasuredMax;
	}

	double getModelBias() const {
		return modelBias;
	}

	// The model output is typically shifted
	double findModelBias() {
		if (settings.stepPoint < 0 || settings.maxPoint <= settings.stepPoint || settings.maxPoint > measuredData.getNumDataPoints())
			return 0;
		// find the real 'min value' of the measured output data (before the step function goes up).
		avgMeasuredMin = measuredData.getAveragedData(0, (int)settings.stepPoint);
		// find the real 'max value' of the measured output data (after the output saturation).
		avgMeasuredMax = measuredData.getAveragedData((int)settings.maxPoint, measuredData.getNumDataPoints() - 1);

		if (avgMeasuredMax == avgMeasuredMin)
			return 0;
		// solve the system of equations and find the bias
		return (settings.maxValue * avgMeasuredMin - settings.minValue * avgMeasuredMax) / (settings.maxValue - settings.minValue);
	}

	// If the target value is known, we can estimate the PID offset value based on the model gain and bias.
	float getPidOffset(ModelOpenLoopPlant *model) const {
		if (settings.targetValue == 0.0)
			return 0;
		return (settings.targetValue - modelBias) / model->getParams()[PARAM_K];
	}

	// See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994.
	// Also see: "Practical PID Control", p. 169
	bool findFirstOrderInitialParams2Points(double *params) const {
		int i0 = (int)settings.stepPoint;
		int i1 = (int)settings.maxPoint;

		double dy = avgMeasuredMax - avgMeasuredMin;

		double t[2];
		static const double tCoefs[] = { 0.353, 0.853 };
		for (int i = 0; i < 2; i++) {
			t[i] = getTimeDelta(measuredData.findDataAt((float)(avgMeasuredMin + dy * tCoefs[i]), i0, i1));
			if (t[i] < 0.0)
				return false;
		}

		params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
		params[PARAM_T] = 0.67 * (t[1] - t[0]);
		params[PARAM_L] = 1.3 * t[0] - 0.29 * t[1];

		return true;
	}

	// See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994.
	// Also see: "Practical PID Control", p. 187
	bool findSecondOrderInitialParams3Points(double *params) const {
		int i0 = (int)settings.stepPoint;
		int i1 = (int)settings.maxPoint;

		double dy = avgMeasuredMax - avgMeasuredMin;

		double t[3];
		static const double tCoefs[] = { 0.14, 0.55, 0.91 };
		for (int i = 0; i < 3; i++) {
			t[i] = getTimeDelta(measuredData.findDataAt((float)(avgMeasuredMin + dy * tCoefs[i]), i0, i1));
			if (t[i] == 0.0)
				return false;
		}
		double alpha = (t[2] - t[1]) / (t[1] - t[0]);
#if 0
		// check if usable range?
		if (alpha < 1.2323 || alpha > 2.4850) {
			return false;
		}
#endif
		double beta = log(alpha / (2.485 - alpha));
		double xi = 0.50906 + 0.51743 * beta - 0.076284 * pow(beta, 2) + 0.041363 * pow(beta, 3)
			- 0.0049224 * pow(beta, 4) + 0.00021234 * pow(beta, 5);
		double Tcoef = 0.85818 - 0.62907 * xi + 1.2897 * pow(xi, 2) - 0.36859 * pow(xi, 3) + 0.038891 * pow(xi, 4);
		double Lcoef = 1.39200 - 0.52536 * xi + 1.2991 * pow(xi, 2) - 0.36859 * pow(xi, 3) + 0.037605 * pow(xi, 4);

		double T = Tcoef / (t[1] - t[0]);
		// we've got T and xi, and we have to solve quadratic equation to get T1 and T2:
		// T = T1*T2
		// Xi = (T1 + T2) / (T1*T2)
		double det = (T * xi) * (T * xi) - 4.0 * T;

		params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
		if (det < 0) {
			// that's a hack :(
			// we ignore xi and equalize T and T2 to match the higher-order coefficient.
			if (T < 0)
				return false;
			if (xi > 0) {
				params[PARAM_T2] = T * xi;
				params[PARAM_T] = T / params[PARAM_T2];
			} else {
				params[PARAM_T2] = params[PARAM_T] = sqrt(T);
			}
		} else {
			params[PARAM_T2] = T * xi + sqrt(det) / 2.0;	// we take larger root
			params[PARAM_T] = T / params[PARAM_T2];
		}
		params[PARAM_L] = t[1] - T * Lcoef;

		return true;
	}

	// See: Harriott P. Process control (1964). McGraw-Hill. USA.
	// Also see: "Practical PID Control", p. 182
	bool findSecondOrderInitialParamsHarriott(double *params) const {
		static const HarriotFunction hfunc;

		int i0 = (int)settings.stepPoint;
		int i1 = (int)settings.maxPoint;

		double dy = avgMeasuredMax - avgMeasuredMin;

		double A1 = -measuredData.getArea(i0, i1, (float)avgMeasuredMax) / settings.timeScale;

		double t73 = getTimeDelta(measuredData.findDataAt((float)(avgMeasuredMin + dy * 0.73), i0, i1));
		double tm = i0 + 0.5 * (t73 / 1.3) * settings.timeScale;
		double ym = measuredData.getValue((float)tm);
		// normalize
		double ymn = (ym - avgMeasuredMin) / dy;
		// sanity check?
		if (ymn < HarriotFunction::minX || ymn > HarriotFunction::maxX)
			return false;
		double r = hfunc.getValue(ymn);

		params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
		params[PARAM_L] = A1 - t73 / 1.3;
		params[PARAM_T] = r * t73 / 1.3;
		params[PARAM_T2] = (1.0 - r) * t73 / 1.3;

		return true;
	}

	double getTimeDelta(int i1) const {
		if (i1 < 0)
			return -1.0;
		return (double)(i1 - settings.stepPoint) / settings.timeScale;
	}

	// Use automatic LM-solver to find the best PID coefs which satisfy the minimal PID metric.
	// The initial PID coefs are already calculated using the well-known CHR method (1st or 2nd order).
	bool solveModel(pid_tune_method_e method, ModelAutoSolver & model) {
		double merit0, merit;
		
#ifdef PID_DEBUG
		printf("* Solving for better coefs:\r\n");
#endif
		// todo: is it correct?
		double dTime = 1.0 / settings.timeScale;
		const int numSimPoints = 1024;
		PidSimulatorFactory<numSimPoints> simFactory(simType, getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), dTime, modelBias, pid);
		PidCoefsFinderFunction<numSimPoints> func(&simFactory, params);
		func.justifyParams(model.getParams());
		merit0 = func.calcMerit(model.getParams());

		// now hopefully we'll find even better coefs!
		LevenbergMarquardtSolver<numParamsForPid> solver((LMSFunction<numParamsForPid> *)&func, model.getParams());
		int iterationCount = solver.solve();
		
		merit = func.calcMerit(model.getParams());
#ifdef PID_DEBUG
		printSolverResult(iterationCount, merit0, merit);
#endif
		return true;
	}
	
	template <int numParams>
	void outputFunc(const char *fname, const AbstractDelayLineFunction<numParams> & func, const StepFunction & stepFunc, double *params) {
#ifdef PID_DEBUG
		for (int i = 0; i < func.getNumPoints(); i++) {
			double v = func.getEstimatedValueAtPoint(i, params);
			double sv = stepFunc.getValue((float)i, 0);
			output_csv(fname, (double)i, func.getDataPoint(i), v, sv);
		}
#endif
	}

	void printSolverResult(int iterationCount, double merit0, double merit) {
		if (iterationCount > 0)
			printf("* The solver finished in %d iterations! (Merit: %g -> %g)\r\n", iterationCount, merit0, merit);
		else
			printf("* The solver aborted after %d iterations! (Merit: %g -> %g)\r\n", -iterationCount, merit0, merit);
	}


protected:
	AveragingDataBuffer<MAX_DATA_POINTS> measuredData;
	PidAutoTuneSettings settings;
	pid_sim_type_e simType;
	double params[4] = { 0 };
	pid_s pid;
	pid_s pid0;	// not-optimized
	int iterationCount = 0;
	double avgMeasuredMin, avgMeasuredMax;
	double modelBias;
};