#include "munga.h"
#pragma hdrstop

#include "camship.h"
#include "player.h"
#include "mission.h"
#include "cammppr.h"
#include "app.h"
#include "hostmgr.h"

//##########################################################################
//############################# CameraShip  ################################
//##########################################################################

//#############################################################################
// Shared Data Support
//
Derivation* CameraShip::GetClassDerivations()
{	static Derivation classDerivations(Mover::GetClassDerivations(), "CameraShip");
	return &classDerivations;
}


CameraShip::SharedData
	CameraShip::DefaultData(
		CameraShip::GetClassDerivations(),
		CameraShip::MessageHandlers,
		CameraShip::GetAttributeIndex(),
		CameraShip::StateCount,
		(Entity::MakeHandler)CameraShip::Make
	);

//#############################################################################
// Attribute Support
//

const CameraShip::IndexEntry
	CameraShip::AttributePointers[]=
{
	ATTRIBUTE_ENTRY(CameraShip, MapRange, mapRange),
	ATTRIBUTE_ENTRY(CameraShip, MapLinearPosition, mapLinearPosition),
	ATTRIBUTE_ENTRY(CameraShip, MapAngularPosition, mapAngularPosition)
};

CameraShip::AttributeIndexSet& CameraShip::GetAttributeIndex()
{
	static CameraShip::AttributeIndexSet attributeIndex(ELEMENTS(CameraShip::AttributePointers),
		CameraShip::AttributePointers,
		Mover::GetAttributeIndex()
	);
	return attributeIndex;
}

//#############################################################################
// Messaging Support
//
const Receiver::HandlerEntry
	CameraShip::MessageHandlerEntries[]=
{
	MESSAGE_ENTRY(CameraShip, Direction)
};

CameraShip::MessageHandlerSet
	CameraShip::MessageHandlers(
		ELEMENTS(CameraShip::MessageHandlerEntries),
		CameraShip::MessageHandlerEntries,
		Entity::GetMessageHandlers()
	);

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::DirectionMessageHandler(DirectionMessage *message)
{
	Check(this);
	Check(message);

	Check(application);
	HostManager *host = application->GetHostManager();
	Check(host);
	SetGoalEntity(host->GetEntityPointer(message->goalEntity));
	Check(goalEntity);
	mapLinearPosition = &goalEntity->localOrigin.linearPosition;
	focusOffset = message->focusOffset;
	SetSimulationState(DefaultState);
	timeOnCamera = message->timeOnCamera;
	lastSwitch = Now();
   lastSwitch -= timeOnCamera;
	Check_Fpu();
}

//#############################################################################
// ControlsMapper Support
//

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::SetMappingSubsystem(CameraControlsMapper *mapper)
{
	Check(this);
	Check(mapper);
	if(subsystemArray[ControlsMapperSubsystem])
	{
		Unregister_Object(subsystemArray[ControlsMapperSubsystem]);
		delete subsystemArray[ControlsMapperSubsystem];
	}
	subsystemArray[ControlsMapperSubsystem] = mapper;
	Check_Fpu();
}

//#############################################################################
// Simulation Support
//

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::DriveCamera(Scalar time_slice)
{
	Check(this);
	CameraControlsMapper* controls =
		Cast_Object(
			CameraControlsMapper*,
			GetSubsystem(CameraShip::ControlsMapperSubsystem)
		);
	Check(controls);

	//
	//-----------------------------------------------
	// If we are in positioning mode, move the camera
	//-----------------------------------------------
	//
	if (controls->driveCamera > 0)
	{
		ApplyControlledPanAndTilt(controls, time_slice);
		ApplyControlledCameraMotion(controls, time_slice);
		ApplyControlledCameraHeight(controls);
		currentCamera->SetLocalOrigin(localOrigin);
	}

	//
	//------------------------------------------------------------------------
	// Operate in camera mode, obeying the limits of the camera as established
	// by the clamps
	//------------------------------------------------------------------------
	//
	else
	{
		//
		//-------------------------
		// Stop any existing motion
		//-------------------------
		//
		worldLinearVelocity = Vector3D::Identity;

		//
		//-----------------------------------------------------------------------
		// Read the controls for panning the camera, and if the clamp override is
		// on, tell the camera to allow this orientation
		//-----------------------------------------------------------------------
		//
		ApplyControlledPanAndTilt(controls, time_slice);
		localToWorld = localOrigin;

		//
		//---------------------------------------------------------------------
		// Establish an aim point along negative Z and figure out where that is
		// in world space, then point the camera there.  The camera clamps will
		// automatically do their thing
		//---------------------------------------------------------------------
		//
		Point3D local_aim(0.0f, 0.0f, -1.0f);
		Point3D world_aim;
		world_aim.Multiply(local_aim, localToWorld);
		if (controls->slideOrClamp > 0)
		{
			currentCamera->AllowLookingAt(world_aim);
		}
		AimCameraAtPoint(world_aim);
	}
	localToWorld = localOrigin;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::ApplyControlledPanAndTilt(
		CameraControlsMapper *controls,
		Scalar  time_slice
	)
{
	Check(this);
	Check(controls);

	//
	//-----------------------------------------
	// Point the Camera in the direction of the
	// Joystick Position Calc Pitch and Yaw only
	//-----------------------------------------
	//
	Scalar  rotation_amount;
	if(controls->stickPosition.x < -0.02f)
	{

		rotation_amount = controls->stickPosition.x + 0.02f;
		rotation_amount *= rotation_amount;
		rotation_amount *= -1.0f;
	}
	else if (controls->stickPosition.x > 0.02f)
	{
		rotation_amount = controls->stickPosition.x - 0.02f;
		rotation_amount *= rotation_amount;
	}
	else
	{
		rotation_amount = 0.0f;
	}
	YawPitchRoll ypr;
	ypr = localOrigin.angularPosition;

	ypr.yaw += rotation_amount * PI * time_slice;
	ypr.pitch += controls->stickPosition.y * PI * 0.2f * time_slice;

	localOrigin.angularPosition = ypr;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::ApplyControlledCameraHeight(CameraControlsMapper *controls)
{
	Check(this);
	Check(controls);

	//
	//----------------------------------------------------
	// Raise and lower the cameraShip according to the pedals
	//----------------------------------------------------
	//
	localOrigin.linearPosition.y += controls->pedalsPosition;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::ApplyControlledCameraMotion(
		CameraControlsMapper *controls,
		Scalar  time_slice
	)
{
	Check(this);
	Check(controls);

	localAcceleration.linearMotion.z =
		-25.0f * controls->throttlePosition * time_slice;
	if (controls->reverseThrust > 0)
	{
		localAcceleration.linearMotion.z = -localAcceleration.linearMotion.z;
	}

	//
	//~~~~~~~~~~~~~~~~~~~
	// Apply Linear Drag
	//~~~~~~~~~~~~~~~~~~~
	//
	Vector3D        drag_force;

	drag_force.Multiply(
		localVelocity.linearMotion,
		positiveLinearDragCoefficients
	);

	Vector3D new_accel;
	new_accel.Subtract(
		localAcceleration.linearMotion,
		drag_force
	);

	localAcceleration.linearMotion = new_accel;
	//
	//~~~~~~~~~~~~~~~~~~~~~~~~
	// ApplyWorldAccelerations
	//~~~~~~~~~~~~~~~~~~~~~~~~
	localVelocity.linearMotion += localAcceleration.linearMotion;

	worldLinearVelocity.Multiply(
		localVelocity.linearMotion,
		localToWorld
	);
	localOrigin.linearPosition += worldLinearVelocity;

	Check_Fpu();
}

//#############################################################################
// Follow GoalEntity Support
//

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::FollowGoal(Scalar time_slice)
{
	Check(this);

	//
	//-----------------------------------------------
	// If no goalEntity assigned ask for one and wait
	//-----------------------------------------------
	//
	if (!goalEntity)
	{
		return;
	}
	Check(goalEntity);
	Point3D target;
	target.Multiply(focusOffset, goalEntity->localToWorld);

	//
	//------------------------------------------------------------------------
	// If time has not yet expired on this camera, keep with it if can see the
	// goal entity
	//------------------------------------------------------------------------
	//
	Check(currentCamera);
	if (
		lastPerformance - lastSwitch > timeOnCamera
		 || !currentCamera->CanCameraSee(target)
	)
	{
		CameraInstance *old_camera = currentCamera;
		GoToClosestCamera(target);
		if (!currentCamera)
		{
			currentCamera = old_camera;
		}
		Check(currentCamera);

		//
		//------------------------------------------------------------------------
		// If we need to switch cameras, have the new camera point directly at the
		// target
		//------------------------------------------------------------------------
		//
		if (currentCamera != old_camera)
		{
			AimCameraAtPoint(target);
			lastSwitch = lastPerformance;
		}
		else
		{
			goto Rotate_Camera;
		}
	}

	//
	//---------------------------------------------------
	// Otherwise, rotate the camera as needed and allowed
	//---------------------------------------------------
	//
	else
	{
Rotate_Camera:
		RotateCameraToPoint(time_slice, target);
	}

	//
	//------------------------
	// Set the tranform matrix
	//------------------------
	//
	localToWorld = localOrigin;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::GoToClosestCamera(const Point3D &point_3D)
{
	Check(this);
	currentCamera = cameraManager.FindClosestCamera(point_3D);
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::AimCameraAtPoint(const Point3D &point_3D)
{
	Check(this);
	Check(currentCamera);
	currentCamera->LookAt(&localOrigin, point_3D);
	localVelocity.angularMotion = Vector3D::Identity;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::RotateCameraToPoint(
		Scalar time_slice,
		const Point3D &point_3D
	)
{
	Check(this);
	Check(currentCamera);
	Check(&point_3D);

	//
	//---------------------------------------------------------
	// Calculate the rotation needed to point to the goalEntity
	//---------------------------------------------------------
	//
	Origin target;
	currentCamera->LookAt(&target, point_3D);
	YawPitchRoll new_yaw_pitch_roll;
	new_yaw_pitch_roll = target.angularPosition;

	Vector3D new_pos(new_yaw_pitch_roll.pitch, new_yaw_pitch_roll.yaw, 0.0f);
	YawPitchRoll    old_pos_euler;
	old_pos_euler = localOrigin.angularPosition;

	Vector3D old_pos(old_pos_euler.pitch, old_pos_euler.yaw, 0.0f);
	old_pos.x = Radian::Normalize(old_pos.x);
	old_pos.y = Radian::Normalize(old_pos.y);

	//
	// new - old position
	//
	if (new_pos.x-old_pos.x > PI) {
		new_pos.x -= TWO_PI;
	}
	else if (new_pos.x-old_pos.x < -PI) {
		new_pos.x += TWO_PI;
	}
	if (new_pos.y-old_pos.y > PI) {
		new_pos.y -= TWO_PI;
	}
	else if (new_pos.y-old_pos.y < -PI) {
		new_pos.y += TWO_PI;
	}
	Vector3D delta_rot;
	delta_rot.Subtract(
		new_pos,
		old_pos
	);

	//
	//-------------------
	// Apply Angular Drag
	//-------------------
	//
	Vector3D
		drag;

	drag.Multiply(
		localVelocity.angularMotion,
		angularDragCoefficients
	);

	//
	//~~~~~~~~~~~~~~~~~~~~~
	// Apply SpringConstant
	//~~~~~~~~~~~~~~~~~~~~~
	//
	Vector3D
		delta_rot_spring;
	delta_rot_spring.Multiply(
		delta_rot,
		elasticityCoefficient
	);

	//
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	// Calculate the new angular acceleration
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//
	localAcceleration.angularMotion.Subtract(
		delta_rot_spring,
		drag
	);

	Scalar halft_squared = 0.5 *(time_slice * time_slice);

	Vector3D
		accel_comp;
	accel_comp.Multiply(
		localAcceleration.angularMotion,
		halft_squared
	);

	//
	// velocity * delta time
	//
	Vector3D
		scaled_velocity;
	scaled_velocity.Multiply(
		localVelocity.angularMotion,
		time_slice
	);

	Vector3D
		tmp_position;
	tmp_position.Add(
		scaled_velocity,
		accel_comp
	);
	Vector3D
		clamped_accel;
	clamped_accel.Add(
		old_pos,
		tmp_position
	);
	//
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	// Apply Acceleration to angular Position
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//
	YawPitchRoll
		new_yawpitchroll(clamped_accel.y, clamped_accel.x, clamped_accel.z);

	LinearMatrix
		rot_matrix(LinearMatrix::Identity);
	rot_matrix = new_yawpitchroll;
	localOrigin.angularPosition = rot_matrix;

	//
	// Calc new Velocity
	//
	Vector3D
		 scaled_accel;
	scaled_accel.Multiply(
		localAcceleration.angularMotion,
		time_slice
	);
	Vector3D
		ang_vel;
	ang_vel.Add(
		localVelocity.angularMotion,
		scaled_accel
	);
	localVelocity.angularMotion = ang_vel;

	Check_Fpu();
}

//#############################################################################
// CameraInstanceManager Support
//

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::CreateCameraInstance()
{
	Check(this);
	Check(&cameraManager);
	currentCamera = cameraManager.CreateCameraInstance(localOrigin);
	DEBUG_STREAM << currentCamera->GetCameraName() << std::endl << std::flush;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::DeleteCameraInstance()
{
	Check(this);

	//
	//---------------------------------------------
	// If this is the last camera, don't delete it!
	//---------------------------------------------
	//
	Check(&cameraManager);
	CameraInstance *next_camera = cameraManager.IncrementIterator();
	if (next_camera == currentCamera)
	{
		return;
	}

	currentCamera = cameraManager.DeleteCameraInstance();
	Check(currentCamera);
	DEBUG_STREAM << currentCamera->GetCameraName() << std::endl << std::flush;
	localOrigin = currentCamera->GetLocalOrigin();
	localToWorld = localOrigin;
	Point3D local_aim(0.0f, 0.0f, -1.0f);
	Point3D world_aim;
	world_aim.Multiply(local_aim, localToWorld);
	AimCameraAtPoint(world_aim);
	localToWorld = localOrigin;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::IncrementCameraInstance()
{
	Check(this);
	Check(&cameraManager);
	currentCamera = cameraManager.IncrementIterator();
	Check(currentCamera);
	DEBUG_STREAM << currentCamera->GetCameraName() << std::endl << std::flush;
	localOrigin = currentCamera->GetLocalOrigin();
	localToWorld = localOrigin;
	Point3D local_aim(0.0f, 0.0f, -1.0f);
	Point3D world_aim;
	world_aim.Multiply(local_aim, localToWorld);
	AimCameraAtPoint(world_aim);
	localToWorld = localOrigin;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CameraShip::DecrementCameraInstance()
{
	Check(this);
	Check(&cameraManager);
	currentCamera = cameraManager.DecrementIterator();
	Check(currentCamera);
	DEBUG_STREAM << currentCamera->GetCameraName() << std::endl << std::flush;
	localOrigin = currentCamera->GetLocalOrigin();
	localToWorld = localOrigin;
	Point3D local_aim(0.0f, 0.0f, -1.0f);
	Point3D world_aim;
	world_aim.Multiply(local_aim, localToWorld);
	AimCameraAtPoint(world_aim);
	localToWorld = localOrigin;
	Check_Fpu();
}

//#############################################################################
// Motion Support Functions
//

//#############################################################################
// Construction and Destruction Support
//
CameraShip::CameraShip(
	CameraShip::MakeMessage *creation_message,
	CameraShip::SharedData &virtual_data
):
	Mover(creation_message, virtual_data)
{
	SetValidFlag();
	if (GetInstance() == ReplicantInstance)
	{
		return;
	}
	//
	//----------------------------
	// Initialize local variables
	//----------------------------
	//
	goalEntity = NULL;
	localOrigin = Origin::Identity;
	focusOffset = Vector3D::Identity;

	//
	//------------------------------
	// Initialize attributes
	//------------------------------
	//
	mapRange = 1000.0;                      // number of meters across display
	mapLinearPosition = NULL;       // force nav display to use object's lin. pos
	mapAngularPosition = NULL;      // force nav display to not turn or tilt
	//
	//------------------------------
	// Initialize the subsystemArray
	//------------------------------
	//
	subsystemCount = BasicSubsystemCount;
	Verify(!subsystemArray);
	subsystemArray = new (Subsystem (*[subsystemCount]));
	Register_Pointer(subsystemArray);
	subsystemArray[ControlsMapperSubsystem] = NULL;

	//
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	// Initialize the camera's viewpoint to the first camera
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//
	currentCamera = cameraManager.GetCurrent();
	if (!currentCamera)
	{
		CreateCameraInstance();
	}
	Check(currentCamera);
	localOrigin = currentCamera->GetLocalOrigin();
	localToWorld = localOrigin;
	Point3D local_aim(0.0f, 0.0f, -1.0f);
	Point3D world_aim;
	world_aim.Multiply(local_aim, localToWorld);
	AimCameraAtPoint(world_aim);
	localToWorld = localOrigin;
	lastSwitch = Now();
	timeOnCamera = 0.0f;

	SetPerformance(&CameraShip::FollowGoal);
	SetSimulationState(DefaultState);

	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
CameraShip*
	CameraShip::Make(MakeMessage *creation_message)
{
	return new CameraShip(creation_message);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
CameraShip::~CameraShip()
{
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	CameraShip::TestInstance() const
{
	return IsDerivedFrom(*GetClassDerivations());
}