#pragma once

#ifndef __AIUTILSHPP__
#define __AIUTILSHPP__

#include <stuff\stuff.hpp>
#include <adept\entity.hpp>

#pragma warning (disable:4284)
#pragma warning (push)
#include <stlport\vector>
#include <stlport\stack>
#include <stlport\string>
#pragma warning (pop)

namespace MW4AI
{
	extern HGOSHEAP g_AIHeap,g_MoverAIHeap,g_CombatAIHeap,g_RailHeap;
	struct AutoHeap
	{
			AutoHeap (HGOSHEAP p1)
			{
				gos_PushCurrentHeap (p1);
			}
			~AutoHeap (void)
			{
				gos_PopCurrentHeap ();
			}
	};

#if !defined(NO_TIMERS)
	extern std::stack<__int64 *> *g_TimerStack;
	struct my_AutoTimer
	{
		__int64 starttime;

		my_AutoTimer (void *p1)
		{
			Verify (g_TimerStack);
			starttime = GetCycles ();
			g_TimerStack->push ((__int64 *) p1);
		}
		~my_AutoTimer (void)
		{
			Verify (g_TimerStack->size ());
			starttime = GetCycles () - starttime;
			*(g_TimerStack->top ()) += starttime;
			g_TimerStack->pop ();
			if (g_TimerStack->size ())
			{
				*(g_TimerStack->top ()) -= starttime;
			}
		}
	};
#endif

};

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// bunch of random helper functions below  ...

#define NOT_YET_IMPLEMENTED Verify(!"Not yet implemented.");

inline Stuff::Scalar NormalizedValue(Stuff::Scalar value, Stuff::Scalar min, Stuff::Scalar max, bool invert = false)
{
	Stuff::Scalar rv((value - min) / (max - min));

	if (invert == true)
	{
		rv = 1 - rv;
	}

	Clamp(rv,0,1);
	return (rv);
}

inline Stuff::Scalar RandomlySkewScalar(Stuff::Scalar value, Stuff::Scalar skew_amount)
{
	Stuff::Scalar skew = (Stuff::Random::GetFraction() * 2) - 1.0f;
	return (value + (skew * skew_amount));
}

inline void OffsetTargetPoint(Stuff::Point3D& p, Stuff::Scalar amount)
{
	p.x = RandomlySkewScalar(p.x,amount);
	p.y = RandomlySkewScalar(p.y,amount * 1.2f);
	p.z = RandomlySkewScalar(p.z,amount);
}

inline Stuff::Scalar NormalizedInterpolation(Stuff::Scalar normalized_value, Stuff::Scalar min, Stuff::Scalar max)
{
	Verify(normalized_value >= 0);
	Verify(normalized_value <= 1);

	return ((normalized_value * max) + ((1 - normalized_value) * min));
}

template <class T>
inline T& RandomElement(std::vector<T>& v)
{
	return (v[gos_rand() % v.size()]);
}

template <class T>
inline const T& RandomElement(const std::vector<T>& v)
{
	return (v[gos_rand() % v.size()]);
}

#if !defined(NO_TIMERS)
	class GOSTimer
	{
	  public:
		GOSTimer(__int64& time_variable)
			: m_Variable(time_variable)
			, m_StartTime(GetCycles())
		{ }

		~GOSTimer()
		{
			m_Variable += GetCycles() - m_StartTime;
		}

	  private:
		__int64& m_Variable;
		__int64 m_StartTime;
	};

	#define TIME_FUNCTION(variable_name) GOSTimer gos_timer##variable_name(variable_name)
#else
	#define TIME_FUNCTION(variable_name)
#endif

inline Stuff::Scalar GetLengthSquared(const Stuff::Vector3D& vector1, const Stuff::Vector3D& vector2)
{
	Stuff::Vector3D delta;
	delta.Subtract(vector1,vector2);

	return (delta.GetLengthSquared());
}

inline Stuff::Scalar GetApproximateLength(const Stuff::Vector3D& vector1, const Stuff::Vector3D& vector2)
{
	Stuff::Vector3D delta;
	delta.Subtract(vector1,vector2);

	return (delta.GetApproximateLength());
}

inline Stuff::Scalar Sign(Stuff::Scalar value)
{
	if (value < 0)
	{
		return (-1);
	}

	return (1);
}

inline Stuff::Vector3D RotateVector(const Stuff::Point3D& point_to_rotate, const Stuff::Point3D& relative_to, Stuff::Scalar radians, Stuff::Scalar length_multiplier = 1)
{
	Stuff::Vector3D delta;
	delta.Subtract(point_to_rotate,relative_to);
	delta.y = 0;

	delta *= length_multiplier;

	Stuff::YawPitchRoll ypr(radians,0,0);
	Stuff::AffineMatrix4D rotation_matrix(ypr);

	Stuff::Vector3D rv;
	rv.Multiply(delta,rotation_matrix);

	rv += relative_to;

	return (rv);
}

inline Stuff::Scalar GetSquaredDistToMatrixForward(const Stuff::Point3D& point, const Stuff::LinearMatrix4D& matrix)
{
	Stuff::UnitVector3D forward_vector;
	matrix.GetLocalForwardInWorld(&forward_vector);
	Stuff::Point3D forward(forward_vector);
	forward += (Stuff::Point3D)matrix;

	return (GetLengthSquared(point,forward));
}

inline Stuff::Scalar GetSquaredDistToMatrixBackward(const Stuff::Point3D& point, const Stuff::LinearMatrix4D& matrix)
{
	Stuff::UnitVector3D backward_vector;
	matrix.GetLocalBackwardInWorld(&backward_vector);
	Stuff::Point3D backward(backward_vector);
	backward += (Stuff::Point3D)matrix;

	return (GetLengthSquared(point,backward));
}

inline Stuff::Scalar GetSquaredDistToMatrixLeft(const Stuff::Point3D& point, const Stuff::LinearMatrix4D& matrix)
{
	Stuff::UnitVector3D left_vector;
	matrix.GetLocalLeftInWorld(&left_vector);
	Stuff::Point3D left(left_vector);
	left += (Stuff::Point3D)matrix;

	return (GetLengthSquared(point,left));
}

inline Stuff::Scalar GetSquaredDistToMatrixRight(const Stuff::Point3D& point, const Stuff::LinearMatrix4D& matrix)
{
	Stuff::UnitVector3D right_vector;
	matrix.GetLocalRightInWorld(&right_vector);
	Stuff::Point3D right(right_vector);
	right += (Stuff::Point3D)matrix;

	return (GetLengthSquared(point,right));
}

inline Stuff::Scalar YawToPoint(const Stuff::LinearMatrix4D& matrix, const Stuff::Point3D& point)
{
	Stuff::Point3D matrix_to_point;
	matrix_to_point.MultiplyByInverse(point,matrix);

	// the following code adapted from YawPitchRange::operator=()
	Stuff::Scalar sub_range(matrix_to_point.x*matrix_to_point.x + matrix_to_point.z*matrix_to_point.z);
	Stuff::Scalar range(SqrtApproximate(sub_range + matrix_to_point.y*matrix_to_point.y));
	if (Stuff::Small_Enough(range))
	{
		return (0.0f);
	}

	sub_range = SqrtApproximate(sub_range);
	if (Stuff::Small_Enough(sub_range))
	{
		return (0.0f);
	}

	return (Stuff::Arctan(matrix_to_point.x, matrix_to_point.z));
}

inline bool MatrixFacesPoint(const Stuff::LinearMatrix4D& matrix, const Stuff::Point3D& point, Stuff::Scalar angle)
{
	Stuff::Point3D matrix_to_point;
	matrix_to_point.MultiplyByInverse(point,matrix);

	angle = Stuff::Fabs(angle);

	// the following code adapted from YawPitchRange::operator=()
	Verify((Vector3D::Forward.z == 1.0f) && (Vector3D::Left.x == 1.0f) && (Vector3D::Up.y == 1.0f));
	Stuff::Scalar sub_range(matrix_to_point.x*matrix_to_point.x + matrix_to_point.z*matrix_to_point.z);
	Stuff::Scalar range(Stuff::SqrtApproximate(sub_range + matrix_to_point.y*matrix_to_point.y));
	if (Stuff::Small_Enough(range))
	{
		return (true);
	}

	sub_range = Stuff::SqrtApproximate(sub_range);
	if (Stuff::Small_Enough(sub_range))
	{
		return (angle < Stuff::Pi_Over_2);
	}

	if (Stuff::Fabs(Stuff::Arctan(matrix_to_point.x, matrix_to_point.z)) > angle)
	{
		return (false);
	}

	return (Stuff::Arctan(matrix_to_point.y, sub_range) < angle);
}

inline bool MatrixFacesPoint_PitchOnly(const Stuff::LinearMatrix4D& matrix, const Stuff::Point3D& point, Stuff::Scalar angle)
{
	Stuff::Point3D matrix_to_point;
	matrix_to_point.MultiplyByInverse(point,matrix);
	
	angle = Stuff::Fabs(angle);

	// the following code adapted from YawPitchRange::operator=()
	Verify((Vector3D::Forward.z == 1.0f) && (Vector3D::Left.x == 1.0f) && (Vector3D::Up.y == 1.0f));
	Stuff::Scalar sub_range(matrix_to_point.x*matrix_to_point.x + matrix_to_point.z*matrix_to_point.z);
	Stuff::Scalar range(Stuff::SqrtApproximate(sub_range + matrix_to_point.y*matrix_to_point.y));
	if (Stuff::Small_Enough(range))
	{
		return (true);
	}

	sub_range = Stuff::SqrtApproximate(sub_range);
	if (Stuff::Small_Enough(sub_range))
	{
		return (angle < Stuff::Pi_Over_2);
	}

	return (Stuff::Arctan(matrix_to_point.y, sub_range) < angle);
}

inline Stuff::Scalar TimeToCollide(const Stuff::LinearMatrix4D& collidee_matrix,
								   Stuff::Scalar collidee_speed,
								   const Stuff::Point3D& collider_pos,
								   Stuff::Scalar collider_speed)
{
	Stuff::UnitVector3D _his_forward;
	collidee_matrix.GetLocalForwardInWorld(&_his_forward);

	Stuff::Point3D his_forward(_his_forward);
	his_forward *= collidee_speed;

	Stuff::Point3D my_forward;
	my_forward.Subtract((Stuff::Point3D)collidee_matrix,collider_pos);

	if (Stuff::Small_Enough(my_forward.GetLengthSquared()) == true)
	{
		return (0);
	}

	my_forward.Normalize(my_forward);
	my_forward *= collider_speed;

	Stuff::Point3D delta;
	delta.Subtract(his_forward,my_forward);
	delta.y = 0;

	Stuff::Scalar denominator = delta.GetApproximateLength();

	if (Stuff::Small_Enough(denominator) == true)
	{
		return (0);
	}

	Stuff::Point3D distance;
	distance.Subtract((Stuff::Point3D)collidee_matrix,collider_pos);
	distance.y = 0;

	return (distance.GetApproximateLength() / denominator);
}

inline std::string ScalarToString(Stuff::Scalar scalar)
{
	char buf[40];
	sprintf(buf,"%.1f",scalar);
	return (buf);
}

inline std::string Point3DToString(const Stuff::Point3D& point)
{
	char buf[100];
	sprintf(buf,"(%.1f %.1f %.1f)",point.x,point.y,point.z);
	return (buf);
}

inline std::string IntToString(int i)
{
	char buf[100];
	sprintf(buf,"%d",i);
	return (buf);
}

inline std::string BoolToString(bool b)
{
	if (b == true)
	{
		return ("true");
	}

	return ("false");
}

template <class InputIterator, class EqualityComparable>
__STL_DIFFERENCE_TYPE(InputIterator)
index_of(InputIterator first, InputIterator last, const EqualityComparable& value)
{
	__STL_DIFFERENCE_TYPE(InputIterator) n = 0;
	{for (;
		  first != last;
		  ++first)
	{
		if (*first == value)
		{
			break;
		}

		++n;
	}}

	return (n);
}

inline bool LinePenetrates(const Stuff::Line3D& line, const Stuff::Point3D& point, Stuff::Scalar radius)
{
	Stuff::Sphere sphere(point,radius);
	Stuff::Scalar penetration;
	if (line.GetDistanceTo(sphere,&penetration) < 0)
	{
		return (false);
	}

	return (true);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

namespace MW4AI
{

	Stuff::Scalar GetMapY(Stuff::Scalar x, Stuff::Scalar z,Adept::Entity *who,BYTE& material,Stuff::Scalar basey=-1.0f, Adept::Entity** entity_hit = 0);

	inline void ConvertPointToGrid (Stuff::Point3D& pt)
	{
/*
		pt.x /= 10.0f;
		pt.y /= 10.0f;
		pt.z /= 10.0f;
		pt.x *= 10.0f;
		pt.y *= 10.0f;
		pt.z *= 10.0f;
		pt.x += 5.0f;
		pt.y += 5.0f;
		pt.z += 5.0f;
*/
	}
	inline long ConvertKPHtoMPS (long value)
	{
		Stuff::Scalar temp;

		temp = (Stuff::Scalar) value;
		temp = (temp * 1000.0f)/ (60.0f*60.0f);
		return (long) temp;
	}

	inline float ConvertKPHtoMPSFloat (long value)
	{
		Stuff::Scalar temp;
		
		temp = (Stuff::Scalar) value;
		temp = (temp * 1000.0f)/ (60.0f*60.0f);
		return temp;
	}

class CTimeServer
{
protected:
	Stuff::Time m_StartTime;
	Stuff::Time m_MarkTime;
	Stuff::Time m_PauseTime;
	Stuff::Time m_PauseStartTime;
	Stuff::Time m_StopTime;
	unsigned int m_Paused;
	bool m_Running;

public:
	CTimeServer (void);
	~CTimeServer (void);

	void Tick (Stuff::Time till);

	void Start (void);
	void Stop (void);
	void Pause (bool on);
	void Mark (void)
	{ m_MarkTime = gos_GetElapsedTime (); }

	bool Running (void) const
	{ return m_Running; }

	Stuff::Time Elapsed (void)
	{ return ElapsedRaw (); }
	
	Stuff::Time	CurrTime (void)
	{ return CurrTimeRaw (); }
	Stuff::Time ElapsedRaw (void);			// since last mark point
	Stuff::Time CurrTimeRaw (void);		// since start time
	
	Stuff::Time StartTime (void) const
	{ return m_StartTime; }
	Stuff::Time StopTime (void) const
	{ return m_StopTime; }
	Stuff::Time PauseTime (void) const
	{ return m_PauseTime; }

	void SetTime (Stuff::Time amount)
	{
		Verify (m_Running);
		m_StartTime = 0;
		m_MarkTime = 0;
		if (m_Paused)
		{
			m_PauseStartTime = gos_GetElapsedTime ();
		}
		m_StopTime = 0;
		m_PauseTime = 0;
		m_StartTime = gos_GetElapsedTime () - amount;
	}

	void AddTime (Stuff::Time amount)
	{ m_StartTime -= amount; }
};

inline double Normalize (const double& value,const double& min,const double& max)
{
	Verify ((max-min) != 0);
  return ((value-min) / (max-min));
}
template<class _A1, class _A2, class _R>
	struct binary_function {
	typedef _A1 first_argument_type;
	typedef _A2 second_argument_type;
	typedef _R result_type;
	};

template<class _A1, class _R>
	struct unary_function {
	typedef _A1 first_argument_type;
	typedef _R result_type;
	};

template<class _Ty>
	struct less : binary_function<_Ty, _Ty, bool> {
	bool operator()(const _Ty& _X, const _Ty& _Y) const
		{return (_X < _Y); }
	};

template <class _Type,bool insertoptimize=true , class _Cmp = less <_Type> >
class my_heap
{
private:
		struct my_heapNode
		{
				 my_heapNode *m_Next,*m_Prev;
				 _Type m_Data;
		
				 my_heapNode(const _Type& data)
				 { m_Data = data; m_Next = m_Prev = NULL;}
				 ~my_heapNode(void)
				 { m_Next = m_Prev = NULL;}
		
				 const _Type& Data (void) const
				 { return m_Data; }
				 _Type& Data (void)
				 { return m_Data; }
		
		};

     my_heapNode *m_Root;
		 int m_NumNodes;

public:

     my_heap(void)
		 { m_Root = NULL; m_NumNodes=0;}
     virtual ~my_heap()
		 {
			my_heapNode *cur,*temp;

			 cur = m_Root;
			 while (cur)
			 {
				 temp = cur->m_Next;
					delete cur;
					cur = temp;
			 }
			 m_Root = NULL;
		 }
		 void clear (void)
		 {
			 my_heapNode *cur,*temp;

				cur = m_Root;
				while (cur)
				{
					temp = cur->m_Next;
					 delete cur;
					 cur = temp;
				}
				m_Root = NULL;
				m_NumNodes = 0;
		 }

// The Standard Heap Operations

     void Insert(const _Type& NodeData)
		 {
			 my_heapNode *NewNode;

			 NewNode = new my_heapNode (NodeData);

			 my_heapNode *cur,*prev;

       NewNode->m_Next = NewNode->m_Prev = NULL;
			 m_NumNodes++;
			 if (insertoptimize)
			 {
				 NewNode->m_Next = m_Root;
				 if (m_Root)
					 m_Root->m_Prev = NewNode;
				 m_Root = NewNode;
			 }
			 else
			 {
				 _Cmp value_compare;
				 if (!m_Root)
				 {
					 m_Root = NewNode;
					 m_Root->m_Next = m_Root->m_Prev = NULL;
					 return;
				 }
				 cur = m_Root->m_Next;
				 prev = m_Root;
				 while (cur)
				 {
					 if ( value_compare (cur->Data () , NewNode->Data ()))
					 {
						 NewNode->m_Prev = cur->m_Prev;
						 NewNode->m_Next = cur;
						 if (cur->m_Prev)
							 cur->m_Prev->m_Next = NewNode;
						 cur->m_Prev = NewNode;
						 return;
					 }
					 prev = cur;
					 cur = cur->m_Next;
				 }
				 Verify (prev->m_Next == NULL);
				 prev->m_Next = NewNode;
				 NewNode->m_Prev = prev;
				 NewNode->m_Next = NULL;
			 }
		 }
     void Union(my_heap<_Type> *OtherHeap)
		 {
			 _Type& cur;

			 while (OtherHeap->size ())
			 {
				 cur = OtherHeap->ExtractMin ();
				 Insert (cur);
			 }
		 }

     const _Type& Minimum(void) const
		 {
			 _Type toret;
			 my_heapNode *cur;
			 Verify (m_Root);
			 Verify (m_NumNodes);
			 if (insertoptimize)
			 {
				 _Cmp value_compare;
				 my_heapNode *min;
				 cur = m_Root;
				 min = cur;
				 cur = cur->m_Next;
				 while (cur)
				 {
					 if ( value_compare (cur->Data () , min->Data ()))
						 min = cur;
					 cur = cur->m_Next;
				 }
				 return min->Data ();
			 }
			 else
			 {
				 return m_Root->Data ();
			 }
		 }

     _Type ExtractMin(void)
		 {
			 _Type toret;
			 my_heapNode *cur;
			 Verify (m_Root);
			 Verify (m_NumNodes);
			 m_NumNodes--;
			 if (insertoptimize)
			 {
				 _Cmp value_compare;
				 my_heapNode *min;
				 cur = m_Root;
				 min = cur;
				 cur = cur->m_Next;
				 while (cur)
				 {
					 if ( value_compare (cur->Data () , min->Data ()))
						 min = cur;
					 cur = cur->m_Next;
				 }
				 if (min->m_Prev)
					 min->m_Prev->m_Next = min->m_Next;
				 if (min->m_Next)
					 min->m_Next->m_Prev = min->m_Prev;
				 if (m_Root == min)
					 m_Root = min->m_Next;
				 toret = min->Data ();
				 delete min;
			 }
			 else
			 {
				 cur = m_Root;
				 m_Root = m_Root->m_Next;
				 m_Root->m_Prev = NULL;
				 toret = cur->Data ();
				 delete cur;
			 }
			 return toret;
		 }

     void DecreaseKey(const _Type& theNode)
		 {
			 if (Delete (theNode))
				Insert (theNode);
		 }

		 bool Member (const _Type& theNode)
		 {
			 my_heapNode *cur;

			 cur = m_Root;
			 while (cur)
			 {
				 if (cur->Data () == theNode)
					 return true;
				 cur = cur->m_Next;
			 }
			 return false;
		 }
     bool Delete(const _Type& theNode)
		 {
			 my_heapNode *cur;

			 cur = m_Root;
			 while (cur)
			 {
				 if (cur->Data () == theNode)
					 break;
				 cur = cur->m_Next;
			 }
			 if (!cur)
				 return false;
			 m_NumNodes--;
			 if (cur->m_Next)
				 cur->m_Next->m_Prev = cur->m_Prev;
			 if (cur->m_Prev)
				 cur->m_Prev->m_Next = cur->m_Next;
			 if (cur == m_Root)
				 m_Root = m_Root->m_Next;
			 cur->m_Next = NULL;
			 cur->m_Prev = NULL;
			 delete cur;
			 return true;
		 }

// Extra utility functions


     long size(void) { return m_NumNodes; }
};

}

#endif