#include "MW4Headers.hpp"
#include "Adept\AdeptHeaders.hpp"

#include "mw4.hpp"
#include "moverai.hpp"
#include "Adept\Interface.hpp"
#include "Adept\Application.hpp"
#include "Adept\player.hpp"
#include "torso.hpp"
#include "move_rect.hpp"
#include "planeai.hpp"

#include "aimovedata.hpp"

using namespace MechWarrior4;
using namespace MW4AI;
#pragma warning (push)
#include <algorithm>
#pragma warning (pop)

CFollowData::CFollowData (MW4AI::CRailGraph *graph,MoverAI *owner) : CMoveData (graph,owner)
{
	m_WhoFollow = NULL;
	m_OffsetX = m_OffsetZ = 0.0f;
	m_LastEndPoint = Stuff::Point3D (0,0,0);
	m_LastCalcTime = 0;
	m_MoveType = MOVE_FOLLOW;
}

CFollowData::~CFollowData (void)
{
	m_WhoFollow = NULL;
	m_OffsetX = m_OffsetZ = 0.0f;
}

void CFollowData::Save (MemoryStream *stream)
{
	*stream << m_OffsetX;
	*stream << m_OffsetZ;
	*stream << m_LastCalcTime;
	*stream << m_LastEndPoint;
	*stream << m_MaxSpeed;
	if (m_WhoFollow)
	{
		*stream << true;
		*stream << m_WhoFollow->objectID;
	}
	else
		*stream << false;
	CMoveData::Save (stream);
}

void CFollowData::Load (MemoryStream *stream)
{
	bool flag;
	ObjectID object_id;

	*stream >> m_OffsetX;
	*stream >> m_OffsetZ;
	*stream >> m_LastCalcTime;
	*stream >> m_LastEndPoint;
	*stream >> m_MaxSpeed;
	*stream >> flag;
	m_WhoFollow = NULL;
	if (flag)
	{
		*stream >> object_id;
		m_WhoFollow = Cast_Object (MWObject *,NameTable::GetInstance()->FindData(object_id));
	}
	CMoveData::Load (stream);
}

void CFollowData::InsertObject (Entity *who)
{
	if (who->IsDerivedFrom (AI::DefaultData))
	{
		AI *ai;
		ai = Cast_Object (AI *,who);
		m_WhoFollow = ai->getEntity ();
	}
	else
		m_WhoFollow = who;
	inherited::InsertObject (who);
}

void CFollowData::StartExecuting (MoverAI *ai,Stuff::Time till)
{
	inherited::StartExecuting (ai,till);
	m_LastCalcTime = 0;
	UpdateFollow (ai,till);
}
void CFollowData::Execute (MoverAI *ai,Stuff::Time till)
{
	UpdateFollow (ai,till);
	inherited::Execute (ai,till);
}
void CFollowData::PathDone (MoverAI *ai,Stuff::Time till,bool failed)
{
	inherited::PathDone (ai,till,failed);
	if (failed)
		return;

	m_LastCalcTime = 0;
	ai->NewPath (NULL);
	m_LastEndPoint = Stuff::Point3D (-1,-1,-1);
	UpdateFollow (ai,till);
}

void CFollowData::CreateRequests (const Stuff::Point3D& start,int usageflags)
{
	inherited::CreateRequests (start,usageflags);

}

bool CFollowData::operator== (const CFollowData& p1)
{
	if (!inherited::operator== ((inherited)p1))
		return false;
	if (m_WhoFollow->objectID == p1.m_WhoFollow->objectID)
		return false;
	return true;
}

Stuff::Scalar CFollowData::Speed (void) const
{
	Point3D src,dest,extra;
	Scalar dist,comp;
	bool specpoint;

	if (!m_AI)
		return 0;
	if (!m_WhoFollow)
		return 0;
	Verify (m_WhoFollow);
	Verify (m_AI->getVehicle ());
	src = (Point3D) m_AI->getVehicle ()->GetLocalToWorld ();
	dest = FollowPoint (specpoint);
	extra.Subtract (src,dest);
	dist = extra.GetLengthSquared ();
	if (m_WhoFollow->IsDerivedFrom (Vehicle::DefaultData))
	{
		Vehicle *veh;
		comp = (m_OffsetX*m_OffsetX) + (m_OffsetZ*m_OffsetZ);
		if (comp == 0)
			comp = 10.0f;
		veh = Cast_Object (Vehicle *,m_WhoFollow);
		if (veh->currentSpeedMPS == 0)
		{
			Stuff::Scalar temp;
			if (comp <= 200.0f)
				temp = NormalizedValue (dist,0,comp);
			else
				temp = NormalizedValue (dist,200,comp);
			Max_Clamp (temp,1.0);
			Max_Clamp (temp,m_MaxSpeed);
			return temp;
		}
		else
		{
			const Vehicle__GameModel *model = static_cast<const MechWarrior4::Vehicle__GameModel *> (m_AI->getVehicle ()->GetGameModel ()) ;
			Stuff::Scalar temp,myspeed;


			if (dist < 400.0f)
			{
				UnitVector3D p1,p2;
				Point3D extra,otherloc;
				m_AI->getVehicle ()->GetLocalToWorld ().GetLocalForwardInWorld (&p1);
				Scalar dot1 = 5.0f,dot2 = 5.0f;
				otherloc = (Point3D) veh->GetLocalToWorld ();
				extra.Subtract (otherloc,src);
				if (Small_Enough (extra.GetLengthSquared ()))			// get point in reference to us
					dot1 = 0.0f;
				else
				{
					p2 = extra;
					dot1 = (p1.x*p2.x) + (p1.z*p2.z);
				}

				veh->GetLocalToWorld ().GetLocalForwardInWorld (&p2);			// get whofollow dir in relation to us
				dot2 = (p1.x*p2.x) + (p1.z*p2.z);

				if (dot1 <= 0.0f)		// point is behind us,
					return 0;
//				if ((dot1 <= 0.0f) && (dot2 >= 0.0f))		// point is behind us, and person we are following if moving towards us
//					return 0;
			}


			if (specpoint)		// need to avoid hitting wall
			{
				Scalar slowdist,slowtime;
				slowtime = model->maxSpeed / model->decceleration;
				slowdist = (0.5f * model->decceleration * (slowtime*slowtime)) + (m_AI->getVehicle ()->currentSpeedMPS*slowtime);
				slowdist *= slowdist;
				if (slowdist >= dist)
				{
					temp = NormalizedValue (dist,100,slowdist);
				}
				else
					temp = 1.0f;
				Max_Clamp (temp,m_MaxSpeed);
				return temp;
			}
			else
			{
				temp = veh->currentSpeedMPS-2.0f;

				myspeed = NormalizedValue (temp,0,model->maxSpeed);
				Max_Clamp (myspeed,1.0);
				if (dist < 100.0f)
					return myspeed;
				temp = NormalizedValue (dist,0,comp);
				Max_Clamp (temp,1.0);
				temp = (myspeed) + ((1.0f-myspeed) * temp);
				Max_Clamp (temp,m_MaxSpeed);
				return temp;
			}
		}
	}
	else
	{
		if (dist < 100.0f)
			return 0;
		return m_MaxSpeed;
	}
}

Stuff::Point3D CFollowData::FollowPoint (bool& modified) const
{
	Point3D dest,offset,third;
	Point3D targloc;

	Verify (m_WhoFollow);
	offset.x = m_OffsetX;
	offset.y = 0;
	offset.z = m_OffsetZ;

	modified = false;
	targloc = (Point3D) m_WhoFollow->GetLocalToWorld ();

	LinearMatrix4D tempmat (m_WhoFollow->GetLocalToWorld ());

	tempmat.AlignLocalAxisToWorldVector (Vector3D::Up,Y_Axis,X_Axis,Z_Axis);
	third.Multiply(offset,tempmat);
//	dest = (Point3D) m_WhoFollow->GetLocalToWorld ();
	dest = third;

	if (m_AI->IsDerivedFrom (PlaneAI::DefaultData))
	{
		return dest;
	}

	const OBRect *brid1,*brid2;
	brid1 = m_Graph->RectList ()->Inside (targloc);
	brid2 = m_Graph->RectList ()->Inside (dest);
	
	unsigned short value = m_Graph->Passable (dest.x,dest.z);
	if (brid1 != brid2)
	{
		offset.x = 0;
		offset.y = 0;
		offset.z = m_OffsetZ;
		third.Multiply(offset,tempmat);
		dest = third;
		const OBRect *brid3;
		brid3 = m_Graph->RectList ()->Inside (dest);
		value = m_Graph->Passable (dest.x,dest.z);
		if (brid3 != brid1)
			return (Point3D) m_AI->getEntity ()->GetLocalToWorld ();
	}

	unsigned char mask=0;
	mask = (unsigned char) ConvertUsagetoMovePass (m_Usage);

	if ((value&~BRIDGE_FLAG) & mask)
	{
		Point3D loc,temp,extra;
		Scalar dist;
		
		modified = true;
		loc = (Point3D) m_AI->getVehicle ()->GetLocalToWorld ();

		temp = m_Graph->PullPointElement (loc,dest,m_Usage);

		extra.Subtract (loc,temp);
		dist = extra.GetLengthSquared ();

		m_LastCalcTime = 0;
		if ((temp == Point3D (-1,-1,-1)) || (dist < 100.0f))
		{
			dest = (Point3D) m_WhoFollow->GetLocalToWorld ();
			ConvertPointToGrid (dest);
			return dest;
		}
		ConvertPointToGrid (temp);
		return temp;
	}

	ConvertPointToGrid(dest);
	return dest;
}


#if 1
void CFollowData::UpdateFollow (MoverAI *ai,Stuff::Time till)
{
	
	Point3D src,dest,extra;
	Scalar dist,comp,olddist;
	bool specpoint;

	Verify (m_AI);
	Verify (m_WhoFollow);
	Verify (m_AI->getVehicle ());

	m_AI->m_OrderSpeed = Speed ();
	m_AI->m_NewSpeedRequest = m_AI->m_OrderSpeed;
	if (m_AI->IsDerivedFrom (PlaneAI::DefaultData))
	{
		return;
	}
	if (m_AI->m_OrderSpeed == 0)
	{
		ai->NewPath (NULL,false);
		return;
	}
	dest = FollowPoint (specpoint);
	src = (Point3D) m_AI->getVehicle ()->GetLocalToWorld ();
	extra.Subtract (src,dest);
	dist = extra.GetLengthSquared ();

	extra.Subtract (dest,m_LastEndPoint);
	olddist = extra.GetLengthSquared ();

	if (m_WhoFollow->IsDerivedFrom (Vehicle::DefaultData))
	{
//		Vehicle *veh;
		comp = (m_OffsetX*m_OffsetX) + (m_OffsetZ*m_OffsetZ);
//		if (dist > comp)
		{
			src = (Point3D) m_AI->getVehicle ()->GetLocalToWorld ();
			if (m_PathRequest)
			{
				if (m_PathRequest->Done ())
				{
					CRailPath *path;
					path = m_PathRequest->DetachNodePath ();
					delete m_PathRequest;
					m_PathRequest = NULL;
					ai->NewPath (path,false);
					m_LastCalcTime = till;
					return;
				}
				else
				{
					if (!m_PathRequest->UpdateSrcDest (src,dest))
					{
						g_PathManager->RemoveRequest (m_PathRequest);
						delete m_PathRequest;
						m_PathRequest = NULL;
					}
					else
						return;
				}
			}

			if ((dist < 62500) || 
				(((m_LastCalcTime + 10.0f) < till) && (dist > 62500) && (dist<250000)) || 
				(((m_LastCalcTime + 20.0f) < till) && (dist > 25000)))
			{
				if (dist < 100)
				{
					ai->SetMoveHead (Point3D (-1,-1,-1),0);
					return;
				}
//				veh = Cast_Object (Vehicle *,m_WhoFollow);
				const Vehicle__GameModel *model = static_cast<const MechWarrior4::Vehicle__GameModel *> (m_AI->getVehicle ()->GetGameModel ()) ;

				unsigned short usa = (unsigned short) (MW4AI::ConvertTypetoUsage (model->moveTypeFlag)&(~JUMPFLAG));
				if (m_Graph->QuickCheck ((int) src.x,(int) src.z,(int) dest.x,(int) dest.z,(unsigned short) ConvertUsagetoMovePass (usa)))
				{
					ai->NewPath (NULL);
					ai->SetMoveHead (dest,m_AI->m_OrderSpeed);
					return;
				}

				gos_PushCurrentHeap (g_RailHeap);
				m_PathRequest = g_PathManager->AddRequest (gos_GetElapsedTime (),m_Graph,src,dest,MW4AI::ConvertTypetoUsage (model->moveTypeFlag),m_AI,m_AI->AllowTenMeter ());
				gos_PopCurrentHeap ();

				m_LastEndPoint = dest;

				m_LastCalcTime = till;
			}
		}
	}
}
#else
void CFollowData::UpdateFollow (MoverAI *ai,Stuff::Time till)
{
	
	Point3D src,dest,extra;
	Scalar dist,comp,olddist;
	bool specpoint;

	Verify (m_AI);
	Verify (m_WhoFollow);
	Verify (m_AI->getVehicle ());

	m_AI->m_OrderSpeed = Speed ();
	m_AI->m_NewSpeedRequest = m_AI->m_OrderSpeed;
	if (m_AI->IsDerivedFrom (PlaneAI::DefaultData))
	{
		return;
	}
	if (m_AI->m_OrderSpeed == 0)
	{
		ai->NewPath (NULL,false);
		return;
	}
	dest = FollowPoint (specpoint);
	src = (Point3D) m_AI->getVehicle ()->GetLocalToWorld ();
	extra.Subtract (src,dest);
	dist = extra.GetLengthSquared ();

	extra.Subtract (dest,m_LastEndPoint);
	olddist = extra.GetLengthSquared ();

	if (m_WhoFollow->IsDerivedFrom (Vehicle::DefaultData))
	{
		Vehicle *veh;
		comp = (m_OffsetX*m_OffsetX) + (m_OffsetZ*m_OffsetZ);
//		if (dist > comp)
		{
			CRailPath *path;
			Point3D outvec;
			src = (Point3D) m_AI->getVehicle ()->GetLocalToWorld ();
			gos_PushCurrentHeap (g_RailHeap);
			path = new CRailPath (MW4AI::g_MissionGraph,src,dest,false,m_AI,m_AI->AllowTenMeter ());
			gos_PopCurrentHeap ();

			m_LastEndPoint = dest;

			veh = Cast_Object (Vehicle *,m_WhoFollow);
			const Vehicle__GameModel *model = static_cast<const MechWarrior4::Vehicle__GameModel *> (veh->GetGameModel ()) ;
			path->CalcPath (MW4AI::ConvertTypetoUsage (model->moveTypeFlag));
			ai->NewPath (path,false);
			m_LastCalcTime = till;
		}
	}
}

#endif