/** SGX

The   sss    gggg x   x   
     s      g      x x    Engine
      sss   g  gg   x
         s  g   g  x x          Copyright 1995-2009.
     ssss    gggg x   x         Steven Goodwin. All Rights Reserved.
    
This version has been released under the GNU Public License v2.0.

Please see the license files for details.

File: sgx/src/sgx/graphics/curves/path.cpp

*/

#include "sgx/core/core.hpp"
#include "sgx/filesystem/filesystem.hpp"
#include "sgx/graphics/graphics.hpp"

/*
This method is passed a list of bezier control points. The number of bezier 
segments is (numPoints - 3). numPoints must be >= 4. The method must calculate 
8 subpoints per bezier section, equally spaced in parameter space along each 
bezier section. 

It must also calculate the local orientation at each subpoint. 
Assume an "up" vector of (0,1,0) so that your local orientations have no roll 
along the spline. Allocate space for the required number of matrices and store 
the calculated positions/orientations in each matrix.
*/

void CBezierSegment::calculatePositionAtT(sgxVector3 &pos, const float t)
{
	tREAL32 time = evaluateTime(t);

	// The Berstein coefficients are used here.
	// This could be further optimized by taking sucessive differences
	// (as we worked through in the interview) but I'd wait until
	// seeing the profiling before optimizing.
	float tm1 = 1.0f-time;
	float tm12= tm1*tm1;
	float t2  = time*time;
	float b1 = tm12*tm1;	// (1-t)^3
	float b2 = 3*tm12*time;	// 3(1-t)^2.t
	float b3 = 3*t2*tm1;	// 3t^2.(1-t)
	float b4 = t2*time;		// t^3

	// Hand multiple the matrix, as no members exist in the sample provided
	// m[0][0] = 1st node, x.
	// m[0][1] = 1st node, y, etc
	pos.x = m_Control.m[0][0]*b1 + m_Control.m[1][0]*b2 + m_Control.m[2][0]*b3 + m_Control.t.x*b4;
	pos.y = m_Control.m[0][1]*b1 + m_Control.m[1][1]*b2 + m_Control.m[2][1]*b3 + m_Control.t.y*b4;
	pos.z = m_Control.m[0][2]*b1 + m_Control.m[1][2]*b2 + m_Control.m[2][2]*b3 + m_Control.t.z*b4;
}

void CBezierSegment::evaluateSections()
{
	int i;	// loop counter moved outside loops because of MSVC 6 bug
	float t = 0;
	float delta_t = 1.0f/SGX_BEZIER_SECTIONS;

	for(i=0; i<SGX_BEZIER_SECTIONS; ++i, t+=delta_t) {
		calculatePositionAtT(m_Section[i].pos, t);
		m_Section[i].t = t;
	}

	// Store a placeholder for the end control point.
	// This makes the calcs easier as we don't have
	// to explicitly handle the edge cases.
	// (Do this before the delta's are calculated so we have data to use with m_Section[i+1])
	m_Section[SGX_BEZIER_SECTIONS].t = 1.0f;
	m_Section[SGX_BEZIER_SECTIONS].pos = m_Control.t;
	m_Section[SGX_BEZIER_SECTIONS].delta.x = 
	m_Section[SGX_BEZIER_SECTIONS].delta.y = 
	m_Section[SGX_BEZIER_SECTIONS].delta.z = 0;

	// NOTE: Vector should have overloaded functions to make
	// this code neater.
	// OPT: Speed-ups available by using ptr = &m_Section[i]; and ptr++
	for(i=0;i<SGX_BEZIER_SECTIONS;++i) {
		m_Section[i].delta.x = m_Section[i+1].pos.x - m_Section[i].pos.x;
		m_Section[i].delta.y = m_Section[i+1].pos.y - m_Section[i].pos.y;
		m_Section[i].delta.z = m_Section[i+1].pos.z - m_Section[i].pos.z;
	}
}

tREAL32 CBezierSegment::evaluateTime(tREAL32 t)
{
	if (t < 0) {
		return m_bLoopAtLimits ? t+1 : 0; 
	} else if (t > 1) {
		return m_bLoopAtLimits ? t-1 : 1; 
	}

	return t;
}

void CBezierSegment::getPosition(sgxVector3 &pos, const float t)
{
	tREAL32 time = evaluateTime(t);

	if (time <= 0) {
		pos.x = m_Control.m[0][0];
		pos.y = m_Control.m[0][1];
		pos.z = m_Control.m[0][2];
	} else if (time >= 1.0f) {
		pos = m_Control.t;
	}

	calculatePositionAtT(pos, time);
}

void CBezierSegment::getFastPosition(sgxVector3 &pos, const float t)
{
	// These cases should not get called. Depending on the
	// philosophy of the engine, these could assert and return failure.
	if (t < 0) {
		pos.x = m_Control.m[0][0];
		pos.y = m_Control.m[0][1];
		pos.z = m_Control.m[0][2];
	} else if (t >= 1.0f) {
		pos = m_Control.t;
	}

	// Generic calculations:
	// Just slerp between the closest two entries.
	int first = int(t*SGX_BEZIER_SECTIONS);

	// how far between the first & second segments are we? [0-1 range]
	float fraction = (t - m_Section[first].t) / (m_Section[first+1].t - m_Section[first].t);

	// Basic slerp: pos = diff*fraction + start;
	pos.x = m_Section[first].delta.x * fraction + m_Section[first].pos.x;
	pos.y = m_Section[first].delta.y * fraction + m_Section[first].pos.y;
	pos.z = m_Section[first].delta.z * fraction + m_Section[first].pos.z;
}

tBOOL CSpline::generateLocalSpaces(int numPoints, sgxVector3* pPoints)
{
	// Check inputs & return failure
	sgxAssertRts(numPoints >= 4, FALSE);
	sgxAssertRts(pPoints, FALSE);

	m_Beziers.reserve(numPoints-3);

	// Find the bezier equation:
	// We calculate a bezier between points 1,2 & 3,4, followed by 2,3 & 4,5, etc
	// until we reach the end. The tangent at each changeover must be
	// identical.
	// NOTE: This is sub-optimal because in the second, and subsequent,
	// beziers where are 3 repeated segments.
	for(int i=0;i<numPoints-3;++i) {
		// Temporary ptr for speed
		float *p = &m_Beziers[i].m_Control.m[0][0];

		// NOTE: Would rather move this functionality into the matrix class
		*p++ = pPoints[i].x;
		*p++ = pPoints[i].y;
		*p++ = pPoints[i].z;

		*p++ = pPoints[i+1].x;
		*p++ = pPoints[i+1].y;
		*p++ = pPoints[i+1].z;

		*p++ = pPoints[i+2].x;
		*p++ = pPoints[i+2].y;
		*p++ = pPoints[i+2].z;

		// end control point is placed in the matrix's translation member
		// Not sure if this completely satisfactory, though.
		m_Beziers[i].m_Control.t = pPoints[i+3];
		m_Beziers[i].evaluateSections();
	}

	return TRUE;
}

float CBezierSections::getDistanceSquared(const sgxVector3 *pPos) const
{
	sgxAssert(pPos && "The pointer must not be null");
	return pPos->getDistanceSquared(pos);
}

const CBezierSections *CSpline::getSection(const CBezierHandle handle) const
{
	sgxAssert(handle.m_iBezier >= 0 && handle.m_iBezier <= m_Beziers.size());
	sgxAssert(handle.m_iSection >= 0 && handle.m_iSection < SGX_BEZIER_SECTIONS);
	return &m_Beziers[handle.m_iBezier].m_Section[handle.m_iSection];
}

float CSpline::getDistanceSquared(const CBezierHandle &current, const sgxVector3 *pPos) const
{
	return getSection(current)->getDistanceSquared(pPos);
}

tBOOL CSpline::getClosestSegment(CBezierHandle &rts, const sgxVector3 *pPos) const
{
	if (!pPos) {
		return FALSE;
	}

	// search all
	CBezierHandle curr;	// ctor makes this point to the beginning object
	CBezierHandle next;

	// Prepare current closest point
	float closest = getDistanceSquared(curr, pPos);
	rts = curr;

	// iterator through all other segments
	while(getNextSegment(next, curr)) {
		float dist = getDistanceSquared(next, pPos);
		if (dist < closest) {
			closest = dist;
			rts = curr;
		}
		curr = next;	
	}
	
	return TRUE;	// can not fail, if input data is correct
}

tBOOL CSpline::getNextSegment(CBezierHandle &rts, const CBezierHandle &current) const
{
	sgxAssert(current.m_iBezier >= 0 && current.m_iBezier <= m_Beziers.size() && "Invalid bezier segment");
	sgxAssert(current.m_iSection >= 0 && current.m_iSection < SGX_BEZIER_SECTIONS && "Invalid section");

	rts.m_iBezier = current.m_iBezier;
	rts.m_iSection = current.m_iSection+1;

	if (SGX_BEZIER_SECTIONS == rts.m_iSection) {
		// Because the beziers overlap, there are a number of repeated
		// sections. In this case, with 8 sections, the 3-1 split maps
		// to 6-2 of repeated-to-unique sections.
		rts.m_iSection = (3*SGX_BEZIER_SECTIONS)/4;

		if (m_Beziers.size() == rts.m_iBezier++) {
			return FALSE; // end of bezier
		}
	}
	return TRUE;
}

tBOOL CSpline::getPrevSegment(CBezierHandle &rts, const CBezierHandle &current) const
{
	sgxAssert(current.m_iBezier >= 0 && current.m_iBezier <= m_Beziers.size() && "Invalid bezier segment");
	sgxAssert(current.m_iSection >= 0 && current.m_iSection < SGX_BEZIER_SECTIONS && "Invalid section");

	rts.m_iBezier = current.m_iBezier;
	rts.m_iSection = current.m_iSection;

	if (0 == rts.m_iSection--) {
		// The overlap occurs in both directions. Note, however,
		// that the specific segements will vary, depending on the
		// direction of travel.
		rts.m_iSection = (SGX_BEZIER_SECTIONS-1) - (3*SGX_BEZIER_SECTIONS)/4;
		if (0 == rts.m_iBezier--) {
			return FALSE; // start of bezier
		}
	}
	return TRUE;
}

