/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include <cmath>
		#include "geometry/curve.h"
		#include "sort/sort.h"

		using namespace GS;


//------------------------------------------------------------------------------
void				Curve::Update(const CurvePoint &p, const Time &t_epsilon)
{
	for (uint n = 0; n < points.GetCount(); ++n)
	{
		CurvePoint *point = points[n];
		if	((p.t >= (point->t - t_epsilon)) && (p.t <= (point->t + t_epsilon)))
		{
			point->v = p.v;
			return;
		}
	}
	Insert(p);
}
void				Curve::Insert(const CurvePoint &k)
{
	int idx = GetPointIndex(k.t);
	points.Insert(new CurvePoint(k), (idx == -1) ? points.GetCount() : idx);
}
void				Curve::Append(const CurvePoint &k)
{
	points.Insert(new CurvePoint(k), points.GetCount());
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Curve::Delete(CurvePoint *k)
{
	points.Remove(k);
	delete k;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
int					Curve::GetPointIndex(const Time &t, bool t_greater) const
{
	if	(points.GetCount() == 0)
		return -1;

	if	(t_greater)
	{
		for (uint n = 0; n < points.GetCount(); ++n)
			if	(points[n]->t > t)
				return n;
	}
	else
	{
		for (int n = points.GetCount() - 1; n >= 0; --n)
			if	(points[n]->t <= t)
				return n;
	}
	return -1;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
TimeRange			Curve::GetTimeRange() const
{
	return points.GetCount() == 0 ? TimeRange() : TimeRange(points[0]->t, points[points.GetCount() - 1]->t);
}
Range <float>		Curve::GetValueRange() const
{
	if	(points.GetCount() == 0)
		return Range <float> ();

	Range <float> range(points[0]->v, points[0]->v);
	for (uint n = 1; n < points.GetCount(); ++n)
	{
		range.start = Types::Min(range.start, points[n]->v);
		range.end = Types::Max(range.end, points[n]->v);
	}
	return range;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
uint				Curve::Optimize(uint point_count, const CurvePoint *skf, CurvePoint *dkf, float threshold)
{
	if	(point_count < 3)
		return 0;

	uint ckf = 0, n;
	for	(n = 1; n < (point_count - 1); n += 2)
	{
		float k = (skf[n].t - skf[n - 1].t).toSec() / (skf[n + 1].t - skf[n - 1].t).toSec();
		float iv = (skf[n - 1].v * k) + (skf[n + 1].v * (1.f - k));

		dkf[ckf++] = skf[n - 1];
		if	(fabs(skf[n].v - iv) > threshold)
			dkf[ckf++] = skf[n];
	}
	if	(n == (point_count - 1))
		dkf[ckf++] = skf[point_count - 2];
	dkf[ckf++] = skf[point_count - 1];
	return point_count - ckf;
}
uint				Curve::Optimize(float threshold)
{
	if	(!points.GetCount())
		return 0;

	Array <CurvePoint> skf(points.GetCount(), Alloc::Curve), dkf(points.GetCount(), Alloc::Curve);
	if	(skf.IsNull() || dkf.IsNull())
		__ERR__(__LOG__ << "Not enough memory.\n", 0);

	// Freeze array.
	for (uint n = 0; n < points.GetCount(); ++n)
		skf[n] = *points[n];

	// Optimize curve.
	uint gain = Optimize(points.GetCount(), skf.c_ptr(), dkf.c_ptr(), threshold), out = points.GetCount() - gain;

	if	(gain)
	{
		// Send back to curve.
		if	(!AllocatePoint(out))
			__ERR__(__LOG__ << "Failed to reallocate optimized array.\n", 0);
		for (uint n = 0; n < points.GetCount(); ++n)
			SetPoint(n, dkf[n]);
	}
	return gain;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool				Curve::AllocatePoint(uint n)
{
	ArrayListDeleteAllPtr(CurvePoint *, points)
	while (n--)
		if	(!points.Add(new CurvePoint))
			return false;

	return true;
}
void				Curve::SetPoint(uint i, const CurvePoint &p) const
{	*points[i] = p;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Curve::Sort()
{
	// Make sure there is work to do.
	bool need_sorting = false;
	for (uint n = 1; n < points.GetCount(); ++n)
		if	(points[n - 1]->t > points[n]->t)
		{
			need_sorting = true;
			break;
		}
	if	(!need_sorting)
		return;

	// Sort keys.
	uint count = points.GetCount();

	Array <GS::Sort<Time, CurvePoint *>::Entry> entries(count);
	for (uint n = 0; n < count; ++n)
	{
		entries[n].v = points[n]->t;
		entries[n].o = points[n];
	}
	GS::Sort<Time, CurvePoint *>::QuickSort(count, entries);

	// Drop current array and rewrite ordered one.
	points.Clear(false);
	for (uint n = 0; n < count; ++n)
		points.Add(entries[n].o);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
static float		range(float v, float lo, float hi, int *i)
{
	float r = hi - lo;

	if	(r == 0.f)
	{
		if	(i)
			*i = 0;
		return lo;
	}

	float v2 = v - lo;
	if	(v2 >= 0.f)
		v2 = lo + v2 - r * floor(v2 / r);
	else
		v2 = hi + v2 - r * ceil(v2 / r);

	if	(i)
		*i = -(int)((v2 - v) / r + (v2 > v ? 0.5f : -0.5f));

	return Types::Clamp(v2, lo, hi);
}
static void			hermite(float t, float *h1, float *h2, float *h3, float *h4)
{
	float t2 = t * t, t3 = t * t2;

	*h2 = 3.f * t2 - t3 - t3;
	*h1 = 1.f - *h2;
	*h4 = t3 - t2;
	*h3 = *h4 - t2 + t;
}
static float		bezier(float x0, float x1, float x2, float x3, float t)
{
	float a, b, c, t2 = t * t, t3 = t * t2;

	c = 3.f * (x1 - x0);
	b = 3.f * (x2 - x1) - c;
	a = x3 - x0 - c - b;

	return a * t3 + b * t2 + c * t + x0;
}
static float		bez2_time(float x0, float x1, float x2, float x3, float time, float *t0, float *t1)
{
	float t = *t0 + (*t1 - *t0) * 0.5f, v = bezier(x0, x1, x2, x3, t);

	if	((fabs(*t1 - *t0) > .0001f) && (fabs(time - v) > .0001f))
	{
		if	(v > time)
			*t1 = t;
		else
			*t0 = t;

		return bez2_time(x0, x1, x2, x3, time, t0, t1);
	}
	return t;
}
static float		bez2(const CurvePoint *key0, const CurvePoint *key1, float time)
{
	float x, y, t, t0 = 0.f, t1 = 1.f;

	if	(key0->shape == CurvePoint::Shape_Bezier2)
		x = key0->t.toSec() + key0->param[2];
	else
		x = key0->t.toSec() + (key1->t - key0->t).toSec() / 3.f;

	t = bez2_time(key0->t.toSec(), x, key1->t.toSec() + key1->param[0], key1->t.toSec(), time, &t0, &t1);

	if	(key0->shape == CurvePoint::Shape_Bezier2)
		y = key0->v + key0->param[3];
	else
		y = key0->v + key0->param[1] / 3.f;

	return bezier(key0->v, y, key1->param[1] + key1->v, key1->v, t);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
float				Curve::Outgoing(const CurvePoint *key0, const CurvePoint *key1, const CurvePoint *keyp) const
{
	float a, b, d, t, out;

	switch (key1->shape)
	{
		case CurvePoint::Shape_Linear:
			d = key1->v - key0->v;
			if	(keyp)
			{
				t = (key1->t - key0->t).toSec() / (key1->t - keyp->t).toSec();
				out = t * ((key0->v - keyp->v) + d);
			}
			else
				out = d;
			break;

		case CurvePoint::Shape_TCB:
			a = (1 - key0->tension)
			  * (1 + key0->continuity)
			  * (1 + key0->bias);
			b =	(1 - key0->tension)
			  * (1 - key0->continuity)
			  * (1 - key0->bias);
			d = key1->v - key0->v;

			if	(keyp)
			{
				t = (key1->t - key0->t).toSec() / (key1->t - keyp->t).toSec();
				out = t * (a * (key0->v - keyp->v) + b * d);
			}
			else
				out = b * d;
			break;

		case CurvePoint::Shape_Bezier:
		case CurvePoint::Shape_Hermite:
			out = key0->param[0];
			if	(keyp)
				out *= (key1->t - key0->t).toSec() / (key1->t - keyp->t).toSec();
			break;

		case CurvePoint::Shape_Bezier2:
			out = key0->param[3] * (key1->t - key0->t).toSec();
			if	(fabs(key0->param[2]) > 1e-5f)
				out /= key0->param[2];
			else
				out *= 1e5f;
			break;

		case CurvePoint::Shape_Step:
		default:
			out = 0;
			break;
	}
	return out;
}
float				Curve::Incoming(const CurvePoint *key0, const CurvePoint *key1, const CurvePoint *key2) const
{
	float a, b, d, t, in;

	switch (key1->shape)
	{
		case CurvePoint::Shape_Linear:
			d = key1->v - key0->v;
			if	(key2)
			{
				t = (key1->t - key0->t).toSec() / (key2->t - key0->t).toSec();
				in = t * ((key2->v - key1->v) + d);
			}
			else
				in = d;
			break;

		case CurvePoint::Shape_TCB:
			a = (1 - key1->tension)
			  * (1 - key1->continuity)
			  * (1 + key1->bias);
			b =	(1 - key1->tension)
			  * (1 + key1->continuity)
			  * (1 - key1->bias);
			d = key1->v - key0->v;

			if	(key2)
			{
				t = (key1->t - key0->t).toSec() / (key2->t - key0->t).toSec();
				in = t * (b * (key2->v - key1->v) + a * d);
			}
			else
				in = a * d;
			break;

		case CurvePoint::Shape_Bezier:
		case CurvePoint::Shape_Hermite:
			in = key1->param[0];
			if	(key2)
				in *= (key1->t - key0->t).toSec() / (key2->t - key0->t).toSec();
			break;

		case CurvePoint::Shape_Bezier2:
			in = key1->param[1] * (key1->t - key0->t).toSec();
			if	(fabs(key1->param[0]) > 1e-5f)
					in /= key1->param[0];
			else	in *= 1e5f;
			break;

		case CurvePoint::Shape_Step:
		default:
			in = 0;
			break;
	}
	return in;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Curve::Evaluate(Time t, float *p, LoopMode loop, Time loop_start, Time loop_end) const
{
	int point_count = points.GetCount();

	if	(point_count == 0)
	{
		*p = 0;
		return;
	}
	if	(point_count == 1)
	{
		*p = points[0]->v;
		return;
	}

	// Loop mode.
	CurvePoint *skey = points[0], *ekey = points[point_count - 1];

	loop_start = (loop_start == Time::Inf) ? skey->t : Types::Clamp(loop_start, skey->t, ekey->t);
	loop_end = (loop_end == Time::Inf) ? ekey->t : Types::Clamp(loop_end, skey->t, ekey->t);

	int noff = 0;
	float offset = 0;
	if	(t < loop_start)
	{
		switch (loop)
		{
			case Reset:
				*p = 0.f;
				return;

			default:
			case Constant:
				Evaluate(loop_start, p, loop, loop_start, loop_end);
				return;
			case Repeat:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), NULL));
				break;
			case Oscillate:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), &noff));
				if	(noff % 2)
					t = loop_end + loop_start - t;
				break;
			case OffsetAndRepeat:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), &noff));
				offset = noff * (ekey->v - skey->v);		// Broken on custom loop point.
				break;
		}
	}
    else if (t > loop_end)
	{
		switch (loop)
		{
			case Reset:
				*p = 0.f;
				return;

			default:
			case Constant:
				Evaluate(loop_end, p, loop, loop_start, loop_end);
				return;
			case Repeat:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), NULL));
				break;
			case Oscillate:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), &noff));
				if	(noff % 2)
					t = loop_end + loop_start - t;
				break;
			case OffsetAndRepeat:
				t.setSec(range(t.toSec(), loop_start.toSec(), loop_end.toSec(), &noff));
				offset = noff * (ekey->v - skey->v);
				break;
		}
	}

	// Seek to current key.
	int ikey0;
#if	1
	{
		uint lo = 0, hi = points.GetCount() - 1;

		forever
		{
			uint mid = (lo + hi) / 2;

			if	(points[mid]->t > t)
				hi = mid;
			else
			{
				if	(lo == mid)
				{
					ikey0 = lo;
					break;
				}
				else
					lo = mid;
			}
		}
	}
#else
	ikey0 = 0;
	while (((ikey0 + 1) < point_count) && (t > points[ikey0 + 1]->t))
		ikey0++;
#endif

	CurvePoint *pkey0 = points[ikey0];

	if	(pkey0 == NULL)
		return;

	// Sample curve.
	CurvePoint *pkeyp = ikey0 > 0 ? points[ikey0 - 1] : NULL;

	int ikey1 = ikey0 + 1;
	CurvePoint *pkey1 = points[ikey1], *pkey2 = ikey1 < (point_count - 1) ? points[ikey1 + 1] : NULL;

	if	(t == pkey0->t)
		*p = pkey0->v + offset;

	else if (t == pkey1->t)
		*p = pkey1->v + offset;

	else
	{
		const float	k_t = (t - pkey0->t).toSec() / (pkey1->t - pkey0->t).toSec();

		switch (pkey0->shape)
		{
			case CurvePoint::Shape_TCB:
			case CurvePoint::Shape_Bezier:
			case CurvePoint::Shape_Hermite:
			{
				float out = Outgoing(pkey0, pkey1, pkeyp), in = Incoming(pkey0, pkey1, pkey2);

				float h1, h2, h3, h4;
				hermite(k_t, &h1, &h2, &h3, &h4);
				*p = h1 * pkey0->v + h2 * pkey1->v + h3 * out + h4 * in + offset;
			}
			break;

			case CurvePoint::Shape_Bezier2:
				*p = bez2(pkey0, pkey1, k_t) + offset;
				break;

			case CurvePoint::Shape_Linear:
				*p = pkey0->v + k_t * (pkey1->v - pkey0->v) + offset;
				break;

			case CurvePoint::Shape_Step:
				*p = pkey0->v + offset;
				break;

			default:
				*p = offset;
				break;
		}
	}
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Curve::Clear()
{
	ArrayListDeleteAllPtr(CurvePoint *, points)
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Curve::~Curve()
{	Clear();	}
//------------------------------------------------------------------------------