#include "StdAfx.h"
#include "MotionOptimizerView.h"
#include "bsplineapprtest.h"
#include "BSplineApproximator.h"
#include "GLUtils.h"
#include "BezierApproximation.h"
#include "BezierVec3dApproximation.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

#undef malloc
#undef calloc
#undef realloc
#undef free

void* CryModuleMalloc(size_t size) 
{
	return malloc(size);
};

void* CryModuleCalloc(size_t num,size_t size)
{
	return calloc(num, size);

};

void* CryModuleRealloc(void *memblock,size_t size)
{
	return realloc(memblock, size);
};

void  CryModuleFree(void *ptr)
{
	free(ptr);
};

void CBSplineApprTest::initApproximator (const std::vector<int>& vTimes, const std::vector<Vec3d>& vPos)
 {
	for (unsigned i = 0; i < tmin(vPos.size() , vTimes.size()); ++i)
	{
		(*m_pApproximator)[i].pt = vPos[i];
		(*m_pApproximator)[i].t  = vTimes[i] * m_fTimeScale;
	}
}

CBSplineApprTest::CBSplineApprTest (const std::vector<int>& vTimes, const std::vector<Vec3d>& vPos, Params params, bool isOpen):
	m_fTimeScale (1.0f/*/float(BSplineKnots::g_nStdTicksPerSecond)*/),
	m_Params(params),
	m_isOpen(isOpen)
{
	m_fMinY = m_fMaxY = 0;

	static int nAttempt = 0;

	int nPosSize= vPos.size() , nTimesSize = vTimes.size();

	// calculate the ranges for each coordinate, and use the maximum to calculate the
	// required target error
	m_ptMin = m_ptMax = vPos[0];
	for (unsigned i = 1; i < tmin(vPos.size() , vTimes.size()); ++i)
	{
		for (int nCoord = 0; nCoord < 3; ++nCoord)
		{
			m_ptMin[nCoord] = tmin (m_ptMin[nCoord], vPos[i][nCoord]);
			m_ptMax[nCoord] = tmax (m_ptMax[nCoord], vPos[i][nCoord]);
		}
	}

	// calculate the range for displaying the curves
	m_fMinY = tmin (m_ptMin.x, m_ptMin.y, m_ptMin.z);
	m_fMaxY = tmax (m_ptMax.x, m_ptMax.y, m_ptMax.z);

	// the time range to display the curves
	m_fMinX = vTimes[0] * m_fTimeScale;
	m_fMaxX = vTimes.back() * m_fTimeScale;

	m_pApproximator = new BSplineApproximator (vTimes.size(), 1, m_isOpen);
	initApproximator (vTimes, vPos);

	optimize ();
}

void CBSplineApprTest::optimize()
{
	m_pApproximator->setSmoothness((float)m_Params.fSmoothing);
	// the target error per one frame/sample of data
	// we determine the max error adaptively depending on the data
	float fRangeY =
		0.2f*tmax (m_ptMax.x - m_ptMin.x, m_ptMax.y - m_ptMin.y, m_ptMax.z - m_ptMin.z) +
		0.8f*(m_fMaxY-m_fMinY);
	tforceRangeIncl<float>(fRangeY, m_Params.fValRangeClampMin, m_Params.fValRangeClampMax);
	float fErrSingleTarget = tsqr (fRangeY/m_Params.fPrecision);
	float fErrTarget = m_pApproximator->numKeys() * fErrSingleTarget;

	BSplineApproximator::Quality quality;
	// first try to find if it's a linear/constant sequence
	for (int d = 1; d <= m_Params.nDegree; ++d)
	{
		m_pApproximator->setDegree (d);
		m_pApproximator->initKnotsEven (2);
		m_pSpline = m_pApproximator->newSpline(&quality);
		if (quality.fRtotal < fErrTarget)
			break;
		else
			m_pSpline = NULL;
	}

	int nIters = m_Params.nNRIters;
	float fB3PError = -1;

	if (!m_pSpline)
	{
		m_pApproximator->setDegree(m_Params.nDegree);
		//initApproximator (vTimes, vPos);

		if (false)
		{
			BSplineApproximator::OptCtxComboRefine ctxCR;
			ctxCR.fErrSingleTarget = fErrSingleTarget;
			ctxCR.fErrSingle = ctxCR.fErrSingleTarget/m_Params.fErrorFallback;
			ctxCR.bOptimizeNR = m_Params.bEnableNR?true:false;  // set this to true for non-realtime optimization
			ctxCR.fMinSPKForNR = m_Params.fMinSPKForNR;

			m_pApproximator->newSplineComboRefine(ctxCR, m_pSpline, &quality);
		}
		else if (true)
		{
			BSplineApproximator::OptCtxProg optCtx;
			m_pApproximator->newSplineAutoProgressive(optCtx, m_pSpline, &quality);
		}
		else
		{
			BSplineApproximator::OptCtxKD optCtx;
			optCtx.bOptimizeNR = true;
			m_pApproximator->newSplineAutoKDFull(optCtx, m_pSpline, &quality);
		}

		if (false)
		{
			BSplineApproximator::OptCtx3Step optCtx;
			optCtx.nMaxIterationsNR = nIters;
			optCtx.fErrFrom = 0.004f;
			optCtx.fErrSingleTarget = 0.006f;
			m_pApproximator->newSplineAuto3Step(optCtx, m_pSpline, &quality);
		}

		if (false)
		{
			BSplineApproximator::OptCtxLD optCtx;
			optCtx.bOptimizeNR = true;
			optCtx.nMaxIterationsNR = nIters;
			optCtx.fErrLD = 0.001f;
			fB3PError = m_pApproximator->constructB3PApproximation(m_B3P, m_Params.nDegree-1, optCtx.fErrLD);
			m_pApproximator->newSplineAutoLD(optCtx, m_pSpline, &quality);
		}
	}

	if (!m_pSpline)
		throw ("Cannot make the spline");

	m_fMinY = m_fMaxY = m_pSpline->getCP(0).x;
	for (int i = 0; i < (int)m_pSpline->numCPs(); ++i)
	{
		Vec3d vCP = m_pSpline->getCP(i);
		m_fMinY = tmin<double> (m_fMinY, vCP.x, vCP.y, vCP.z);
		m_fMaxY = tmax<double> (m_fMaxY, vCP.x, vCP.y, vCP.z);
	}

	{
		char sz[0x800];
		_snprintf (sz, sizeof(sz), "%d iterations, error %g*(b %g + 1)=%g. KEI=%g. KPS^-1=%g. %d samples, %d CPs, %d knots. B3Perr=%g\nBBox:{%g..%g,%g..%g,%g..%g}  %s", nIters, quality.fRtotal ,quality.fKnotPenalty, quality.fTargetF, m_pApproximator->estimateKnotDistributionIntegral(), m_pApproximator->numKeys()/float(m_pSpline->numCPs()), m_pApproximator->numKeys(), m_pSpline->numCPs(), m_pSpline->numKnots(), fB3PError,
			m_ptMin.x, m_ptMax.x, m_ptMin.y, m_ptMax.y, m_ptMin.z, m_ptMax.z, m_pSpline->isOpen()?"Open.":"Closed.");
		//AfxMessageBox(sz);
		m_sMsg = sz;

		_snprintf (sz, sizeof(sz), " Time range %g..%g", m_fMinX, m_fMaxX);
		m_sMsg += sz;
	}
	//nIters += 10;
}

CBSplineApprTest::CBSplineApprTest(const char* szFileName):
	m_Params (Params::nDefPos)
{
	FILE* f = fopen (szFileName, "rt");
	if (f)
	{
//		std::string file("e:\\controller0.txt");
//		FILE * f = fopen(file.c_str(), "rb");
		std::vector<int> vTimes;
		std::vector<Vec3d> vPos;
		//while(!feof(f))
		//for (uint32 i =0; i < numCPs(); ++i )
		while(!feof(f))
		{
			float time;
			Vec3 quat;
			Vec3 pos;
			fscanf(f, "%f %f %f %f %f %f %f", &time, &quat.x, &quat.y, &quat.z, &pos.x, &pos.y, &pos.z);

//			time *= 60;
			vPos.push_back(pos);
			vTimes.push_back(time);
		}

		fclose(f);


		//int t;
		//while (fscanf (f, "%d", &t) == 1)
		//	vTimes.push_back(t);
		//fscanf (f, "#");
		//Vec3d v;
		//while (fscanf (f, "%f %f %f;", &v.x, &v.y, &v.z) == 3)
		//	vPos.push_back(v);
		//fclose (f);

		m_fMinY = m_fMaxY = 0;
		static int nDegree = 3;
		static int nAttempt = 0;
		m_pApproximator = new BSplineApproximator (vTimes.size(), nDegree, strstr(szFileName,"loop")?false:true);
		//nDegree++;
		int nPosSize= vPos.size() , nTimesSize = vTimes.size();
		for (unsigned i = 0; i < tmin(vPos.size() , vTimes.size()); ++i)
		{
			(*m_pApproximator)[i].pt = vPos[i];
			m_fMinY = tmin<double> (m_fMinY, vPos[i].x, vPos[i].y, vPos[i].z);
			m_fMaxY = tmax<double> (m_fMaxY, vPos[i].x, vPos[i].y, vPos[i].z);

			(*m_pApproximator)[i].t  = vTimes[i] / 60.0f;
		}

		m_fMinX = vTimes[0]/60.0f;
		m_fMaxX = vTimes.back()/60.0f;

		BSplineApproximator::Quality quality;
		static int nKnots = 200;
		static int nIters = 2;

		//if (nAttempt++ == 0)
			m_pApproximator->initKnotsDerivDistributed(nKnots, 0);
		//else
		//	m_pApproximator->initKnotsRandom (nKnots);
		m_pApproximator->newSplineNewtonRaphson (nIters, m_pSpline, &quality);

		if (!m_pSpline)
			throw ("Cannot make the spline");

		{
			char sz[200];
			sprintf (sz, "%d iterations, error %g*(b %g + 1)=%g", nIters, quality.fRtotal ,quality.fKnotPenalty, quality.fTargetF);
			AfxMessageBox(sz);
		}
		nIters += 10;
		//nKnots+=2;
	}
	else
		throw ("cannot open file");
}


void CBSplineApprTest::onDraw (CDC* pDC, CMotionOptimizerView* pView)
{
	pDC->SelectObject (&m_fntMain);
	int nKey, nCoord;

	RECT rc;
	pView->GetClientRect(&rc);
	pDC->DrawText(m_sMsg.c_str(), m_sMsg.length(), &rc, DT_END_ELLIPSIS|DT_LEFT);

	float fFirstKnots[2] = {m_pSpline->getKnotTime(0),m_pSpline->getKnotTime(1)};
	Vec3d vTest = m_pSpline->getValue ((fFirstKnots[1]+fFirstKnots[0])/2);

	setSize (pView);
	pDC->SelectObject(GetStockObject (WHITE_BRUSH));
	
	// draw the keys
	pDC->SelectObject(&m_penGrid);
	for (nKey = 0; nKey < m_pSpline->numKnots(); ++nKey)
	{
		float t = m_pSpline->getKnotTime(nKey);
		
		pDC->MoveTo (point (t, m_fMinY));
		pDC->LineTo (point (t, m_fMaxY));
	}

	for (nCoord = 0; nCoord < 3; ++nCoord)
	{
		CPoint ptPrevCP;
		for (nKey = 0; nKey < m_pSpline->numCPs(); ++nKey)
		{
			float t = m_pSpline->getCPPeak (nKey);
			Vec3d vCP = m_pSpline->getCP(nKey);
			pDC->SelectObject(&m_penCPRGB[nCoord]);
			CPoint pt = point (t, vCP[nCoord]);

			// draw the control point cross
			pDC->MoveTo (pt+CSize(-3,0));
			pDC->LineTo (pt+CSize(+2,0));
			pDC->MoveTo (pt+CSize(0,-3));
			pDC->LineTo (pt+CSize(0,+2));

			// draw the cage line between two CPs
			pDC->SelectObject(m_penWeakSplineRGB[nCoord]);
			if (nKey)
			{
				pDC->MoveTo (ptPrevCP);
				pDC->LineTo (pt);
			}

			ptPrevCP = pt;
		}
	}

  //draw the piecewise approximation
	int nPP, nSample;
	for (nCoord = 0; nCoord < 3; nCoord++)
	{
		pDC->SelectObject(&m_penWeakSplineRGB[nCoord]);
		for (nSample = 0, nPP = 0; nPP < m_B3P.numPieces(); ++nPP)
		{
			const CBezierVec3dApproximation& appr = m_B3P.getPiece(nPP);
			double fStep = 1.0/(appr.numSamples()-1);
			for (int i = 0; i < appr.numSamples() && nSample < m_pApproximator->numKeys()-1; ++i, ++nSample)
			{
				int numSubSamples = 4;
				for (int j = 0; j < numSubSamples; ++j)
				{
					if (j > 1 && i == appr.numSamples()-1)
						break;
					CPoint pt = point (
						((*m_pApproximator)[nSample].t*(numSubSamples-j)+(*m_pApproximator)[nSample+1].t*j)/numSubSamples,
						appr.getValueAtTime(fStep*(i+j/float(numSubSamples)))[nCoord]);
					if (i>0 || j>0)
						pDC->LineTo (pt);
					else
						pDC->MoveTo (pt);
				}
			}
		}
	}

	// draw the delta graph
	if (false)
	for (nCoord = 0; nCoord < 3; ++nCoord)
	{
		pDC->SelectObject(&m_penGridRGB[nCoord]);
		CSize szDelta(0,10+nCoord);
		pDC->MoveTo(point(m_pSpline->getKnotTime(0), 0)+szDelta);
		pDC->LineTo(point(m_pSpline->getKnotTime(m_pSpline->numKnots()-1), 0)+szDelta);
		for (nKey = 1; nKey < m_pSpline->numKnots()-1; ++nKey)
		{
			Vec3d ptDelta = m_pSpline->getDelta(nKey);
			CPoint pt = point (m_pSpline->getKnotTime(nKey), ptDelta[nCoord]*200) + szDelta;
			if (nKey == 1)
				pDC->MoveTo(pt);
			else
				pDC->LineTo(pt);
		}
	}

	// draw the original data
	for (int nCoord = 0; nCoord < 3; ++nCoord)
	{
		pDC->SelectObject (m_penRGB + nCoord);

		// draw the original data
		for (int i = 0; i < m_pApproximator->numKeys(); ++i)
		{
			CPoint pt = point ((*m_pApproximator)[i].t, (&(*m_pApproximator)[i].pt.x)[nCoord]);
			pDC->Ellipse (pt.x-2, pt.y-2, pt.x+3, pt.y+3);
		}
	}

	// draw the approximation
	if (false)
	for (int nCoord = 0; nCoord < 3; ++nCoord)
	{
		pDC->SelectObject(&m_penSpline);
		//pDC->SelectObject (m_penRGB + nCoord);
		float t0, t = (*m_pApproximator)[0].t;
		Vec3d v = m_pSpline->getValue(t);
		pDC->MoveTo(point (t, (&v.x)[nCoord]));

		for (int i = 1; i < m_pApproximator->numKeys(); ++i)
		{
			t0 = t;
			t = (*m_pApproximator)[i].t;
			for (int i = 0; i <= 4; ++i)
			{
				float tMiddle = t0 + i*(t-t0)/4;
				v = m_pSpline->getValue(tMiddle);
				pDC->LineTo(point (tMiddle, (&v.x)[nCoord]));
			}
		}
	}

	// draw the deep derivative
	if (false)
	for (int i = 0; i < m_pApproximator->numKeys(); ++i)
	{
		pDC->SelectObject(&m_penGrid);
		CPoint pt = point ((*m_pApproximator)[i].t, m_pApproximator->estimateDeepDerivative(i).x);
		if (i)
			pDC->LineTo(pt);
		else
			pDC->MoveTo(pt);
	}

	// draw the message
	//pDC->ExtTextOut(0,0,0, 0, m_sMsg.c_str(), 0);
}


void CBSplineApprTest::Reoptimize (Params params)
{
	m_Params = params;
	optimize();
}

CBSplineApprTest::Params::Params(DefEnum nDef)
{
	bEnableNR = 1;
	fErrorFallback = 2;
	fPrecision = 100;
	fValRangeClampMin = 0.1f;
	fValRangeClampMax = 100;
	nNRIters = 25;
	nDegree = 3;
	fSmoothing = 0.1;
	fMinSPKForNR = 3.5;

	switch (nDef)
	{
	case nDefRot:
		fErrorFallback = 1.2f;
		fPrecision = 120;
		fValRangeClampMin = 3;
		fValRangeClampMax = 5;
		nDegree = 3;
		fSmoothing = 0.03;
		break;

	case nDefPos:
		fErrorFallback = 2;
		fPrecision = 200;
		fValRangeClampMin = 1;
		fValRangeClampMax = 100;
		nDegree = 3;
		break;
	}
}
