/*
    PIP - Platform Independent Primitives
    PIMathSolver
    Ivan Pelipenko peri4ko@yandex.ru

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program 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 Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "pimathsolver.h"

const char PIMathSolver::methods_desc[]          = "b{Methods:}\
\n  -1 - Global settings\
\n  01 - Eyler 1\
\n  02 - Eyler 2\
\n  14 - Runge-Kutta 4\
\n  23 - Adams-Bashfort-Moulton 3\
\n  24 - Adams-Bashfort-Moulton 4\
\n  32 - Polynomial Approximation 2\
\n  33 - Polynomial Approximation 3\
\n  34 - Polynomial Approximation 4\
\n  35 - Polynomial Approximation 5";

PIMathSolver::Method PIMathSolver::method_global = PIMathSolver::Eyler_2;


PIMathSolver::PIMathSolver() {
	times.resize(4);
}


void PIMathSolver::solve(double u, double h) {
	switch (method) {
	case Global:
		switch (method_global) {
		case Eyler_1: solveEyler1(u, h); break;
		case Eyler_2: solveEyler2(u, h); break;
		case RungeKutta_4: solveRK4(u, h); break;
		case AdamsBashfortMoulton_2: solveABM2(u, h); break;
		case AdamsBashfortMoulton_3: solveABM3(u, h); break;
		case AdamsBashfortMoulton_4:
		default: solveABM4(u, h); break;
		case PolynomialApproximation_2: solvePA2(u, h); break;
		case PolynomialApproximation_3: solvePA3(u, h); break;
		case PolynomialApproximation_4: solvePA4(u, h); break;
		case PolynomialApproximation_5: solvePA5(u, h); break;
		}
		break;
	case Eyler_1: solveEyler1(u, h); break;
	case Eyler_2: solveEyler2(u, h); break;
	case RungeKutta_4: solveRK4(u, h); break;
	case AdamsBashfortMoulton_2: solveABM2(u, h); break;
	case AdamsBashfortMoulton_3: solveABM3(u, h); break;
	case AdamsBashfortMoulton_4:
	default: solveABM4(u, h); break;
	case PolynomialApproximation_2: solvePA2(u, h); break;
	case PolynomialApproximation_3: solvePA3(u, h); break;
	case PolynomialApproximation_4: solvePA4(u, h); break;
	case PolynomialApproximation_5: solvePA5(u, h); break;
	}
	step++;
}


void PIMathSolver::fromTF(const TransferFunction & TF) {
	if (TF.vector_An.size() >= TF.vector_Bm.size())
		size = TF.vector_An.size() - 1;
	else {
		piCout << "PIMathSolver error: {A} should be greater than {B}";
		return;
	}
	if (size == 0) return;

	step = 0;
	times.fill(0.);
	A.resize(size, size);
	d.resize(size + 1);
	d.fill(0.);
	a1.resize(size + 1);
	a1.fill(0.);
	b1.resize(size + 1);
	b1.fill(0.);
	X.resize(size);
	X.fill(0.);
	F.resize(5);
	for (uint i = 0; i < 5; ++i)
		F[i].resize(size), F[i].fill(0.);
	k1.resize(size);
	k1.fill(0.);
	k2.resize(size);
	k2.fill(0.);
	k3.resize(size);
	k3.fill(0.);
	k4.resize(size);
	k4.fill(0.);
	xx.resize(size);
	xx.fill(0.);
	XX.resize(size);
	XX.fill(0.);
	for (uint i = 0; i < size + 1; ++i)
		a1[size - i] = TF.vector_An[i];
	for (uint i = 0; i < TF.vector_Bm.size(); ++i)
		b1[size - i] = TF.vector_Bm[i];
	double a0 = a1[0];
	a1 /= a0;
	b1 /= a0;

	d[0] = b1[0];
	for (uint i = 1; i < size + 1; ++i) {
		sum = 0.;
		for (uint m = 0; m < i; ++m)
			sum += a1[i - m] * d[m];
		d[i] = b1[i] - sum;
	}

	for (uint i = 0; i < size - 1; ++i)
		for (uint j = 0; j < size; ++j)
			A[j][i] = (j == i + 1);
	for (uint i = 0; i < size; ++i)
		A[i][size - 1] = -a1[size - i];
	for (uint i = 0; i < size; ++i)
		d[i] = d[i + 1];
}


void PIMathSolver::setTime(double time) {
	times.pop_back();
	times.push_front(time);
}


void PIMathSolver::solveEyler1(double u, double h) {
	F[0] = A * X + d * u;
	X += F[0] * h;
	moveF();
}


void PIMathSolver::solveEyler2(double u, double h) {
	F[0] = A * X + d * u;
	X += (F[0] + F[1]) * h / 2.;
	moveF();
}


void PIMathSolver::solveRK4(double u, double h) {
	td = X[0] - F[0][0];
	k1 = A * X + d * u;
	xx = k1 * h / 2.;
	XX = X + xx;
	k2 = A * (XX + k1 * h / 2.) + d * (u + td / 3.);
	xx = k2 * h / 2.;
	XX += xx;
	k3 = A * (XX + k2 * h / 2.) + d * (u + td * 2. / 3.);
	xx = k3 * h;
	XX += xx;
	k4 = A * (XX + k3 * h) + d * (u + td);
	// cout << k1 << k2 << k3 << k4 << endl;
	X += (k1 + k2 * 2. + k3 * 2. + k4) * h / 6.;
	F[0] = X;
}


void PIMathSolver::solveABM2(double u, double h) {
	F[0] = A * X + d * u;
	XX   = X + (F[0] * 3. - F[1]) * (h / 2.);
	F[1] = A * XX + d * u;
	X += (F[1] + F[0]) * (h / 2.);
	moveF();
}


void PIMathSolver::solveABM3(double u, double h) {
	F[0] = A * X + d * u;
	XX   = X + (F[0] * 23. - F[1] * 16. + F[2] * 5.) * (h / 12.);
	F[2] = A * XX + d * u;
	X += (F[2] * 5. + F[0] * 8. - F[1]) * (h / 12.);
	moveF();
}


void PIMathSolver::solveABM4(double u, double h) {
	F[0] = A * X + d * u;
	XX   = X + (F[0] * 55. - F[1] * 59. + F[2] * 37. - F[3] * 9.) * (h / 24.);
	F[3] = A * XX + d * u;
	X += (F[3] * 9. + F[0] * 19. - F[1] * 5. + F[2]) * (h / 24.);
	moveF();
}


void PIMathSolver::solvePA(double u, double h, uint deg) {
	F[0] = A * X + d * u;
	M.resize(deg, deg);
	Y.resize(deg);
	pY.resize(deg);

	for (uint k = 0; k < size; ++k) {
		for (uint i = 0; i < deg; ++i) {
			td = 1.;
			ct = times[i];
			for (uint j = 0; j < deg; ++j) {
				M[j][i] = td;
				td *= ct;
			}
		}
		for (uint i = 0; i < deg; ++i)
			Y[i] = F[i][k];
		/// find polynom
		// if (step == 1) cout << M << endl << Y << endl;
		M.invert(&ok, &Y);
		// if (step == 1) cout << Y << endl;
		if (!ok) {
			solveEyler2(u, h);
			break;
		}
		// calc last piece
		x0 = 0.;
		td = 1.;
		t  = times[0];
		for (uint i = 0; i < deg; ++i) {
			x0 += Y[i] * td / (i + 1.);
			td *= t;
		}
		x0 *= t;

		x1 = 0.;
		td = 1.;
		t  = times[1];
		for (uint i = 0; i < deg; ++i) {
			x1 += Y[i] * td / (i + 1.);
			td *= t;
		}
		x1 *= t;
		lp = x0 - x1;

		if (deg > 2) {
			// calc prev piece
			x0 = 0.;
			td = 1.;
			dh = times[1] - times[2];
			if (dh != 0. && step > 1) {
				t = times[2];
				for (uint i = 0; i < deg; ++i) {
					x0 += Y[i] * td / (i + 1.);
					td *= t;
				}
				x0 *= t;
				ct = x1 - x0;
				// calc correction
				ct -= pY[k];
			}
			// calc final
			X[k] += lp - ct;
			pY[k] = lp;
		} else {
			X[k] += lp;
			pY[k] = lp;
		}
	}
	moveF();
}


void PIMathSolver::solvePA2(double u, double h) {
	if (step > 0)
		solvePA(u, h, 2);
	else
		solveEyler1(u, h);
}


void PIMathSolver::solvePA3(double u, double h) {
	if (step > 1)
		solvePA(u, h, 3);
	else
		solvePA2(u, h);
}


void PIMathSolver::solvePA4(double u, double h) {
	if (step > 2)
		solvePA(u, h, 4);
	else
		solvePA3(u, h);
}


void PIMathSolver::solvePA5(double u, double h) {
	if (step > 3)
		solvePA(u, h, 5);
	else
		solvePA4(u, h);
}


void PIMathSolver::moveF() {
	for (uint i = F.size() - 1; i > 0; --i)
		F[i] = F[i - 1];
}