#include "MW4Headers.hpp"

#include "Boat.hpp"
#include "gameinfo.hpp"
#include <AI.hpp>
#include <moverai.hpp>
#include <Adept\CollisionGrid.hpp>

#include <Adept\EntityManager.hpp>

//#############################################################################
//###############################    Boat    ##############################
//#############################################################################

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Boat::ClassData*
Boat::DefaultData = NULL;

DWORD MechWarrior4::Executed_Boat_Count = 0;

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::InitializeClass()
{
	Check_Object(MWMover::ExecutionStateEngine::DefaultData);
	Verify(!DefaultData);
	DefaultData =
	new ClassData(
							 BoatClassID,
							 "MechWarrior4::Boat",
							 BaseClass::DefaultData,
							 0, NULL,
							 (Entity::Factory)Make,
							 (Entity::CreateMessage::Factory)CreateMessage::ConstructCreateMessage,
							 ExecutionStateEngine::DefaultData,
							 (Entity::GameModel::Factory)GameModel::ConstructGameModel,
							 (Entity::GameModel::Factory)GameModel::ConstructOBBStream,
							 (Entity::GameModel::ReadAndVerifier)GameModel::ReadAndVerify,
							 (Entity::GameModel::ModelWrite)GameModel::WriteToText,
							 (Entity::GameModel::ModelSave)GameModel::SaveGameModel,
							 AnimationStateEngine::Make
							 );
	Register_Object(DefaultData);


}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::TerminateClass()
{
	Unregister_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Boat* Boat::Make(CreateMessage *message,ReplicatorID *base_id)
{
	Check_Object(message);
	gos_PushCurrentHeap(Heap);
	Boat *new_entity = new
										 Boat(DefaultData, message, base_id, NULL);
	gos_PopCurrentHeap();
	Check_Object(new_entity);


	Check_Object(EntityManager::GetInstance());
	EntityManager::GetInstance()->AddMover(new_entity);
	return new_entity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Boat::Boat(ClassData *class_data,CreateMessage *message,ReplicatorID *base_id,ElementRenderer::Element *element):
		Vehicle(class_data, message, base_id, element)
{
	Check_Pointer(this);
	Check_Object(message);

	HookUpSubsystems();
	CommonCreation(message);
}

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

Boat::~Boat()
{
	Check_Object(EntityManager::GetInstance());
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::Reuse(
					 const CreateMessage *message,
					 ReplicatorID *base_id
					 )
{
	Check_Object(this);
	Check_Object(message);

	STOP(("Not implemented"));
	BaseClass::Reuse(message, base_id);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::CommonCreation(CreateMessage *message)
{
	Check_Object(this);
	Check_Object(message);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::Respawn(Entity::CreateMessage *message)
{
	Check_Object(this);
	Check_Object(message);

	BaseClass::Respawn(message);
	CreateMessage *vehicle_message = Cast_Pointer(CreateMessage *, message);
	CommonCreation(vehicle_message);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::TurnOn (void)
{
	lastParameterization = gos_GetElapsedTime ();
	executionState->RequestState(ExecutionStateEngine::DrivingMotionState);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::PreCollisionExecute(Time till)
{
	Check_Object(this);
	PRECOLLISION_LOGIC("Boat");

#if defined(LAB_ONLY)
	if (!instanceName)
		MWGameInfo::g_LastVehicle[0] = '\0';
	else
	{
		strncpy(MWGameInfo::g_LastVehicle, instanceName, sizeof(MWGameInfo::g_LastVehicle)-1);
		MWGameInfo::g_LastVehicle[sizeof(MWGameInfo::g_LastVehicle)-1] = '\0';
	}
	MWGameInfo::g_LastVehiclePos = GetLocalToWorld ();
#endif


	//
	//-------------------------------------------------------------
	// Let the base class deal with setting up the motion variables
	//-------------------------------------------------------------
	//
	BaseClass::PreCollisionExecute(till);
	Set_Statistic(Executed_Boat_Count, Executed_Boat_Count+1);

	//
	//----------------------------------------------------------------------
	// Any executing vehicle should not be in never execute state and should
	// always use post collision
	//----------------------------------------------------------------------
	//
	Check_Object(executionState);
	int pre_state = executionState->GetState();
	Verify(pre_state != ExecutionStateEngine::NeverExecuteState);

	//
	//-------------------------------------------------------------------------
	// Do the appropriate type of simulation.  Animated motion state is for BRB
	//-------------------------------------------------------------------------
	//
	Scalar time_slice = GetTimeSlice(till);
	switch (pre_state)
	{
		//
		//----------------------------------------------------
		// Always execute state is for the normal driving code
		//----------------------------------------------------
		//
		case ExecutionStateEngine::DrivingMotionState:
			ComputeForwardSpeed(time_slice);
			UpdateVehiclePosition(time_slice);
			break;
		case ExecutionStateEngine::AIMotionState:
			Stuff::Scalar maxSpeedMod=1.0f;
			switch (materialHit)
			{
				case WaterMaterial:
				case ConcreteMaterial:
				case SnowMaterial:
				case FakeShallowWaterMaterial:
					maxSpeedMod = 0.0f;
					break;
			}

			if (maxSpeedMod == 0.0f)				// tell the ai we had a collision based on material type
			{
				if ((m_AI) && (m_AI->IsDerivedFrom (MoverAI::DefaultData)))
				{
					MoverAI *ai = Cast_Object(MoverAI*, m_AI);
					ai->SlopeCollide();
				}
				currentSpeedMPS = 0;
				speedDemandKPH = 0.0f;
				currentSpeedKPH = 0.0f;
			}
			break;
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Boat::TestInstance() const
{
	Verify(IsDerivedFrom(DefaultData));
}

void Boat::ComputeForwardSpeed(Stuff::Scalar time_slice)
{
	Check_Object(this);
	const GameModel *model = GetGameModel();
	Check_Object(model);
	Stuff::Scalar maxSpeedMod;

	//
	//----------------------------------------------------
	// Set the demands to zero if the vehicle is shut down
	//----------------------------------------------------
	//
	if (vehicleShutDown)
	{
		speedDemand = 0.0f;
		speedDemandKPH = 0.0f;
	}

	Verify (model->moveTypeFlag == MWObject__GameModel::WATER_MOVETYPE);			// these should be boats
	maxSpeedMod = 1.0f;
	switch (materialHit)
	{
		case WaterMaterial:
		case ConcreteMaterial:
		case SnowMaterial:
		case FakeShallowWaterMaterial:
			maxSpeedMod = 0.0f;
			break;
	}

	if (maxSpeedMod == 0.0f)				// tell the ai we had a collision based on material type
	{
		if ((m_AI) && (m_AI->IsDerivedFrom (MoverAI::DefaultData)))
		{
			MoverAI *ai = Cast_Object(MoverAI*, m_AI);
			ai->SlopeCollide();
		}
	}

	//
	//------------------------------------------------------------------------
	// Figure out our desired speed.  If we are stopped, set the KPH variables
	// to zero and quit
	//------------------------------------------------------------------------
	//
	if (speedDemand >= 0.0f)
		speedDemandMPS = (speedDemand * GetMaxSpeed()*maxSpeedMod);
	else
		speedDemandMPS = -(speedDemand * model->maxReverseSpeed*maxSpeedMod);
	if (speedDemandMPS == 0.0f && currentSpeedMPS == 0.0f)
	{
		speedDemandKPH = 0.0f;
		currentSpeedKPH = 0.0f;
		return;
	}

	//
	//--------------------------------------------------------------------
	// If we are not going backwards, find out what speed we want after we
	// consider braking
	//--------------------------------------------------------------------
	//
	if (currentSpeedMPS >= 0.0f)
	{
		if (speedDemandMPS > currentSpeedMPS)
		{
			currentSpeedMPS += (model->acceleration * time_slice);
			Max_Clamp(currentSpeedMPS, speedDemandMPS);
		}
		else if (speedDemandMPS < currentSpeedMPS)
		{
			currentSpeedMPS -= (model->decceleration * time_slice);
			Min_Clamp(currentSpeedMPS, speedDemandMPS);
		}
	}

	else
	{

		if (speedDemandMPS < currentSpeedMPS)
		{
			currentSpeedMPS -= (model->acceleration * model->reverseAccelerationMultiplier * time_slice);
			Min_Clamp(currentSpeedMPS, speedDemandMPS);
		}
		else if (speedDemandMPS > currentSpeedMPS)
		{
			currentSpeedMPS += (model->decceleration * model->reverseDeccelerationMultiplier * time_slice);
			Max_Clamp(currentSpeedMPS, speedDemandMPS);
		}
	}

	//
	//----------------------------------------------------
	// Now adjust all this to the limits in the model file
	//----------------------------------------------------
	//
	Clamp(currentSpeedMPS, model->maxReverseSpeed*maxSpeedMod, GetMaxSpeed()*maxSpeedMod);

	//
	//---------------------------
	// Set up the velocity vector
	//---------------------------
	//
	localSpaceVelocity.linearMotion.z = currentSpeedMPS;

	//
	//----------------------
	// Set the KPH variables
	//----------------------
	//
	currentSpeedKPH = currentSpeedMPS * 3.6f;
	speedDemandKPH = speedDemandMPS * 3.6f;


	//
	//----------------------
	// Compute pitching and rolling in the water
	//----------------------
	//
	// x is pitch
	// y is yaw
	// z is roll

	YawPitchRoll ypr (GetLocalToWorld ());

	localSpaceAcceleration.angularMotion.x = -ypr.pitch;
	localSpaceAcceleration.angularMotion.y = 0;
	localSpaceAcceleration.angularMotion.z = -ypr.roll;
	
//	localSpaceAcceleration.angularMotion.x += ((Stuff::Random::GetFraction ()*Pi)-Pi_Over_2);
//	localSpaceAcceleration.angularMotion.z += ((Stuff::Random::GetFraction ()*Pi)-Pi_Over_2);

}

void Boat::UpdateVehiclePosition(Scalar time_slice)
{
	
	//
	//---------------------------------------------------------------------
	// Apply velocity/acceleration to position and acceleration to velocity
	//---------------------------------------------------------------------
	//
	const LinearMatrix4D &vehicle_to_world = GetLocalToWorld();
	Point3D world_translation(vehicle_to_world);
	Vector3D delta;
	delta.AddScaled(
		localSpaceVelocity.linearMotion,
		localSpaceAcceleration.linearMotion,
		0.5f * time_slice
	);
	Vector3D world_delta;
	world_delta.Multiply(delta, vehicle_to_world);
	world_translation.AddScaled(world_translation, world_delta, time_slice);

	localSpaceVelocity.linearMotion.y += localSpaceAcceleration.linearMotion.y*time_slice;

	localSpaceVelocity.angularMotion.x += localSpaceAcceleration.angularMotion.x * time_slice;
	localSpaceVelocity.angularMotion.y += localSpaceAcceleration.angularMotion.y * time_slice;
	localSpaceVelocity.angularMotion.z += localSpaceAcceleration.angularMotion.z * time_slice;

	//
	//--------------------------------------
	// Update our turn rates and orientation
	//--------------------------------------
	//
	const GameModel *model = GetGameModel();
	Check_Object(model);
	Scalar temp = Lerp(model->fullStopTurnRate, model->topSpeedTurnRate, currentSpeedMPS/GetMaxSpeed());
	YawPitchRoll new_rotation(vehicle_to_world);
	temp *= time_slice;
	new_rotation.yaw += yawDemand * temp;
	new_rotation.pitch += localSpaceVelocity.angularMotion.x * time_slice;
	new_rotation.roll += localSpaceVelocity.angularMotion.z * time_slice;

	//
	//----------------------------------------------
	// Cast a ray down to see what we are driving on
	//----------------------------------------------
	//
	Stuff::Line3D line;
	line.m_length = 40.001f + m_WaterDelta;
	line.m_direction = Vector3D::Down;
	line.m_origin = world_translation;
	line.m_origin.y += 20.0f;
	Stuff::Normal3D normal;
	CollisionQuery query(&line, &normal, CanBeWalkedOnFlag, this);
	Check_Object(CollisionGrid::Instance);

	//
	//------------------------------------------------
	// If we miss the ground, go to where gravity said
	//------------------------------------------------
	//
	LinearMatrix4D new_local_to_world;
	new_local_to_world.BuildRotation(new_rotation);
	if (!CollisionGrid::Instance->ProjectLine(&query))
	{
		materialHit = MaterialCount;
		worldSpaceVelocity.linearMotion.Multiply(
			localSpaceVelocity.linearMotion,
			vehicle_to_world
		);
		new_local_to_world.BuildTranslation(world_translation);
	}

	//
	//--------------------------------------------------
	// Otherwise, adapt the vehicle to the ground normal
	//--------------------------------------------------
	//
	else
	{
		materialHit = query.m_material;
		line.FindEnd(&world_translation);
		world_translation.y += m_WaterDelta;
		new_local_to_world.BuildTranslation(world_translation);

		new_local_to_world.AlignLocalAxisToWorldVector(Vector3D::Up, Y_Axis, X_Axis, Z_Axis);

		//
		//--------------------------------------------------
		// Update the velocities to conform to the landscape
		//--------------------------------------------------
		//
		localSpaceVelocity.linearMotion.x = 0.0f;
		localSpaceVelocity.linearMotion.y = 0.0f;
		localSpaceVelocity.linearMotion.z = currentSpeedMPS;
		worldSpaceVelocity.linearMotion.Multiply(
			localSpaceVelocity.linearMotion,
			new_local_to_world
		);
	}
	SetNewLocalToParent(new_local_to_world);
}