#include "MW4Headers.hpp"
#include "cheap_move.hpp"
#include <Adept\CollisionGrid.hpp>

using namespace MechWarrior4;
namespace MW4AI
{
	extern Stuff::Scalar MinZ, MaxZ, MinX, MaxX;
}

CCheapMover::CCheapMover (Stuff::Point3D newloc)
{
	m_Loc = newloc;
	m_Airplane = false;
	m_LastDir = Vector3D::Identity;
}

CCheapMover::CCheapMover (Stuff::Point3D newloc,Stuff::Scalar altitude)
{
	m_Loc = newloc;
	m_Airplane = true;
	m_LastDir = Vector3D::Identity;
	m_FlyingAltitude = altitude;
}

bool CCheapMover::UpdatePos (Point3D dest,Scalar speed,float till)
{
	Point3D extra,third;
	Scalar dist;

	if (dest.x < (MW4AI::MinX+50))						// fix off edge problems.
		dest.x = MW4AI::MinX+50;
	if (dest.z < (MW4AI::MinZ+50))
		dest.z = MW4AI::MinZ+50;
	
	if (dest.x > (MW4AI::MaxX-50))
		dest.x = MW4AI::MaxX-50;
	if (dest.z > (MW4AI::MaxZ-50))
		dest.z = MW4AI::MaxZ-50;

	dest.y = 0;
	m_Loc.y = 0;
	extra.Subtract (dest,m_Loc);
	dist = extra.GetLengthSquared ();

	Scalar move;
	if (speed < 0)
		move = (float) (speed * till * 6);		// use 6 frame buffer
	else
		move = (float) (speed * till * 6);		// use 6 frame buffer
	move *= move;

	if ((dist <= 9.0f) || (dist < move))	
	{
		m_Loc = dest;
		m_Dir = Vector3D::Identity;
		m_LastDir = Vector3D::Identity;
		return true;
	}
	speed *= (float) till;
	extra.y = 0;
	third.Normalize (extra);
	m_Dir = third;
	third *= speed;
	m_Loc += third;

	return false;
}

void CCheapMover::UpdatePos (Vehicle *veh,Stuff::Point3D dest)
{
	YawPitchRoll new_rotation(YawPitchRoll::Identity);
	new_rotation = (YawPitchRoll) veh->GetLocalToWorld ();
	Point3D new_translation;
	Point3D curlocation (veh->GetLocalToWorld ());

	new_translation = Loc ();

	{
		// Find the ground

		//
		// make a line...
		//
		Stuff::Line3D line;
		line.m_length = 20.0f;
		line.m_direction = Vector3D::Down;
		line.m_origin = new_translation;
		line.m_origin.y = curlocation.y + 5.0f;	// pretty harsh hill!  =)

		//
		// Prepare query
		//	
		Stuff::Normal3D normal;
		Entity::CollisionQuery query(&line, &normal, Entity::CanBeWalkedOnFlag, veh);

		//
		// Cast ray
		//
		Adept::Entity *entity_hit;
		Check_Object(CollisionGrid::Instance);
		entity_hit = CollisionGrid::Instance->ProjectLine(&query);

		if (entity_hit != NULL)
		{
			// if we hit ground, great!
			line.FindEnd(&new_translation);
		}
		else
		{
			// Damnit...then find the ground aggresivly!!!!
			line.m_origin = new_translation;
			line.m_origin.y = curlocation.y + 100.0f;	// pretty harsh hill!  =)
			line.m_length = 1000.0f;
			entity_hit = CollisionGrid::Instance->ProjectLine(&query);
			if (entity_hit != NULL)
			{
				line.FindEnd(&new_translation);
			}
			else // now try up!
			{
				line.m_direction = Vector3D(0.0f, 1.0f, 0.0f);
				entity_hit = CollisionGrid::Instance->ProjectLine(&query);
				if (entity_hit != NULL)
				{
					line.FindEnd(&new_translation);
				}
			}
			// ok stop we are way off the map
		}
	}

	LinearMatrix4D new_local_to_world;
	if (m_Airplane)
	{
		new_translation.y += m_FlyingAltitude;
	}
	if (new_translation.x < (MW4AI::MinX+50))						// fix off edge problems.
		new_translation.x = MW4AI::MinX+50;
	if (new_translation.z < (MW4AI::MinZ+50))
		new_translation.z = MW4AI::MinZ+50;
	
	if (new_translation.x > (MW4AI::MaxX-50))
		new_translation.x = MW4AI::MaxX-50;
	if (new_translation.z > (MW4AI::MaxZ-50))
		new_translation.z = MW4AI::MaxZ-50;



	new_local_to_world.BuildTranslation(new_translation);
	Point3D fred;
	fred.Add (m_Dir,m_LastDir);
	Scalar len;
	len = fred.GetLengthSquared ();
	if ((m_LastDir !=  Vector3D::Identity) && (m_Dir !=  Vector3D::Identity) && (len > 0.01f))
	{
		YawPitchRange fred (m_Dir);
		new_rotation.yaw = fred.yaw;
		new_rotation.pitch = fred.pitch;
		new_rotation.roll = 0;
	}
	m_LastDir = m_Dir;
	new_local_to_world.BuildRotation(new_rotation);

	veh->SetNewLocalToParent(new_local_to_world);
}