#include "MW4Headers.hpp"

#include "Helicopter.hpp"

#include <Adept\Effect.hpp>
#include <Adept\CollisionGrid.hpp>
#include <Adept\EntityManager.hpp>

//#############################################################################
//###############################    Helicopter    ############################
//#############################################################################

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::InitializeClass()
{
	Check_Object(ExecutionStateEngine::DefaultData);
	Verify(!DefaultData);
	DefaultData =
		new ClassData(
			HelicopterClassID,
	        "MechWarrior4::Helicopter",
			BaseClass::DefaultData,
			NULL, 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
	Helicopter::TerminateClass()
{
	Unregister_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

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

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Helicopter::Helicopter(
	ClassData *class_data,
	CreateMessage *message,
	ReplicatorID *base_id,
	ElementRenderer::Element *element
):
	Airplane(class_data, message, base_id, element),
	m_takeOffEffect(NULL),
	m_takeOffGroundEffect(NULL),
	m_landingEffect(NULL),
	m_landingGroundEffect(NULL)
{
	Check_Pointer(this);
	Check_Object(message);
	
	CommonCreation(message);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Helicopter::~Helicopter()
{
	Check_Pointer(this);

	if(m_takeOffEffect.GetCurrent())
		delete m_takeOffEffect.GetCurrent();
	if(m_takeOffGroundEffect.GetCurrent())
		delete m_takeOffGroundEffect.GetCurrent();
	if(m_landingEffect.GetCurrent())
		delete m_landingEffect.GetCurrent();
	if(m_landingGroundEffect.GetCurrent())
		delete m_landingGroundEffect.GetCurrent();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::CommonCreation(CreateMessage *message)
{
	Check_Object(this);
	Check_Object(message);
	
	m_StartTakeoffEffect = false;
	m_StartLandingEffect = false;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::Respawn(Entity__CreateMessage *message)
{
	Check_Object(this);
	Check_Object(message);
	
	BaseClass::Respawn(message);
	CreateMessage *helicopter_message = Cast_Pointer(CreateMessage *, message);
	CommonCreation(helicopter_message);
}

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

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Helicopter::PostCollisionExecute(Time till)
{
	Check_Object(this);

	if (m_StartTakeoffEffect)
	{
		CreateTakeOffEffect();
		CreateTakeOffGroundEffect(m_EffectGroundPosition);
		m_StartTakeoffEffect = false;
	}
	if (m_StartLandingEffect)
	{
		CreateLandingEffect();
		CreateLandingGroundEffect(m_EffectGroundPosition);
		m_StartLandingEffect = false;
	}
	Airplane::PostCollisionExecute (till);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::TakeOff(Stuff::Scalar runway_distance)
{
	Check_Object(this);
	
	const GameModel *model = GetGameModel();
	Check_Object(model);

	takeOffTargetPosition = GetLocalToWorld();

	Stuff::Line3D line;
	Point3D point;

	point.x = takeOffTargetPosition.x;
	point.z = takeOffTargetPosition.z;
	point.y = flyingAltitude+100.0f;

	line.m_length = flyingAltitude+300.0f;
	line.m_direction = Vector3D::Down;
	line.SetOrigin(point);

	//
	// Prepare query
	//	
	Stuff::Normal3D inormal;
	Stuff::Line3D iline;
	Stuff::Normal3D normal;
	Entity::CollisionQuery query(&line, &normal, Entity::CanBeWalkedOnFlag, NULL);

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

	line.FindEnd(&point);
	
	m_StartTakeoffEffect = true;
	m_EffectGroundPosition = point;

//	CreateTakeOffEffect();
//	CreateTakeOffGroundEffect(point);

	takeOffTargetPosition = Vector3D(takeOffTargetPosition.x, point.y + flyingAltitude, takeOffTargetPosition.z);

	localSpaceDrag.linearMotion = model->linearDragCoefficients;
	localSpaceDrag.angularMotion = model->angularDragCoefficients;

	LinearMatrix4D world_to_local;
	world_to_local.Invert(GetLocalToWorld());
	localSpaceAcceleration.linearMotion.Multiply(
		worldSpaceAcceleration.linearMotion,
		world_to_local
	);
	localSpaceAcceleration.angularMotion.Multiply(
		worldSpaceAcceleration.angularMotion,
		world_to_local
	);
	Check_Object(executionState);
	executionState->RequestState(ExecutionStateEngine::TakingOffMotionState);
	m_OnGround = false;

}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Helicopter::Land(void)
{
	Check_Object(this);
	Check_Object(executionState);

	landTargetPosition = GetLocalToWorld();

	Stuff::Line3D line;
	Point3D point;

	point.x = landTargetPosition.x;
	point.z = landTargetPosition.z;
	point.y = landTargetPosition.y-10;

	line.m_length = flyingAltitude+300.0f;
	line.m_direction = Vector3D::Down;
	line.SetOrigin(point);

	//
	// Prepare query
	//	
	Stuff::Normal3D inormal;
	Stuff::Line3D iline;
	Stuff::Normal3D normal;
	Entity::CollisionQuery query(&line, &normal, Entity::CanBeWalkedOnFlag, NULL);

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

	line.FindEnd(&point);

	m_StartLandingEffect = true;
	m_EffectGroundPosition = point;
//	CreateLandingEffect();
//	CreateLandingGroundEffect(point);

	landTargetPosition = Vector3D(landTargetPosition.x, point.y, landTargetPosition.z);

	executionState->RequestState(ExecutionStateEngine::LandingMotionState);
	m_OnGround = true;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Helicopter::TakeOffThrusterSimulation(Stuff::Time till, const Stuff::Vector3D& new_velocity, const Stuff::Vector3D& instantanious_angular_velocity,bool canswitch)
{
	Check_Object(this);

	Point3D current_pos;
	current_pos = GetLocalToWorld();
	Scalar time_slice = GetTimeSlice(till);

	const GameModel *model = GetGameModel();
	Check_Object(model);
	Vector3D new_speed = Vector3D::Identity;
	new_speed.y = model->takeOffSpeed;
	new_speed.x = new_speed.z = 0;
	Clamp(new_speed.y, 0.0f, model->takeOffSpeed);
	localSpaceVelocity.linearMotion = new_speed;
	worldSpaceVelocity.linearMotion = localSpaceVelocity.linearMotion;						// helicopters go straight up
//	worldSpaceVelocity.linearMotion.Multiply(new_speed, GetLocalToWorld());

	pitchDemand = 0.3f;

	Vector3D rotation_vector;
	rotation_vector.x = pitchDemand * model->pitchSpeed * time_slice;
	Clamp(rotation_vector.x, -model->pitchDegree, model->pitchDegree);
	rotation_vector.y = (yawDemand   * model->fullStopTurnRate);
	rotation_vector.z = (rollDemand   * model->fullStopTurnRate);
	localSpaceVelocity.angularMotion = rotation_vector;
	worldSpaceVelocity.angularMotion.Multiply(rotation_vector, GetLocalToWorld());

	//
	//---------------------------------------------------
	// Make a matrix representing the rotation this frame
	//---------------------------------------------------
	//
	Vector3D spin_axis;
	spin_axis.Multiply(worldSpaceVelocity.angularMotion, time_slice);
	UnitQuaternion spin;
	spin = spin_axis;

	//
	//-------------------------------------------------------------------------
	// Convert the EulerAngles into a matrix and create the new rotation matrix
	// through concatenation
	//-------------------------------------------------------------------------
	//
	UnitQuaternion rotation;
	rotation = initialLocalToParent;
	UnitQuaternion desire;
	desire.Multiply(rotation, spin);
	LinearMatrix4D new_position(true);
	new_position.BuildRotation(desire);

	Vector3D delta;
	delta.Multiply(worldSpaceVelocity.linearMotion, time_slice);
	Vector3D rotated_vector;
	rotated_vector.Multiply(delta, new_position);

	Point3D old_translation(initialLocalToParent);
	Point3D new_translation;
	new_translation.Add(
		old_translation,
		rotated_vector
	);
	new_position.BuildTranslation(new_translation);


	//
	//----------------------------------------------
	// Adjust position from network
	//----------------------------------------------
	//

	YawPitchRoll correction_angle;
	Point3D correction_position;
	
	GetNetworkAdjustment(time_slice, correction_position, correction_angle);

	YawPitchRoll new_rotation;
	new_rotation = desire;
	
	new_translation += correction_position;
	new_rotation.yaw.angle += correction_angle.yaw.angle;
	new_rotation.pitch.angle += correction_angle.pitch.angle;
	new_rotation.roll.angle += correction_angle.roll.angle;

	new_position = LinearMatrix4D::Identity;
	new_position.BuildRotation(desire);
	new_position.BuildTranslation(new_translation);


	//
	//----------------------------------------------
	// Set the new position
	//----------------------------------------------
	//


	SetNewLocalToParent(new_position);

	if(current_pos.y >= takeOffTargetPosition.y)
	{
		Check_Object(executionState);
		if(m_takeOffEffect.GetCurrent())
		{
			m_takeOffEffect.GetCurrent()->executionState->RequestState(Effect::ExecutionStateEngine::StoppingState);
		}
		if(m_takeOffGroundEffect.GetCurrent())
		{
			m_takeOffGroundEffect.GetCurrent()->executionState->RequestState(Effect::ExecutionStateEngine::StoppingState);
		}
		executionState->RequestState(ExecutionStateEngine::FlyingMotionState);
		m_OnGround = false;
	}

}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Helicopter::LandingMovementSimulation(Stuff::Time till)
{
	Check_Object(this);

	speedDemand = 0.0f;
	speedDemandKPH = 0.0f;
	currentSpeedMPS = 0;
	speedDemandMPS = 0;

	Point3D current_pos;
	current_pos = GetLocalToWorld();
	Scalar time_slice = GetTimeSlice(till);

	const GameModel *model = GetGameModel();
	Check_Object(model);
	Vector3D new_speed = Vector3D::Identity;
	new_speed.y = -model->takeOffSpeed;
	new_speed.x = new_speed.z = 0;
	Clamp(new_speed.y, -model->takeOffSpeed, 0.0f);
	localSpaceVelocity.linearMotion = new_speed;
	worldSpaceVelocity.linearMotion = localSpaceVelocity.linearMotion;						// helicopters come straight down
//	worldSpaceVelocity.linearMotion.Multiply(new_speed, GetLocalToWorld());

	if (pitchAngle < 0)
	{
		pitchAngle += (model->pitchSpeed*time_slice);
		Max_Clamp(pitchAngle, 0);
	}
	else if (pitchAngle > 0)
	{
		pitchAngle -= (model->pitchSpeed*time_slice);
		Min_Clamp(pitchAngle, 0);
	}

	Vector3D rotation_vector;
	rotation_vector.x = pitchDemand * model->pitchSpeed * time_slice;
	Clamp(rotation_vector.x, -model->pitchDegree, model->pitchDegree);
	rotation_vector.y = (yawDemand   * model->fullStopTurnRate);
	rotation_vector.z = (rollDemand   * model->fullStopTurnRate);
	localSpaceVelocity.angularMotion = rotation_vector;
	worldSpaceVelocity.angularMotion.Multiply(rotation_vector, GetLocalToWorld());

	//
	//---------------------------------------------------
	// Make a matrix representing the rotation this frame
	//---------------------------------------------------
	//
	Vector3D spin_axis;
	spin_axis.Multiply(worldSpaceVelocity.angularMotion, time_slice);
	UnitQuaternion spin;
	spin = spin_axis;

	//
	//-------------------------------------------------------------------------
	// Convert the EulerAngles into a matrix and create the new rotation matrix
	// through concatenation
	//-------------------------------------------------------------------------
	//
	UnitQuaternion rotation;
	rotation = initialLocalToParent;
	UnitQuaternion desire;
	desire.Multiply(rotation, spin);
	LinearMatrix4D new_position(true);
	new_position.BuildRotation(desire);

	Vector3D delta;
	delta.Multiply(worldSpaceVelocity.linearMotion, time_slice);
	Vector3D rotated_vector;
	rotated_vector.Multiply(delta, new_position);

	Point3D old_translation(initialLocalToParent);
	Point3D new_translation;
	new_translation.Add(old_translation,rotated_vector);
	new_position.BuildTranslation(new_translation);


	//
	//----------------------------------------------
	// Adjust position from network
	//----------------------------------------------
	//

	YawPitchRoll correction_angle;
	Point3D correction_position;
	
	GetNetworkAdjustment(time_slice, correction_position, correction_angle);

	YawPitchRoll new_rotation;
	new_rotation = desire;
	new_rotation.pitch = pitchAngle;
	
	new_translation += correction_position;
	new_rotation.yaw.angle += correction_angle.yaw.angle;
	new_rotation.pitch.angle += correction_angle.pitch.angle;
	new_rotation.roll.angle += correction_angle.roll.angle;

	new_position = LinearMatrix4D::Identity;
	new_position.BuildRotation(new_rotation);
	new_position.BuildTranslation(new_translation);

	pitchAngle = new_rotation.pitch;
	//
	//----------------------------------------------
	// Set the new position
	//----------------------------------------------
	//

	SetNewLocalToParent(new_position);

	if(current_pos.y <= landTargetPosition.y)
	{
		Check_Object(executionState);
		if(m_landingEffect.GetCurrent())
		{
			m_landingEffect.GetCurrent()->executionState->RequestState(Effect::ExecutionStateEngine::StoppingState);
		}
		if(m_landingGroundEffect.GetCurrent())
		{
			m_landingGroundEffect.GetCurrent()->executionState->RequestState(Effect::ExecutionStateEngine::StoppingState);
		}
		executionState->RequestState(ExecutionStateEngine::NeverExecuteState);
		m_OnGround = false;
		pitchDemand = 0;
		yawDemand = 0;
		rollDemand = 0;
		speedDemand = 0;
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::CreateTakeOffEffect()
{
	Check_Object(this);

	const GameModel *model = GetGameModel();
	Check_Object(model);

	if(model->takeOffResource != ResourceID::Null)
	{
		if(!m_takeOffEffect.GetCurrent())
		{
			Effect *effect = CreateLoopingEffect(model->takeOffResource, this, true);
			if(effect)
				m_takeOffEffect.Add(effect);
		}
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::CreateTakeOffGroundEffect(Point3D point)
{
	Check_Object(this);

	const GameModel *model = GetGameModel();
	Check_Object(model);

	if(model->takeOffGroundResource != ResourceID::Null)
	{
		if(!m_takeOffGroundEffect.GetCurrent())
		{
			Effect *effect = CreateLoopingEffect(model->takeOffGroundResource, point);
			if(effect)
				m_takeOffGroundEffect.Add(effect);
		}
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::CreateLandingEffect()
{
	Check_Object(this);

	const GameModel *model = GetGameModel();
	Check_Object(model);

	if(model->landingResource != ResourceID::Null)
	{
		if(!m_landingEffect.GetCurrent())
		{
			Effect *effect = CreateLoopingEffect(model->landingResource, this, true);
			if(effect)
				m_landingEffect.Add(effect);
		}
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Helicopter::CreateLandingGroundEffect(Point3D point)
{
	Check_Object(this);

	const GameModel *model = GetGameModel();
	Check_Object(model);

	if(model->landingGroundResource != ResourceID::Null)
	{
		if(!m_landingGroundEffect.GetCurrent())
		{
			Effect *effect = CreateLoopingEffect(model->landingGroundResource, point);
			if(effect)
				m_landingGroundEffect.Add(effect);
		}
	}
}

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

void
	Helicopter::TestInstance() const
{
	Verify(IsDerivedFrom(DefaultData));
}