#pragma once

#include "BSplineOpen.h"
#include "BigMatrix.h"
#include "TArray.h"

//////////////////////////////////////////////////////////////////////
// Class encapsulating approximation algorithm for sampled data,
// like mocap data. It holds the given number of samples and
// approximates them with a natural BSpline.
//
// Controls:
//   smoothness of knot distribution - can be controlled via the filter size
//   number of keys/knots
//   spline degree
//   deep derivative depth influences the initial knot distribution
//   power of the deep derivative may be controlled in the initKnotsDerivDistributed
//
//   for automatic tuning, try several:
//     minus-degrees of the filter
//     degrees of the spline
//     number of knots
//
//     each time, try to refine the knots by iterative algorithm
//
// Usage:
//   for construction of a typical spline, construct the approximator
//   supplying the data size and degree; then feed the data through [] operator;
//   then form knot distribution with initKnot... (nNumKnots); then call newSplineNewtonRaphson()
//
//////////////////////////////////////////////////////////////////////
class BSplineApproximator:
	public _reference_target_t
{
public:
	typedef std::vector<Vec3d> Vec3dArray;

	BSplineApproximator (unsigned nNumKeys, int nDegree, bool isOpen);
	~BSplineApproximator (void);

	struct Key
	{
		Vec3d pt;
		float t;
	};

	struct KeyTimeSort
	{
		bool operator() (const Key& left, const Key& right) const {return left.t < right.t;}
		bool operator() (const Key& left, float right) const {return left.t < right;}
		bool operator() (float left, const Key& right) const {return left < right.t;}
	};

	Key& operator [] (int i) {return m_arrKeys[i];}
	const Key& operator [] (int i) const {return m_arrKeys[i];}

	// quality of approximation structure
	struct Quality
	{
		//double fR[3]; // residual sum of squared differences
		double fRtotal; // sum of fR[0..2]
		double fKnotPenalty; // log of mult of differencies between knots
		double fKnotPenaltyBlended;// multiplier that is multipled by the residual to get the target functional
		double fTargetF; // the functional to be minimized
	};

	// Knot Deletion context
	struct OptCtxKD
	{
		// error to reach by deleting knots, per sample
		float fErrSingleTarget;
		// optimize by Newton-raphson?
		bool bOptimizeNR;
		// max number of iterations during one NR step
		int nMaxIterationsNR;
		// the minimal ratio samples/knots when the NR pass is enabled
		float fMinSPKForNR;

		OptCtxKD():
			fErrSingleTarget (0.0001f),
			nMaxIterationsNR(15),
			bOptimizeNR (true),
			fMinSPKForNR(1)
		{
		}
	};

	struct OptCtxLD: public OptCtxKD
	{
		// lower degree spline initial error
		float fErrLD;
		// optimize by removing knots?
		bool bOptimizeKD;

		OptCtxLD ():
			fErrLD(0.003f),
			bOptimizeKD (true)
			{
			}
	};

	// optimization context for 3-step algorithm
	struct OptCtx3Step: public OptCtxLD
	{
		// error to reach by incrementing number of knots
		float fErrFrom;
		// choose knots by lower degree piecewise approximant algorithm?
		bool bKnotsFromPieces;

		OptCtx3Step():
			fErrFrom (0.001f),
			bKnotsFromPieces(true)
			{
			}
	};

	// progressive building
	struct OptCtxProg
	{
		// desirable error of the resulting spline
		float fErr;
		// number of variable spans
		int nSpans;
		// step of discrete search
		float fStep;
		// use Newton-Raphson last step?
		bool bOptimizeNR;
		// max number of iterations during one NR step
		int nMaxIterationsNR;
		// coefficients telling how fast the progressive spline should move forward
		// or backward
		float fFwdSpeed, fBwdSpeed;

		OptCtxProg ():
			fErr (0.005f),
			nSpans (2),
			bOptimizeNR (true),
			fStep (0.25f),
			fFwdSpeed (0.25f),
			fBwdSpeed (0.5f)
			{
			}
	};

	// progressive refinement
	struct OptCtxPR
	{
		// the desirable target error (sum of error squares / number of samples) 3e-4 is quite good
    float fErrSingle; 
		// max number of knots inserted simultaneously
		int nMaxKIN;

		OptCtxPR ():
			fErrSingle (0.0002f),
			nMaxKIN (10)
			{
			}
	};

	// autorefine history
	struct HistoryARElement
	{
		double fError;
		int numKnots;

		HistoryARElement (double _error, int _knots): fError(_error), numKnots(_knots) {}
	};
	typedef std::deque<HistoryARElement> HistoryAR;

	struct OptCtxComboRefine:
		public OptCtxPR,
		public OptCtxKD
	{
		int nMaxNRIterations2; // max NR iterations during post-refinement improvement with NR

		OptCtxComboRefine ():
			nMaxNRIterations2(30)
			{
			}
	};

	// combined refinement-NR-KD algorithm
	void newSplineComboRefine (OptCtxComboRefine& ctx, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// automatic insertion of knots until reached the desirable error
	void newSplineAutoRefine (OptCtxPR& ctx, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// creates a spline using 3-step refinement strategy:
	// initial distribution, addition of knots, deletion of knots
	// automatically estimates the number of knots needed
	void newSplineAuto3Step (OptCtx3Step& params, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// initializes the knots, as described by the optimization context
	void initKnotsAuto3StepDD (OptCtx3Step& params, int& numKnots);

	// automatic reduction from full-power spline
	void newSplineAutoKDFull (OptCtxKD& ctx, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);
	
	// automatic lower-dimension piecewise approximant 
	void newSplineAutoLD (OptCtxLD& ctx, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// automatic progressive combinatory spline
	void newSplineAutoProgressive (OptCtxProg& ctx, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// optimizes position of the given knot, trying to reach the farthest distance with the next knots
	float optimizeProgressiveStep (OptCtxProg& ctx, int nKnot, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality)
	{
		return optimizeProgressiveStep(ctx, nKnot, ctx.nSpans, pSpline, pQuality);
	}
	float optimizeProgressiveStep (OptCtxProg& ctx, int nKnot, int nDepth, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	float getPartialError (BSplineVec3d* pSpline, int nSampleStart, int nSampleEnd);

	// optimizes the spline to the given values of error to,
	// with knot removal
	void optimizeKD(OptCtxKD& ctx, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// creates a new spline (or refines an existing one) corresponding to the optimization context
	void newSplineLD (OptCtxLD& ctx, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// initializes knots from the given lower-degree spline
	void initKnotsFromLDS (class CB3PApproximation& appr);

	// creates a spline using n-th derivative uniform initial knot distribution
	// and Newton-Raphson algorithm with ... modification to limit jumps for optimization of knot positions
	// NOTE: before calling this function, a knot distribution function must be called
	void newSplineNewtonRaphson (int nMaxIterations, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	bool refineSplineNewtonRaphson (BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);

	// gets the best approximation to the data with the given number of knots
	BSplineVec3d* newSplineRandomKnots (int nNumKnots, Quality* pQuality = NULL);

	int numKeys()const
	{
		return m_arrKeys.size();
	}

	// makes initial knot estimate, given the tolerance (average error per sample)
	void estimateKnots(float fAveErr);

	BSplineVec3d* newSplineDense (Quality*pQuality = 0);

	// makes a spline whose knots are evently distributed according to the d+1'th derivative distribution
	void newSplineDistributed (int nNumKnots, int nMinusDegree, int nMaxIterations, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality = 0);

	// makes up random knots in the m_arrKnots
	void initKnotsRandom (int nNumKnots);

	// copies the array of knots from the given spline knots
	void initKnotsFromSpline  (BSplineVec3d* pSpline);

	// computes the jacobian into the given matrix, for the given spline
	void initJacobian (CBigMatrix& mxJ, BSplineVec3d* pSpline, int numSamples/*ToTakeIntoAccount*/);

	// computes CPs of the spline, given the inverse jacobian of the current data/this spline
	void initCPs (BSplineVec3d* pSpline, CBigMatrix& mxInvJ);
	void initCPs (BSplineVec3d* pSpline, CBigMatrix& mxInvJ, int numSamples);

	// set the given quality structure from the spline
	void initQuality (Quality* pQuality, BSplineVec3d* pSpline);
	// calculates the quality only for the first numSamples
	void initQuality (Quality* pQuality, BSplineVec3d* pSpline, int numSamples);

	// sets the knots distributed evently in the space of d+1-alpha-th derivative of the approximated function.
	// alpha == nMinusDegree
	// the approximated function is reconstructed using 2*m_nHalfFilterSize knots around the interval,
	// as a polynom of d+1 degree. The value of the derivative in the middle of the interval
	// is summed and projected back into the time space to produce knot spacing that is sensitive to the changing
	// d+1-alpha-th derivative of the original data
	void initKnotsDerivDistributed (int nNumKnots, int nMinusDegree = 0);



	// test - fill the first samples with pure parabola and check the estimation capabilities of the high derivative estimator
	void testDerivativeEstimator();

	// calculates the sum of errors of the spline from 0-th to i-th sample and records it into the i-th element of the OUT array
	// each sample error is recorded and summed in corresponding array item
	void calculateSumError (BSplineVec3d* pSpline, FloatArray& arrSumError);

	// this moves knots so that they're distributed evently according to the error
	// returns the metric of knot movement around
	// the array must contain error sum for each interval (m_nNumKeys-1 total)
	float distributeKnotsEvenError (FloatArray& arrSumError);

	// intitializes the knot array so that the knots are evently distributed
	void initKnotsEven (int nNumKnots);

	// initializes the knot array so that each knot corresponds to one sample point
	void initKnotsFull();

	// deletes the knot that has the least delta function
	// copies the knots from the spline, and removes one of them
	float removeLeastContributingKnot (BSplineVec3d* pSpline);

	// removes duplicate knots within one sampling interval
	void removeKnotsExtra();

	// constructs approximation of the samples
	float constructB3PApproximation (class CB3PApproximation& approximation, int nDegree, float fError);

	// returns the degree
	int getDegree () const {return m_nDegree;}

	// sets the degree, recalculates all internal tables and invalidates knot storage
	void setDegree (int nDegree);

	// sets the smoothness factor - how smooth the spline should be. 0 is no smoothing at all
	void setSmoothness (float fSmoothness);

	// creates a new spline out of the given array of knots
	BSplineVec3d* newSpline (Quality* pQuality);
protected:
	// finds the first sample that's >= the given time
	int findSample (float t);

	// sets the given basis for NR calculations random
	void initRandomBase (CBigMatrix& mxBase);

	// moves the knots of the current spline, according to the anti-hessian computed in the previous step
	// out of the given basis; also, the array of samples from the previous spline must be in place
	bool splineNR_MoveKnots (CBigMatrix& mxBase, const CBigMatrix& mxHInv, Vec3dArray& arrSplineSamples, BSplineVec3d_AutoPtr& pSpline, Quality* pQuality);
	
	// finds Hessian for the given starting spline, possibly modifying the spline
	void splineNR_Hessian(const CBigMatrix& mxBase, CBigMatrix& mxH, BSplineVec3d_AutoPtr& pSpline, Vec3dArray& arrSplineSamples, Quality* pQuality);
	
	// given the predicted ort increments and basis functions, clamp the ort increment vector
	// to disallow breaknig the knot sequence rules
	void clampKnotDelta (const CBigMatrix& mxBase, DoubleArray& arrOrts);

	// creates a new spline out of the given array of knots
	BSplineVec3d* newSpline (Quality* pQuality, int numKeys);

	// creates a spline of the given degree and the number of knots recorded in the array m_arrKnots.
	// copies knot times to the spline
	BSplineVec3d* newEmptySpline(int nDegree);

	// fills in the array of knots with dense values (dense enough for the approximation to become almost an interpolation)
	void initDenseKnots ();
public:
	// computes spline samples into the given array (upon output, the array is of the same size as the sample array)
	void computeSplineSamples (BSplineVec3d* pSpline, Vec3dArray& arrSplineSamples);

	// adds ort nOrt multiplied by k to the array of inner knots
	void moveInnerKnots (const CBigMatrix& mxBase, int nOrt, double k);

	// estimates average (d+1)'th derivative at the sample interval [i,i+1], d == m_nDegree
	Vec3d estimateDeepDerivative (int i, int nMinusDegree = 0);

	// estimates the integral of the deep derivative over the whole approximation interval 
	float estimateKnotDistributionIntegral (int nMinusDegree = 0);
	

	// initializes the matrix m_mxFilter:
	// if multiplied by vector of samples on the right, it produces 

private:
	typedef TFixedArray<Key> KeyArray;
	KeyArray m_arrKeys;

	// the degree of the spline to be constructed
	int m_nDegree;

	// the array of estimated knots
	FloatArray m_arrKnots;

	// the filter with which we estimate the n-th derivative of the function being approximated
	// this is essentially t^i, there are m_nFilterSize of them, with m_nFilterSize being slightly more than m_nDegree/2
	// the filter goes on the right and on the left of the interval being analysed.
	// the interval left and right boundaries
	//double* m_pFilter; 
	int m_nHalfFilterSize ;
	//unsigned* m_arr


	// inverse of Basis matrix, size d*2
	CBigMatrix m_mxFilter;

	// this is the last time best step in NR iteration
	float m_fStepNRmu;

	// this is the importance of smoothness of the spline. ->0 means not important
	double m_fSmoothness;

	// if set to true, then jacobian is formed without the last CP, and CP0 += last one
	// the new spline is made with matching start and end knots
	bool m_bLoop;
	bool m_isOpen;
};

TYPEDEF_AUTOPTR(BSplineApproximator);