/************ (C) Copyright 2003 Valve, L.L.C. All rights reserved. ***********
**
** The copyright to the contents herein is the property of Valve, L.L.C.
** The contents may be used and/or copied only with the written permission of
** Valve, L.L.C., or in accordance with the terms and conditions stipulated in
** the agreement/contract under which the contents have been supplied.
**
*******************************************************************************
**
** Contents:
**
**	      interpolation.cpp: implementation of the interpolation class
**
******************************************************************************/

#include "hud.h"
#include "cl_util.h"
#include "interpolation.h"

// = determinant of matrix a,b,c
#define Determinant(a,b,c)		( (a)[2] * ( (b)[0]*(c)[1] - (b)[1]*(c)[0] ) + \
								  (a)[1] * ( (b)[2]*(c)[0] - (b)[0]*(c)[2] ) + \
								  (a)[0] * ( (b)[1]*(c)[2] - (b)[2]*(c)[1] ) )

// slove 3 vector linear system of equations v0 = x*v1 + y*v2 + z*v3 (if possible)
bool SolveLSE (vec3_t v0, vec3_t v1, vec3_t v2, vec3_t v3, float * x, float * y, float * z)
{
	float d = Determinant(v1,v2,v3);

	if (d==0.0f)
		return false;

	if ( x )
		*x = Determinant(v0,v2,v3) / d;

	if ( y )
		*y= Determinant(v1,v0,v3) / d;

    if ( z )
		*z= Determinant(v1,v2,v0) / d;
      
	return true;
}

// p = closest point between vector lines a1+x*m1 and a2+x*m2
bool GetPointBetweenLines(vec3_t &p, vec3_t a1, vec3_t m1, vec3_t a2, vec3_t m2 )
{
	float x,z;
	
	vec3_t t1 = CrossProduct(m1, m2);
	vec3_t t2 = a2 - a1;

	if ( !SolveLSE( t2, m1, t1, m2, &x , NULL, &z ) )
		return false;

	t1 = a1 + x*m1;
	t2 = a2 + (-z)*m2;

	p = ( t1 + t2 ) / 2.0f;

	return true;
}

// Bernstein Poynom B(u) with n = 2, i = 0
#define BernsteinPolynom20(u)	((1.0f-u)*(1.0f-u))
#define BernsteinPolynom21(u)	(2.0f*u*(1.0f-u))
#define BernsteinPolynom22(u)	(u*u)

CInterpolation::CInterpolation()
{
}

CInterpolation::~CInterpolation()
{
	m_SmoothStart = m_SmoothEnd = false;
}

void CInterpolation::SetViewAngles( vec3_t start, vec3_t end )
{
	m_StartAngle = start;
	m_EndAngle  = end;
	NormalizeAngles( m_StartAngle );
	NormalizeAngles( m_EndAngle );
}

void CInterpolation::SetFOVs(float start, float end)
{
	m_StartFov = start;
	m_EndFov = end;
}

void CInterpolation::SetWaypoints( vec3_t * prev, vec3_t start, vec3_t end, vec3_t * next)
{
	m_StartPoint = start;
	m_EndPoint = end;
	

	vec3_t a,b,c,d;

	if ( !prev && !next )
	{
		// no direction given, straight linear interpolation
		m_Center = (m_StartPoint + m_EndPoint) / 2.0f;
	}
	else if ( !prev )
	{
		a = start - end; 
		float dist = a.Length() / 2.0f; 
		a = a.Normalize();
		b = *next - end;
		b = b.Normalize();
		c = a - b;
		c = c.Normalize();
		m_Center = end + c*dist;

	}
	else if ( !next )
	{
		a = *prev - start;
		a = a.Normalize(); 
		b = end - start; 
		float dist = b.Length() / 2.0f;
		b = b.Normalize();
		c = b - a;
		c = c.Normalize();
		m_Center = start + c*dist;
	}
	else
	{
		// we have a previous and a next point, great!
		a = *prev - start;
		a = a.Normalize();
		b = end - start;
		b = b.Normalize();
		c = b - a;

		a = start - end;
		a = a.Normalize();
		b = *next - end;
		b = b.Normalize();
		d = a - b;

		GetPointBetweenLines( m_Center, start, c, end, d);
	}
}

void CInterpolation::Interpolate( float t, vec3_t &point, vec3_t &angle, float * fov)
{
	
	if ( m_SmoothStart && m_SmoothEnd )
	{
		t = (1.0f-t)*(t*t)+t*(1.0f-((t-1.0f)*(t-1.0f)));
	}
	else if ( m_SmoothStart )
	{
		t = t*t;
	}
	else if ( m_SmoothEnd )
	{
		t = t - 1.0f;
		t = -(t*t)+1;
	}
	
	if ( point )
	{
		BezierInterpolatePoint(t, point);
	}

	if ( angle )
	{
		InterpolateAngle(t, angle);
	}

	if ( fov )
	{
		*fov = m_StartFov + (t * (m_EndFov-m_StartFov));
	}
}

void CInterpolation::BezierInterpolatePoint( float t, vec3_t &point )
{
	point = m_StartPoint * BernsteinPolynom20(t);
	point = point + m_Center * BernsteinPolynom21(t);
	point = point + m_EndPoint * BernsteinPolynom22(t);
}

void CInterpolation::SetSmoothing(bool start, bool end)
{
	m_SmoothStart = start;
	m_SmoothEnd = end;
	
}

void CInterpolation::InterpolateAngle( float t, vec3_t &angle )
{
	int i;
	float ang1, ang2;
	float d;
	
	for ( i = 0 ; i < 3 ; i++ )
	{
		ang1 = m_StartAngle[i];
		ang2 = m_EndAngle[i];

		d = ang2 - ang1;
		if ( d > 180 )
		{
			d -= 360;
		}
		else if ( d < -180 )
		{	
			d += 360;
		}

		angle[i] = ang1 + d * t;
	}

	NormalizeAngles( angle );
}