
#include "Core/Asura.h"

#include "Cutscene/Asura_Cutscene_Track.h"

#include "Core/Memory/Asura_ResourceAllocator.h"
#include "Debugging/Asura_Assert.h"

///////////////////////////////////////////////////////////////////////////////////////////////////

static const int g_iNumIntermediateTrackPoints = 20;

Asura_Cutscene_Track::Asura_Cutscene_Track()
{
	NumberOfPivots = 0;
	Pivots = 0;
	NumberOfSections = 0;
	TotalLengths = 0;
	TrackLength = 0;
	PointArrivalTimes = 0;
	PointDurations = 0;
	PointFlags = 0;
	m_uTrackFlags = 0;
	m_pxPointOrientations = 0;
	m_pfSubTotalLengths = 0;
	m_pfPointSpeeds = 0;
	#ifdef ASURA_TOOLS
	m_pxRelativePivots = 0;
	m_pxRelativeOrients = 0;
	#endif
}


Asura_Cutscene_Track::~Asura_Cutscene_Track()
{
	Asura_ResourcePushType(ASURA_RESOURCE_CUTSCENE);
	{
		Asura_ResourceDeallocateArray(Pivots);
		Asura_ResourceDeallocateArray(TotalLengths);
		Asura_ResourceDeallocateArray(m_pfSubTotalLengths);
		Asura_ResourceDeallocateArray(PointArrivalTimes);
		Asura_ResourceDeallocateArray(PointFlags);
		Asura_ResourceDeallocateArray(PointDurations);
		Asura_ResourceDeallocateArray(m_pxPointOrientations);
		Asura_ResourceDeallocateArray(m_pfPointSpeeds);
#ifdef ASURA_TOOLS
		Asura_ResourceDeallocateArray(m_pxRelativePivots);
		Asura_ResourceDeallocateArray(m_pxRelativeOrients);
#endif
	}
	Asura_ResourcePopType();
}

void Asura_Cutscene_Track::Copy(const Asura_Cutscene_Track* pxTrack)
{
	Asura_ResourcePushType(ASURA_RESOURCE_CUTSCENE);
	{
		int iNumberOfPoints = pxTrack->GetNumberOfPoints();
		m_uTrackFlags = pxTrack->m_uTrackFlags;

		SetPoints(iNumberOfPoints,pxTrack->GetPoints());

		for(int i=0;i<iNumberOfPoints;i++)
		{
			PointArrivalTimes[i] = pxTrack->PointArrivalTimes[i];
			PointFlags[i] = pxTrack->PointFlags[i];
			m_pxPointOrientations[i] = pxTrack->m_pxPointOrientations[i];
		}

		if(pxTrack->PointDurations)
		{
			Asura_ResourceAllocateArray(PointDurations, iNumberOfPoints);
			for(int i=0;i<iNumberOfPoints;i++)
			{
				PointDurations[i] = pxTrack->PointDurations[i];
			}
		}

		TrackName = pxTrack->TrackName;

		CalculateLengths();
	}
	Asura_ResourcePopType();
}

#ifdef ASURA_TOOLS
void Asura_Cutscene_Track::CalculateRelativePosition(const Asura_Matrix_3x3& xCutsceneOrientInverse,const Asura_Vector_3 xCutscenePosition)
{
	Asura_ResourcePushType(ASURA_RESOURCE_CUTSCENE);
	{
		//get pivot positions relative to cutscene
		if(!m_pxRelativePivots)
		{
			Asura_ResourceAllocateArray(m_pxRelativePivots, NumberOfPivots);
		}

		for(int i=0;i<NumberOfPivots;i++)
		{
			m_pxRelativePivots[i] = (Pivots[i]-xCutscenePosition)*xCutsceneOrientInverse;
		}

		//and orientations (if present)
		if(m_pxPointOrientations)
		{
			int iNumberOfPoints = GetNumberOfPoints();

			if(!m_pxRelativeOrients)
			{
				Asura_ResourceAllocateArray(m_pxRelativeOrients, iNumberOfPoints);
			}

			Asura_Quat xQuat;
			xQuat.FromMatrix(xCutsceneOrientInverse);

			for(int i=0;i<iNumberOfPoints;i++)
			{
				m_pxRelativeOrients[i] = xQuat*m_pxPointOrientations[i];
			}

		}
	}
	Asura_ResourcePopType();
}

void Asura_Cutscene_Track::UpdateRelativeToPosition(const Asura_Matrix_3x3& xCutsceneOrient,const Asura_Vector_3 xCutscenePosition)
{

	if(m_pxRelativePivots)
	{
		for(int i=0;i<NumberOfPivots;i++)
		{
			Pivots[i] = xCutscenePosition + m_pxRelativePivots[i]*xCutsceneOrient;
		}
	}

	if(m_pxPointOrientations && m_pxRelativeOrients)
	{
		Asura_Quat xQuat;
		xQuat.FromMatrix(xCutsceneOrient);

		int iNumberOfPoints = GetNumberOfPoints();
		for(int i=0;i<iNumberOfPoints;i++)
		{
			m_pxPointOrientations[i] = xQuat*m_pxRelativeOrients[i];
			m_pxPointOrientations[i].Normalise();
		}
	}
}

#endif //ASURA_TOOLS

void Asura_Cutscene_Track::SetPoints(int numberOfPoints, const Asura_Vector_3* points)
{
	Asura_ResourcePushType(ASURA_RESOURCE_CUTSCENE);
	{

		int i;


		//delete any old point data
		Asura_ResourceDeallocateArray(Pivots);
		Pivots = 0;
		Asura_ResourceDeallocateArray(TotalLengths);
		TotalLengths = 0;
		Asura_ResourceDeallocateArray(m_pfSubTotalLengths);
		m_pfSubTotalLengths = 0;
		Asura_ResourceDeallocateArray(PointArrivalTimes);
		PointArrivalTimes = 0;
		Asura_ResourceDeallocateArray(PointDurations);
		PointDurations = 0;
		Asura_ResourceDeallocateArray(PointFlags);
		PointFlags = 0;
		Asura_ResourceDeallocateArray(m_pxPointOrientations);
		m_pxPointOrientations = 0;
		Asura_ResourceDeallocateArray(m_pfPointSpeeds);
		m_pfPointSpeeds = 0;

#ifdef ASURA_TOOLS
		Asura_ResourceDeallocateArray(m_pxRelativePivots);
		m_pxRelativePivots = 0;
		Asura_ResourceDeallocateArray(m_pxRelativeOrients);
		m_pxRelativeOrients = 0;
#endif


		Asura_Assert(numberOfPoints>=1,"");
		Asura_Assert(points,"");

		NumberOfSections = 0;
		TotalLengths = 0;
		TrackLength = 0;




		if(numberOfPoints==1)
		{
			//no need for the extra pivot points or the section length data
			//if there is only 1 point
			NumberOfPivots = 1;
			Asura_ResourceAllocateArray(Pivots, NumberOfPivots);
			Pivots[0] = points[0];
		}
		else
		{
			NumberOfPivots = numberOfPoints+2;
			Asura_ResourceAllocateArray(Pivots, NumberOfPivots);
			//copy the normal points into the pivot array
			for(i=0;i<numberOfPoints;i++)
			{
				Pivots[i+1] = points[i];
			}

			//calculate the start and end control points
			CalculateEndControlPoints();

			NumberOfSections = numberOfPoints-1;

		}

		//allocate PointArrivalTimes array , and initialise with default values
		Asura_ResourceAllocateArray(PointArrivalTimes, numberOfPoints);
		for(i=0;i<numberOfPoints;i++)
		{
			PointArrivalTimes[i] = (float)i;
		}

		//allocate flag array , and initialise
		Asura_ResourceAllocateArray(PointFlags, numberOfPoints);
		for(i=0;i<numberOfPoints;i++)
		{
			PointFlags[i] = 0;
		}

		//and point orientations
		Asura_ResourceAllocateArray(m_pxPointOrientations, numberOfPoints);
		for(i=0;i<numberOfPoints;i++)
		{
			m_pxPointOrientations[i].x = 0.0f;
			m_pxPointOrientations[i].y = 0.0f;
			m_pxPointOrientations[i].z = 0.0f;
			m_pxPointOrientations[i].w = 1.0f;
		}
	}
	Asura_ResourcePopType();
}

void Asura_Cutscene_Track::CalculateEndControlPoints()
{
	if(NumberOfPivots<4) return;
	Asura_Assert(Pivots,"");

	if((m_uTrackFlags & TrackFlag_Circular) && GetNumberOfPoints()>=3)
	{
		Pivots[0] = Pivots[NumberOfPivots-3];
		Pivots[NumberOfPivots-1] = Pivots[2];
	}
	else
	{
		Pivots[0] = Pivots[1]*2.0f-Pivots[2];
		Pivots[NumberOfPivots-1] = Pivots[NumberOfPivots-2]*2.0f-Pivots[NumberOfPivots-3];
	}
	
}

void Asura_Cutscene_Track::CalculateLengths()
{
	if(NumberOfPivots==1) return;
	CalculateEndControlPoints();

	Asura_ResourcePushType(ASURA_RESOURCE_CUTSCENE);
	{
		//determine the section lengths
		if(!TotalLengths)
		{
			Asura_ResourceAllocateArray(TotalLengths, NumberOfSections+1);
		}

		if(!m_pfSubTotalLengths && (m_uTrackFlags & TrackFlag_IncreasedSpeedAccuracy))
		{
			Asura_ResourceAllocateArray(m_pfSubTotalLengths, g_iNumIntermediateTrackPoints*NumberOfSections+1);
		}

		TotalLengths[0] = 0;
		float* pfSubTotal = m_pfSubTotalLengths;
		TrackLength = 0.0f;
		for(int i=0;i<NumberOfSections;i++)
		{
			float fLength = 0.0f;

			if (PointFlags[i] & TrackPointFlag_SectionStraight)
			{
				fLength = (Pivots[i+2] - Pivots[i+1]).Magnitude();

				if(m_uTrackFlags & TrackFlag_IncreasedSpeedAccuracy)
				{
					for(int j=0;j<g_iNumIntermediateTrackPoints;j++)
					{
						*pfSubTotal++ = TrackLength + (fLength*(float)j/(float)g_iNumIntermediateTrackPoints);
					}
				}
			}
			else
			{
				// to approximate distance along curve, divide into 20 straight 
				// segments and add up the lengths of the segments.
				Asura_Vector_3 axSegmentPoints[g_iNumIntermediateTrackPoints + 1];

				for (int j = 0; j <= g_iNumIntermediateTrackPoints; j++)
				{
					float f = static_cast<float>(j) / (float)g_iNumIntermediateTrackPoints;
					GetPosition(i, f, &axSegmentPoints[j]);
				}

				for (int j = 0; j < g_iNumIntermediateTrackPoints; j++)
				{
					if(m_uTrackFlags & TrackFlag_IncreasedSpeedAccuracy)
					{
						*pfSubTotal++ = TrackLength + fLength;
					}
					fLength += (axSegmentPoints[j] - axSegmentPoints[j + 1]).Magnitude();
				}
			}

			TrackLength += fLength;

			TotalLengths[i+1] = TrackLength;
		}
		if(pfSubTotal) *pfSubTotal = TrackLength;



		//calculate speeds for each point if needed
		if(m_uTrackFlags & TrackFlag_SmoothSpeed)
		{
			if(!m_pfPointSpeeds)
			{
				Asura_ResourceAllocateArray(m_pfPointSpeeds, NumberOfSections+1);
			}


			//set speeds at each point to be equal to average speed around it
			for(int i=0;i<=NumberOfSections;i++)
			{
				if(PointFlags[i] & TrackPointFlag_Stationary)
				{
					m_pfPointSpeeds[i] = 0;
					continue;
				}

				int iStart = i ? (i-1) : 0;
				int iEnd = i<NumberOfSections ? (i+1) : NumberOfSections;

				float fTime = PointArrivalTimes[iEnd]-PointArrivalTimes[iStart];
				if(PointDurations) fTime -= PointDurations[iStart];
				if(fTime)
				{
					m_pfPointSpeeds[i] = (TotalLengths[iEnd]-TotalLengths[iStart])/fTime;
				}
				else
				{
					m_pfPointSpeeds[i] = 0;//wil probably do as well as anything in this case...
				}
			}
			//do a second pass on the speeds in attempt to make them closer to being constant acceleration
			//in each section.
			for(int i=0;i<=NumberOfSections;i++)
			{
				if(PointFlags[i] & TrackPointFlag_Stationary) continue;

				//get speed that would be obtained if doing constant accel from previous point
				float fSpeed1 = m_pfPointSpeeds[i];
				if(i)
				{
					float fTime = PointArrivalTimes[i]-PointArrivalTimes[i-1];
					if(PointDurations) fTime -= PointDurations[i-1];

					if(fTime)
					{
						fSpeed1 = (2*(TotalLengths[i]-TotalLengths[i-1]))/fTime - m_pfPointSpeeds[i-1];
						if(fSpeed1<0) fSpeed1 = 0;
					}
				}

				//get required start speed for doing constant accel to next point
				float fSpeed2 = m_pfPointSpeeds[i];
				if(i<NumberOfSections)
				{
					float fTime = PointArrivalTimes[i+1]-PointArrivalTimes[i];
					if(PointDurations) fTime -= PointDurations[i];

					if(fTime)
					{
						fSpeed2 = (2*(TotalLengths[i+1]-TotalLengths[i]))/fTime - m_pfPointSpeeds[i+1];
						if(fSpeed2<0) fSpeed2 = 0;
					}
				}

				//go with the average
				m_pfPointSpeeds[i] = (fSpeed1+fSpeed2)/2.0f;
			}
			if((m_uTrackFlags & TrackFlag_Loop) && (m_uTrackFlags & TrackFlag_Circular))
			{
				//for looping circular tracks , make sure the first and last points have the same speed.
				if(m_pfPointSpeeds[0] == 0 || m_pfPointSpeeds[NumberOfSections] == 0)
				{
					//having zero speed takes priority
					m_pfPointSpeeds[0] = 0;
					m_pfPointSpeeds[NumberOfSections] = 0;
				}
				else
				{
					//take the average
					m_pfPointSpeeds[0] += m_pfPointSpeeds[NumberOfSections];
					m_pfPointSpeeds[0] /= 2.0f;
					m_pfPointSpeeds[NumberOfSections] = m_pfPointSpeeds[0];
				}
			}
		}
	}
	Asura_ResourcePopType();
}

void Asura_Cutscene_Track::GetPositionFromDistance(float distanceAlongTrack,Asura_Vector_3* output) const
{
	//deal with tracks with 1 point
	if(NumberOfPivots==1)
	{
		*output = Pivots[0];
		return;
	}

	//find the section we are on
	int iSection;
	float fLerp;
	GetLerpAndSectionFromDistance(distanceAlongTrack,fLerp,iSection);

	//and get the position from this
	GetPosition(iSection,fLerp,output);



}

void Asura_Cutscene_Track::GetPositionFromTime(float fTime,Asura_Vector_3* pxOutput,const Asura_Cutscene_Track* pxNextTrack /*=0*/) const
{
	
	if(NumberOfPivots==1)
	{
		//deal with tracks with 1 point
		*pxOutput = Pivots[0];
		
	}
	else
	{
		float fCappedTime = ConvertTimeToBeInRange(fTime);
   		//find the section we are on
		int iSection;
		float fLerp;
		GetLerpAndSectionFromTime(fCappedTime,fLerp,iSection);

		//and get the position from this

		GetPosition(iSection,fLerp,pxOutput);
	}

	//Check for interpolation between tracks
	if(m_uTrackFlags & TrackFlag_InterpolateToNextTrack)
	{
		float fEndTime = GetEndTime();
		if((fTime > fEndTime) && pxNextTrack)
		{
			float fNextStart = pxNextTrack->GetStartTime();
			if(fNextStart > fEndTime)
			{
				float fLerp = (fTime-fEndTime)/(fNextStart-fEndTime);
				if(fLerp>1.0f) fLerp = 1.0f;

				Asura_Vector_3 xDestination;
				pxNextTrack->GetPositionFromTime(fNextStart,&xDestination);

				*pxOutput += (xDestination-*pxOutput)*fLerp;
			}
		}
	}
}



void Asura_Cutscene_Track::GetDirectionFromDistance(float distanceAlongTrack,Asura_Vector_3* output) const
{
	if(NumberOfPivots==1)
	{
		Asura_Matrix_3x3 xMat;
		m_pxPointOrientations[0].ToMatrix(xMat);
		output->x = xMat.e13;
		output->y = xMat.e23;
		output->z = xMat.e33;
	}
	else
	{
        const float fDIST_SAMPLE_DELTA = 0.1f;

        Asura_Vector_3 p1, p2;

        GetPositionFromDistance( distanceAlongTrack - fDIST_SAMPLE_DELTA, &p1 );
        GetPositionFromDistance( distanceAlongTrack + fDIST_SAMPLE_DELTA, &p2 );

		*output = p2-p1;
		if(output->x || output->y || output->z)
		{
			output->Normalise();
		}
	}
}

void Asura_Cutscene_Track::GetDirectionFromTime(float fTime,Asura_Vector_3* pxOutput, const Asura_Cutscene_Track* pxNextTrack /*=0*/) const
{
	if(NumberOfPivots==1)
	{
		//GetDirectionFromDistance(0,pxOutput);
		Asura_Matrix_3x3 xMat;
		m_pxPointOrientations[0].ToMatrix(xMat);
		pxOutput->x = xMat.e13;
		pxOutput->y = xMat.e23;
		pxOutput->z = xMat.e33;
		
	}
	else
	{
	
		float fDistance = GetDistanceFromTime(fTime);
		GetDirectionFromDistance(fDistance,pxOutput);
	}

	//Check for interpolation between tracks
	if(m_uTrackFlags & TrackFlag_InterpolateToNextTrack)
	{
		float fEndTime = GetEndTime();
		if((fTime > fEndTime) && pxNextTrack)
		{
			float fNextStart = pxNextTrack->GetStartTime();
			if(fNextStart > fEndTime)
			{
				float fLerp = (fTime-fEndTime)/(fNextStart-fEndTime);
				if(fLerp>1.0f) fLerp = 1.0f;

				Asura_Vector_3 xEndDirection;
				pxNextTrack->GetDirectionFromTime(fNextStart,&xEndDirection);

				if(*pxOutput!=xEndDirection)
				{
					//interpolate between the 2 direction vectors
					Asura_Matrix_3x3 xMat1,xMat2;
					Asura_Maths::OrientationFromDirection(xMat1,*pxOutput);
					Asura_Maths::OrientationFromDirection(xMat2,xEndDirection);

					Asura_Quat xQuat1,xQuat2;
					xQuat1.FromMatrix(xMat1);
					xQuat2.FromMatrix(xMat2);

					Asura_Quat xQuat;
					xQuat.Slerp(xQuat1,xQuat2,fLerp);
					Asura_Matrix_3x3 xMat;
					xQuat.ToMatrix(xMat);

					pxOutput->x = xMat.e13;
					pxOutput->y = xMat.e23;
					pxOutput->z = xMat.e33;
				}
			}
		}
	}
}

void Asura_Cutscene_Track::GetPointOrientationFromDistance(float fDistance,Asura_Matrix_3x3* pxOutput) const
{
	Asura_Assert(pxOutput,"");
	Asura_Assert(NumberOfPivots,"");

	//deal with tracks with 1 point
	if(NumberOfPivots==1)
	{
		m_pxPointOrientations[0].ToMatrix(*pxOutput);
		return;
	}

	//find the section we are on
	int iSection;
	float fLerp;
	GetLerpAndSectionFromDistance(fDistance,fLerp,iSection);
	
	
	Asura_Quat xQuat;
	xQuat.Slerp(m_pxPointOrientations[iSection],m_pxPointOrientations[iSection+1],fLerp);

	xQuat.ToMatrix(*pxOutput);

}

void Asura_Cutscene_Track::GetPointOrientationFromTime(float fTime,Asura_Matrix_3x3* pxOutput) const
{
	Asura_Assert(pxOutput,"");
	Asura_Assert(NumberOfPivots,"");

	//deal with tracks with 1 point
	if(NumberOfPivots==1)
	{
		m_pxPointOrientations[0].ToMatrix(*pxOutput);
		return;
	}

	//find the section we are on
	int iSection;
	float fLerp;
	GetLerpAndSectionFromTime(fTime,fLerp,iSection);

	Asura_Quat xQuat;
	xQuat.Slerp(m_pxPointOrientations[iSection],m_pxPointOrientations[iSection+1],fLerp);

	xQuat.ToMatrix(*pxOutput);

}


void Asura_Cutscene_Track::GetOrientationFromDistance(float fDistanceAlongTrack,Asura_Matrix_3x3* pxOutput) const
{
	
	//Interpolate point orientation if needed
	Asura_Euler_3 xEulerFromPoints;
	if(m_uTrackFlags & (TrackFlag_OrientationFromPoints|TrackFlag_RollFromPoints) )
	{
		Asura_Matrix_3x3 xMat;
		GetPointOrientationFromDistance(fDistanceAlongTrack,&xMat);

		//Should the orientation be taken entirely from the point orientations?
		if( m_uTrackFlags & TrackFlag_OrientationFromPoints )
		{
			*pxOutput = xMat;
			return;
		}
 		xEulerFromPoints.FromMatrix(xMat);
	}

	
	//Get direction along track
	Asura_Euler_3 xEulerFromTrack;
	{
		//get the heading along the track
		Asura_Vector_3 xDirection;
		Asura_Matrix_3x3 xMat;

		GetDirectionFromDistance(fDistanceAlongTrack,&xDirection);

		if(!(m_uTrackFlags & TrackFlag_PitchAlongTrack))
		{
			xDirection.y = 0;
			if(xDirection.x==0 && xDirection.z==0) xDirection.x = 1.0f;

			xDirection.Normalise();
		}

		Asura_Maths::OrientationFromDirection(xMat,xDirection);

		
		if(!(m_uTrackFlags & TrackFlag_RollFromPoints))
		{
			*pxOutput = xMat;
			return;
		}
 		xEulerFromTrack.FromMatrix(xMat);
	}

	//If we get here , then we should be taking yaw and pitch from the track direction , and roll from the points
	//So , get the roll
	xEulerFromTrack.z = xEulerFromPoints.z;

	xEulerFromTrack.ToMatrix(*pxOutput);
}

void Asura_Cutscene_Track::GetOrientationFromTimeMain(float fTime,Asura_Matrix_3x3* pxOutput) const
{
	
	//Interpolate point orientation if needed
	Asura_Euler_3 xEulerFromPoints;
	if(m_uTrackFlags & (TrackFlag_OrientationFromPoints|TrackFlag_RollFromPoints) )
	{
		Asura_Matrix_3x3 xMat;
		GetPointOrientationFromTime(fTime,&xMat);

		//Should the orientation be taken entirely from the point orientations?
		if( m_uTrackFlags & TrackFlag_OrientationFromPoints )
		{
			*pxOutput = xMat;
			return;
		}
 		xEulerFromPoints.FromMatrix(xMat);
	}

	
	//Get direction along track
	Asura_Euler_3 xEulerFromTrack;
	{
		//Need to convert time into a distance
		float fDistance = GetDistanceFromTime(fTime);

		//get the heading along the track
		Asura_Vector_3 xDirection;
		Asura_Matrix_3x3 xMat;

		GetDirectionFromDistance(fDistance,&xDirection);

		if(!(m_uTrackFlags & TrackFlag_PitchAlongTrack))
		{
			xDirection.y = 0;
			if(xDirection.x==0 && xDirection.z==0) xDirection.x = 1.0f;

			xDirection.Normalise();
		}

		Asura_Maths::OrientationFromDirection(xMat,xDirection);

		
		if(!(m_uTrackFlags & TrackFlag_RollFromPoints))
		{
			*pxOutput = xMat;
			return;
		}
 		xEulerFromTrack.FromMatrix(xMat);
	}

	//If we get here , then we should be taking yaw and pitch from the track direction , and roll from the points
	//So , get the roll
	xEulerFromTrack.z = xEulerFromPoints.z;

	xEulerFromTrack.ToMatrix(*pxOutput);
}


void Asura_Cutscene_Track::GetOrientationFromTime(float fTime,Asura_Matrix_3x3* pxOutput, const Asura_Cutscene_Track* pxNextTrack /*=0*/) const
{

	GetOrientationFromTimeMain(fTime,pxOutput);
	

	//Check for interpolation between tracks
	if(m_uTrackFlags & TrackFlag_InterpolateToNextTrack)
	{
		float fEndTime = GetEndTime();
		if((fTime > fEndTime) && pxNextTrack)
		{
			float fNextStart = pxNextTrack->GetStartTime();
			if(fNextStart > fEndTime)
			{
				float fLerp = (fTime-fEndTime)/(fNextStart-fEndTime);
				if(fLerp>1.0f) fLerp = 1.0f;

				Asura_Matrix_3x3 xEndOrient;
				pxNextTrack->GetOrientationFromTimeMain(fNextStart,&xEndOrient);

				
				//interpolate between the 2 matrices
				Asura_Quat xQuat1,xQuat2;
				xQuat1.FromMatrix(*pxOutput);
				xQuat2.FromMatrix(xEndOrient);

				Asura_Quat xQuat;
				xQuat.Slerp(xQuat1,xQuat2,fLerp);
		
				xQuat.ToMatrix(*pxOutput);

			}
		}
	}
}

float Asura_Cutscene_Track::GetDistanceFromTime(float fTime) const
{

	//If there is only one point , distance has to be 0
	if(NumberOfPivots<=1) return 0.0f;

	//find the section we are on
	int iSection;
	float fLerp;
	GetLerpAndSectionFromTime(fTime,fLerp,iSection,TrackFlag_IncreasedSpeedAccuracy);
	
	//and get the distance from that
	float fDistance = TotalLengths[iSection];
	fDistance += (TotalLengths[iSection+1]-TotalLengths[iSection])*fLerp;

	return fDistance;
}


int Asura_Cutscene_Track::GetNumberOfPoints() const
{
	if(NumberOfPivots<=1) return NumberOfPivots;
	return NumberOfPivots-2;
}

Asura_Vector_3* Asura_Cutscene_Track::GetPoints() const
{
	if(NumberOfPivots<=1) return Pivots;
	return Pivots+1;//skip past the start control point
}

float Asura_Cutscene_Track::GetStartTime() const
{
	return PointArrivalTimes[0];
}

float Asura_Cutscene_Track::GetEndTime() const
{
	float fTime = PointArrivalTimes[GetNumberOfPoints()-1];
	if(PointDurations)
	{
		fTime += PointDurations[GetNumberOfPoints()-1];
	}
	return fTime;
}

float Asura_Cutscene_Track::GetSpeedFromTime(float fTime) const
{
	if(NumberOfPivots==1) return 0;

	fTime = ConvertTimeToBeInRange(fTime);

	//find the section that we are on
	int iSection;
	float fLerp;
	GetLerpAndSectionFromTime(fTime,fLerp,iSection,TrackFlag_IncreasedSpeedAccuracy|TrackFlag_SmoothSpeed);

	if(iSection==NumberOfSections) return 0;//at end of track

	if(PointDurations)
	{
		//currently stopped at one of the points?
		if(PointArrivalTimes[iSection]+PointDurations[iSection]>fTime)
		{
			return 0;
		}
	}
	
	if(m_uTrackFlags & TrackFlag_SmoothSpeed)
	{
		Asura_Assert(m_pfPointSpeeds,"Speeds should have been calculated");
		
		//get speeds at start and end of section
		float v0,v1;
		v0 = m_pfPointSpeeds[iSection];
		v1 = m_pfPointSpeeds[iSection+1];

		//get section length and duration
		float fDist = TotalLengths[iSection+1]-TotalLengths[iSection];
		if(fDist<=0) return 0;

		float fDuration = PointArrivalTimes[iSection+1]-PointArrivalTimes[iSection];
		if(PointDurations) fDuration-=PointDurations[iSection];
		if(fDuration<=0) return 0;

		/*
		pivots :
		p0 = fDist - 2*v0*fDuration
		p1 = 0
		p2 = fDist
		p3 = 2*v1*fDuration + p1

		//spline contants
		a = -p0 + 3*p1 - 3*p2 + p3
		  = -4*fDist + 2*v0*fDuration + 2*v1*fDuration

		b = 2*p0 - 5*p1 +4*p2 -p3
		  = 6*fDist - 4*v0*fDuration - 2*v1*fDuration

		c = -p0 + p2
		  = 2*v0*fDuration
		*/

		const float fScaledV0 = v0*fDuration;
		const float fScaledV1 = v1*fDuration;

		
		const float a = -4.0f*fDist + (fScaledV0+fScaledV1)*2;
		const float b = 6*fDist - 4*fScaledV0 - 2*fScaledV1;
		const float c = 2*fScaledV0;
		

		/*
		d : distance
		t : time
		T : duration
		d = (a*fLerp^3 + b*fLerp^2 +c*fLerp) / 2 + p1
		  = (a*t^3/T^3 + b*t^2/T^2 +c*t/T) / 2 + p1

		v = (3*a*t^2/T^3 + 2*b*t/T^2 +c/T) / 2
		  = (3*a*fLerp^2 + 2*b*fLerp + c) / (2*T)
		*/

		return (3*a*fLerp*fLerp+2*b*fLerp+c) / (2*fDuration);
	}
	else
	{
		//Get average speed for section
		float fSectionLength = TotalLengths[iSection+1]-TotalLengths[iSection];
		float fSectionTime = PointArrivalTimes[iSection+1]-PointArrivalTimes[iSection];
		if(PointDurations) fSectionTime -= PointDurations[iSection];

		if(!fSectionTime) return 0;
		return fSectionLength/fSectionTime;
	}
}

float Asura_Cutscene_Track::GetSpeedFromDistance(float fDistanceAlongTrack) const
{
	if(NumberOfPivots==1) return 0.0f;

	//find the section that we are on
	int iSection;
	float fLerp;
	GetLerpAndSectionFromDistance(fDistanceAlongTrack,fLerp,iSection);

	if(iSection==NumberOfSections) return 0;//at end of track

//	if(PointDurations)
//	{
//		//currently stopped at one of the points?
//		if(PointArrivalTimes[iSection]+PointDurations[iSection]>fTime)
//		{
//			return 0;
//		}
//	}

	if(m_uTrackFlags & TrackFlag_SmoothSpeed)
	{
		Asura_Assert(m_pfPointSpeeds,"Speeds should have been calculated");

		//get speeds at start and end of section
		float v0,v1;
		v0 = m_pfPointSpeeds[iSection];
		v1 = m_pfPointSpeeds[iSection+1];

		//get section length and duration
		float fDist = TotalLengths[iSection+1]-TotalLengths[iSection];
		if(fDist<=0) return 0;

		float fDuration = PointArrivalTimes[iSection+1]-PointArrivalTimes[iSection];
		if(PointDurations) fDuration-=PointDurations[iSection];
		if(fDuration<=0) return 0;

		/*
		pivots :
		p0 = fDist - 2*v0*fDuration
		p1 = 0
		p2 = fDist
		p3 = 2*v1*fDuration + p1

		//spline contants
		a = -p0 + 3*p1 - 3*p2 + p3
		= -4*fDist + 2*v0*fDuration + 2*v1*fDuration

		b = 2*p0 - 5*p1 +4*p2 -p3
		= 6*fDist - 4*v0*fDuration - 2*v1*fDuration

		c = -p0 + p2
		= 2*v0*fDuration
		*/

		const float fScaledV0 = v0*fDuration;
		const float fScaledV1 = v1*fDuration;


		const float a = -4.0f*fDist + (fScaledV0+fScaledV1)*2;
		const float b = 6*fDist - 4*fScaledV0 - 2*fScaledV1;
		const float c = 2*fScaledV0;


		/*
		d : distance
		t : time
		T : duration
		d = (a*fLerp^3 + b*fLerp^2 +c*fLerp) / 2 + p1
		= (a*t^3/T^3 + b*t^2/T^2 +c*t/T) / 2 + p1

		v = (3*a*t^2/T^3 + 2*b*t/T^2 +c/T) / 2
		= (3*a*fLerp^2 + 2*b*fLerp + c) / (2*T)
		*/

		return (3*a*fLerp*fLerp+2*b*fLerp+c) / (2*fDuration);
	}
	else
	{
		//Get average speed for section
		float fSectionLength = TotalLengths[iSection+1]-TotalLengths[iSection];
		float fSectionTime = PointArrivalTimes[iSection+1]-PointArrivalTimes[iSection];
		if(PointDurations) fSectionTime -= PointDurations[iSection];

		if(!fSectionTime) return 0;
		return fSectionLength/fSectionTime;
	}
}




void Asura_Cutscene_Track::GetOrientationAtPoint(int iIndex,Asura_Matrix_3x3& xMat) const
{
	Asura_Assert(iIndex>=0 && iIndex<GetNumberOfPoints(),"Track point index out of range");
	m_pxPointOrientations[iIndex].ToMatrix(xMat);
}

void Asura_Cutscene_Track::SetOrientationAtPoint(int iIndex,const Asura_Matrix_3x3& xMat)
{
	Asura_Assert(iIndex>=0 && iIndex<GetNumberOfPoints(),"Track point index out of range");
	m_pxPointOrientations[iIndex].FromMatrix(xMat);
}

float Asura_Cutscene_Track::ConvertTimeToBeInRange(float fTime) const
{
	float fStartTime = GetStartTime();
	float fEndTime = GetEndTime();

	if(fTime < fStartTime) fTime = fStartTime;

	if(fTime > fEndTime)
	{
		float fDuration = fEndTime - fStartTime;
		if((m_uTrackFlags & TrackFlag_Loop) && fDuration>0.0f)
		{
			//get the number of times looped
			float fLoops = (fTime - fStartTime) / fDuration;
			//cut off the integer part
			fLoops -= (float)Asura_Maths::FloatToInt_Chop(fLoops);
			
			fTime = fStartTime + fDuration*fLoops;

			//make sure inaccuracies don't put the time out of range
			if(fTime < fStartTime) fTime = fStartTime;
			if(fTime > fEndTime) fTime = fEndTime;
						

		}
		else
		{
			//cap time to be in range
			fTime = fEndTime;
		}
	}
	return fTime;
}

void Asura_Cutscene_Track::GetLerpAndSectionFromDistance(float fDistance,float& fLerp,int& iSection) const
{
	Asura_Assert(NumberOfPivots>=4,"Shouldn't be using this function on single point tracks");


	//make sure the distance is in range
	if(fDistance<TotalLengths[0]) fDistance = TotalLengths[0];
	else if(fDistance>TotalLengths[NumberOfSections]) fDistance = TotalLengths[NumberOfSections];
	
	//find the section that we are on
	for(iSection=0;iSection<NumberOfSections;iSection++)
	{
		if(TotalLengths[iSection+1]>=fDistance) break;
	}

	//get the proportion along the section
	float u;
	float fSectionStart = TotalLengths[iSection];
	float fSectionLength = TotalLengths[iSection+1]-fSectionStart;
	if(fSectionLength>0 && fDistance>fSectionStart)
	{
		u = (fDistance-fSectionStart)/fSectionLength;
	}
	else
	{
		u = 0;
	}

	//apply affects of more accurate tracks if needed
	if((m_uTrackFlags & TrackFlag_IncreasedSpeedAccuracy) && m_pfSubTotalLengths)
	{
		if(!(PointFlags[iSection] & TrackPointFlag_SectionStraight))
		{
			//Calculate a new u value so that the distance is more evenly spread between 0 and 1
			float fNewDistance = TotalLengths[iSection] + (TotalLengths[iSection+1]-TotalLengths[iSection])*u;

			int iSubSection = 0;
			int iIndex = iSection * g_iNumIntermediateTrackPoints;
			for(;iSubSection<g_iNumIntermediateTrackPoints;iSubSection++,iIndex++)
			{
				if(m_pfSubTotalLengths[iIndex+1]>=fNewDistance) break;
			}
		
			u = (float)iSubSection/(float)g_iNumIntermediateTrackPoints;

			float fSubSectionLength = m_pfSubTotalLengths[iIndex+1] - m_pfSubTotalLengths[iIndex];
			float fDistanceInSubSection =  fNewDistance - m_pfSubTotalLengths[iIndex];

			if(fSubSectionLength>0 && fDistanceInSubSection>0)
			{
				u += (fDistanceInSubSection/fSubSectionLength) / (float) g_iNumIntermediateTrackPoints;
				if(u>1.0f) u = 1.0f;
			}
		}
	}
	fLerp = u;
}

void Asura_Cutscene_Track::GetLerpAndSectionFromTime(float fTime,float& fLerp,int& iSection,u_int uTrackFlagsToIgnore) const
{
	Asura_Assert(NumberOfPivots>=4,"Shouldn't be using this function on single point tracks");

	fTime = ConvertTimeToBeInRange(fTime);

	//make sure the time is in range
	if(fTime<PointArrivalTimes[0]) fTime = PointArrivalTimes[0];
	else if(fTime>PointArrivalTimes[NumberOfSections]) fTime = PointArrivalTimes[NumberOfSections];
	
	//find the section that we are on
	for(iSection=0;iSection<NumberOfSections;iSection++)
	{
		if(PointArrivalTimes[iSection+1]>=fTime) break;
	}

	//get the proportion along the section
	float u;
	float fSectionStart = PointArrivalTimes[iSection] + (PointDurations ? PointDurations[iSection] : 0);
	float fSectionLength = PointArrivalTimes[iSection+1]-fSectionStart;
	if(fSectionLength>0 && fTime>fSectionStart)
	{
		u = (fTime-fSectionStart)/fSectionLength;
	}
	else
	{
		u = 0;
	}

	u_int uTrackFlags = m_uTrackFlags &~ uTrackFlagsToIgnore;

	//apply speed smoothing
	if(fSectionLength > 0 && (uTrackFlags & TrackFlag_SmoothSpeed))
	{
		Asura_Assert(m_pfPointSpeeds,"Speeds should have been calculated");
		
		//get speeds at start and end of section
		float v0,v1;
		v0 = m_pfPointSpeeds[iSection];
		v1 = m_pfPointSpeeds[iSection+1];

		//get distance
		float fDist = TotalLengths[iSection+1]-TotalLengths[iSection];
		if(fDist > 0)
		{
			//pick pivots so that the speeds are correct at the start and end of the section

			/*
			pivots :
			p0 = fDist - 2*v0*fDuration
			p1 = 0
			p2 = fDist
			p3 = 2*v1*fDuration + p1

			//spline contants
			a = -p0 + 3*p1 - 3*p2 + p3
			  = -4*fDist + 2*v0*fDuration + 2*v1*fDuration

			b = 2*p0 - 5*p1 +4*p2 -p3
			  = 6*fDist - 4*v0*fDuration - 2*v1*fDuration

			c = -p0 + p2
			  = 2*v0*fDuration
			*/

			const float fScaledV0 = v0*fSectionLength;
			const float fScaledV1 = v1*fSectionLength;

			
			const float a = -4.0f*fDist + (fScaledV0+fScaledV1)*2;
			const float b = 6*fDist - 4*fScaledV0 - 2*fScaledV1;
			const float c = 2*fScaledV0;

			//do the spline thing to smooth the speed
			const float u2 = (u*u);
			const float u3 = (u2*u);

			u = ((a*u3)+(b*u2)+(c*u))/2.0f;
			u/=fDist;
			if(u>1) u=1;
			if(u<0) u=0;

		}
	}

	//apply affects of more accurate tracks if needed
	if((uTrackFlags & TrackFlag_IncreasedSpeedAccuracy) && m_pfSubTotalLengths)
	{
		if(!(PointFlags[iSection] & TrackPointFlag_SectionStraight))
		{
			//Calculate a new u value so that the distance is more evenly spread between 0 and 1
			float fDistance = TotalLengths[iSection] + (TotalLengths[iSection+1]-TotalLengths[iSection])*u;

			int iSubSection = 0;
			int iIndex = iSection * g_iNumIntermediateTrackPoints;
			for(;iSubSection<g_iNumIntermediateTrackPoints;iSubSection++,iIndex++)
			{
				if(m_pfSubTotalLengths[iIndex+1]>=fDistance) break;
			}
		
			u = (float)iSubSection/(float)g_iNumIntermediateTrackPoints;

			float fSubSectionLength = m_pfSubTotalLengths[iIndex+1] - m_pfSubTotalLengths[iIndex];
			float fDistanceInSubSection =  fDistance - m_pfSubTotalLengths[iIndex];

			if(fSubSectionLength>0 && fDistanceInSubSection>0)
			{
				u += (fDistanceInSubSection/fSubSectionLength) / (float) g_iNumIntermediateTrackPoints;
				if(u>1.0f) u = 1.0f;
			}
		}
	}
	fLerp = u;
}

void Asura_Cutscene_Track::GetPosition(int iSection,float fLerp,Asura_Vector_3* pxOutput) const
{
	//straight or curvy track?
	if(PointFlags[iSection] & TrackPointFlag_SectionStraight)
	{
		*pxOutput = Pivots[iSection+1]*(1-fLerp) + Pivots[iSection+2]*fLerp;
	}
	else
	{
		//Track is curved , but if it is straight before or afterwards  ,then we will need to fiddle with the pivots
		//to make the transition smooth
		bool bStraightBefore = (iSection>0) ? (PointFlags[iSection-1] & TrackPointFlag_SectionStraight)!=0 : false;
		bool bStraightAfter = (iSection<(NumberOfSections-1)) ? (PointFlags[iSection+1] & TrackPointFlag_SectionStraight)!=0 : false;

		if(bStraightBefore || bStraightAfter)
		{
			Asura_Vector_3 axPivots[4] ;
			
			//middle pivots are unaltered regardless
			axPivots[1] = Pivots[iSection+1];
			axPivots[2] = Pivots[iSection+2];

			if(bStraightBefore)
			{
				axPivots[0] = Pivots[iSection+2] + Pivots[iSection] - Pivots[iSection+1];
			}
			else
			{
				axPivots[0] = Pivots[iSection];
			}

			if(bStraightAfter)
			{
				axPivots[3] = Pivots[iSection+1] + Pivots[iSection+3] - Pivots[iSection+2];
			}
			else
			{
				axPivots[3] = Pivots[iSection+3];
			}

			SmoothTrackPosition(axPivots,fLerp,pxOutput);

		}
		else
		{
			//can use the pivots as is
			SmoothTrackPosition(&Pivots[iSection],fLerp,pxOutput);
		}
	}
	
}

///////////////////////////////////////////////////////////////////////////////////////////////////

float LengthOfTrackSection(Asura_Vector_3* axPivots, int iSectionFlags /*= 0*/)
{
	if (iSectionFlags & TrackPointFlag_SectionStraight)
	{
		return (axPivots[2] - axPivots[1]).Magnitude();
	}
	else
	{
		// to approximate distance along curve, divide into 20 straight 
		// segments and add up the lengths of the segments.
		Asura_Vector_3 axSegmentPoints[21];
		int i;

		for (i = 0; i <= 20; i++)
		{
			float f = static_cast<float>(i) / 20.0f;
			SmoothTrackPosition(axPivots, f, &axSegmentPoints[i]);
		}

		float fLength = 0;
		for (i = 0; i < 20; i++)
		{
			fLength += (axSegmentPoints[i] - axSegmentPoints[i + 1]).Magnitude();
		}
		return fLength;
	}
}

// (MH - 24.09.2003) see Steve Rabin's article in Game Programming Gems p.266 
// "A* Aesthetic Optimisations" for an alternative (but equivalent) formulation 
// of this spline (Catmull-Rom Spline).
// For u in [0,1] a position is output between the middle 2 pivots.
void SmoothTrackPosition(const Asura_Vector_3* pivots,float u,Asura_Vector_3* output)
{
	Asura_Assert(u >= 0.0f && u <= 1.0f, "cubic spline parameter out of range");

	const Asura_Vector_3 &pivot_0 = *pivots++;
	const Asura_Vector_3 &pivot_1 = *pivots++;
	const Asura_Vector_3 &pivot_2 = *pivots++;
	const Asura_Vector_3 &pivot_3 = *pivots;
	float u2 = (u*u);
	float u3 = (u2*u);

 	{
		float a = (-pivot_0.x+3.0f*pivot_1.x-3.0f*pivot_2.x+pivot_3.x);
		float b = (2.0f*pivot_0.x-5.0f*pivot_1.x+4.0f*pivot_2.x-pivot_3.x);
		float c = (-1.0f*pivot_0.x+pivot_2.x);
		output->x = ((a*u3)+(b*u2)+(c*u))/2.0f+pivot_1.x;
	}
 	{
		float a = (-pivot_0.y+3.0f*pivot_1.y-3.0f*pivot_2.y+pivot_3.y);
		float b = (2.0f*pivot_0.y-5.0f*pivot_1.y+4.0f*pivot_2.y-pivot_3.y);
		float c = (-1.0f*pivot_0.y+pivot_2.y);
		output->y = ((a*u3)+(b*u2)+(c*u))/2.0f+pivot_1.y;
	}
 	{
		float a = (-pivot_0.z+3*pivot_1.z-3.0f*pivot_2.z+pivot_3.z);
		float b = (2.0f*pivot_0.z-5*pivot_1.z+4.0f*pivot_2.z-pivot_3.z);
		float c = (-1.0f*pivot_0.z+pivot_2.z);
		output->z = ((a*u3)+(b*u2)+(c*u))/2.0f+pivot_1.z;
	}
}


