#include "pch.h"
#include "ZZ_Math.h"

void ZZ_MATH::PolyFit::Eigen_Polyfit(const std::vector<double> &xv, const std::vector<double> &yv, std::vector<double> &coeff, int order)
{
	Eigen::MatrixXd A(xv.size(), order + 1);
	Eigen::VectorXd yv_mapped = Eigen::VectorXd::Map(&yv.front(), yv.size());
	Eigen::VectorXd result;

	assert(xv.size() == yv.size());
	assert(xv.size() >= order + 1);


	for (size_t i = 0; i < xv.size(); i++)
	{
		for (size_t j = 0; j < order + 1; j++)
		{
			A(i, j) = pow(xv.at(i), j);
		}
	}


	result = A.householderQr().solve(yv_mapped);

	coeff.resize(order + 1);
	for (size_t i = 0; i < order + 1; i++)
	{
		coeff[i] = result[i];
	}
}

double ZZ_MATH::PolyFit::Eigen_Polyeval(std::vector<double> coeffs, double x)
{
	double result = 0.0;

	for (int i = 0; i < coeffs.size(); i++)
	{
		result += coeffs[i] * pow(x, i);
	}

	return result;
}


Eigen::VectorXd ZZ_MATH::SplineFit::Eigen_Normalize(const Eigen::VectorXd &x)
{
	using namespace Eigen;
	VectorXd x_norm;
	x_norm.resize(x.size());
	const double min = x.minCoeff();
	const double max = x.maxCoeff();

	for (int k = 0; k < x.size(); k++)
	{
		x_norm(k) = (x(k) - min) / (max - min);
	}

	return x_norm;

}

void ZZ_MATH::SplineFit::Test(std::vector<double> const &x_vec, std::vector<double> const &y_vec)
{
	typedef Spline<double, 1> Spline1D;
	typedef SplineFitting<Spline1D> Spline1DFitting;

	
	//VectorXd vx,vy;
	//vx.resize(x_vec.size());
	//vy.resize(y_vec.size());
	//vx.Map(&x_vec.front(), x_vec.size());
	//vy.Map(&y_vec.front(), y_vec.size());

	Eigen::VectorXd vx = Eigen::VectorXd::Map(&x_vec.front(), 5/*x_vec.size()*/);
	Eigen::VectorXd vy = Eigen::VectorXd::Map(&y_vec.front(), 5/*y_vec.size()*/);

	const double scale = 1 / (vx.maxCoeff() - vx.minCoeff());
	const double scale_sq = scale * scale;

	//VectorXd knots = Eigen_Normalize(vx);
	//Spline1D spline = Spline1DFitting::Interpolate(vy.transpose(),3, knots);
	//double a;
	// a = spline.derivatives(0,1)(0);
	//Eigen::VectorXd xvals(5);
	//Eigen::VectorXd yvals(xvals.rows());
	//xvals << 0, 1, 2,3,4;
	//yvals << 0, 1, 4,9,16;

	SplineInterpolation s(vx, vy);
                return;
}

ZZ_MATH::SplineFit::SplineInterpolation::SplineInterpolation(Eigen::VectorXd const &x_vec, Eigen::VectorXd const &y_vec):
	x_min(x_vec.minCoeff()), x_max(x_vec.maxCoeff()),
	spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::
	Interpolate(y_vec.transpose(), 3, scaled_values(x_vec)))
{

}

double ZZ_MATH::SplineFit::SplineInterpolation::operator()(double x) const
{
	return spline_(scaled_value(x))(0);
}

double ZZ_MATH::SplineFit::SplineInterpolation::scaled_value(double x) const
{
	return (x - x_min) / (x_max - x_min);
}

Eigen::RowVectorXd ZZ_MATH::SplineFit::SplineInterpolation::scaled_values(Eigen::VectorXd const &x_vec) const
{
	return x_vec.unaryExpr([this](double x) { return scaled_value(x); }).transpose();
}