/*************************************************************************
Crytek Source File.
Copyright (C), Crytek Studios, 2001-2006.
-------------------------------------------------------------------------
$Id$
$DateTime$
Description: Implements a vtol vehicle movement

-------------------------------------------------------------------------
History:
- 13:03:2006: Created by Mathieu Pinard

*************************************************************************/
#include "StdAfx.h"
#include "Game.h"

#include "IMovementController.h"
#include "IVehicleSystem.h"
#include "VehicleMovementVTOL.h"
#include "ICryAnimation.h"

#include "IRenderAuxGeom.h"

//------------------------------------------------------------------------
bool CVehicleMovementVTOL::Init(IVehicle* pVehicle, const SmartScriptTable &table)
{
	if (!CVehicleMovementBase::Init(pVehicle, table))
		return false;

#define GetReqValue(name, var) \
	if (!table->GetValue(name, var)) \
	{ \
	CryLog("CVehicleMovementVTOL::Init() - failed to init due to missing <%s> parameter", name); \
	return false; \
	} \

	GetReqValue("liftForce", m_liftForce);
	GetReqValue("forwardForce", m_forwardForce);
	GetReqValue("strafeForce", m_strafeForce);
	GetReqValue("afterburnerForce", m_afterburnerForce);

	GetReqValue("pitchSpeed", m_pitchSpeed);
	GetReqValue("rollSpeed", m_rollSpeed);
	GetReqValue("yawSpeed", m_yawSpeed);

	GetReqValue("dampInertia", m_dampInertia);
	GetReqValue("dampRotation", m_dampRotation);

	m_pitchActionMul = 1.0f;

	m_maxSpeed = 45.f; // empirically determined

	return true;
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::Release()
{
	CVehicleMovementBase::Release();
	delete this;
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::Reset()
{
	CVehicleMovementBase::Reset();

	ResetActions();

	m_powerPID.Reset();
	m_powerPID.m_kP = 0.2f;
	m_powerPID.m_kD = 0.01f;
	m_powerPID.m_kI = 0.0001f;
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::ResetActions()
{
	m_liftAction = 0.0f;
	m_forwardAction = 0.0f;
	m_strafeAction = 0.0f;
	m_afterburnerAction = 0.0f;

	m_pitchAction = 0.0f;
	m_rollAction = 0.0f;
	m_yawAction = 0.0f;
}

//------------------------------------------------------------------------
bool CVehicleMovementVTOL::StartEngine(EntityId driverId)
{
	if (!CVehicleMovementBase::StartEngine(driverId))
		return false;

	m_powerPID.Reset();

  m_isEngineStarting = false;
	m_isEnginePowered = true;

	m_isInVerticalMode = true;

	return true;
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::StopEngine()
{
	CVehicleMovementBase::StopEngine();

	ResetActions();
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::OnEvent(EVehicleMovementEvent event, float value)
{
	CVehicleMovementBase::OnEvent(event, value);

	if (event == eVME_PlayerIsDrivingFP)
	{
		if (ISound* pRun = GetSound(eSID_Run))        
		{
			if (value == 1.f)
				pRun->SetVolume(50);
			else
				pRun->SetVolume(255);
		}
	}  
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::OnAction(const char *actionName, int activationMode, float value)
{
	CVehicleMovementBase::OnAction(actionName, activationMode, value);

	if (!strcmp("rotatepitch", actionName))
		m_pitchAction += value;
	else if (!strcmp("rotateyaw", actionName))
		m_yawAction += value;
	else if (!strcmp("moveforward", actionName))
	{
		if (activationMode == eAAM_OnPress)
			m_forwardAction += value;
		else if (activationMode == eAAM_OnRelease)
			m_forwardAction = min(0.0f, m_forwardAction);
	}
	else if (!strcmp("moveback", actionName))
	{
		if (activationMode == eAAM_OnPress)
			m_forwardAction -= value;
		else if (activationMode == eAAM_OnRelease)
			m_forwardAction = max(0.0f, m_forwardAction);
	}
	else if (!strcmp("moveleft", actionName))
	{
		if (activationMode == eAAM_OnPress)
			m_strafeAction -= value;
		else if (activationMode == eAAM_OnRelease)
			m_strafeAction = max(0.0f, m_strafeAction);
	}
	else if (!strcmp("moveright", actionName))
	{
		if (activationMode == eAAM_OnPress)
			m_strafeAction += value;
		else if (activationMode == eAAM_OnRelease)
			m_strafeAction = min(0.0f, m_strafeAction);
	}
	else if (!strcmp("leanleft", actionName))
	{
		/*
		if (activationMode == eAAM_OnPress)
		m_rollAction -= value;
		else if (activationMode == eAAM_OnRelease)
		m_rollAction = max(0.0f, m_rollAction);
		*/
		if (activationMode == eAAM_OnHold)
			m_rollAction -= value;
	}
	else if (!strcmp("leanright", actionName))
	{
		/*
		if (activationMode == eAAM_OnPress)
		m_rollAction += value;
		else if (activationMode == eAAM_OnRelease)
		m_rollAction = min(0.0f, m_rollAction);
		*/
		if (activationMode == eAAM_OnHold)
			m_rollAction += value;
	}
	else if (!strcmp("jump", actionName))
	{
		if (activationMode == eAAM_OnPress)
			m_liftAction += value;
		else if (activationMode == eAAM_OnRelease)
			m_liftAction = min(0.0f, m_liftAction);
	}
	else if (!strcmp("crouch", actionName))
	{
		if (activationMode == eAAM_OnPress)
			m_liftAction -= value;
		else if (activationMode == eAAM_OnRelease)
			m_liftAction = max(0.0f, m_liftAction);
	}
}

//===================================================================
// ProcessAI
// This treats the vtol as able to move in any horizontal direction
// by tilting in any direction. Yaw control is thus secondary. Throttle
// control is also secondary since it is adjusted to maintain or change
// the height, and the amount needed depends on the tilt.
//===================================================================
void CVehicleMovementVTOL::ProcessAI(const float deltaTime)
{
	// it's useless to progress further if the engine has yet to be turned on
	if (!m_isEnginePowered)
		return;

	m_movementAction.Clear();
	m_movementAction.isAI = true;

  // Our current state
	const Vec3 worldPos = m_pEntity->GetWorldPos();
	const Ang3 worldAngles = m_pEntity->GetWorldAngles();
  const Matrix33 worldMat(m_pEntity->GetRotation());

	pe_status_dynamics	status;
	IPhysicalEntity*	pPhysics = m_pEntity->GetPhysics();
	pPhysics->GetStatus(&status);
	const Vec3 currentVel = status.v;
  const Vec3 currentVel2D(currentVel.x, currentVel.y, 0.0f);

  // Our inputs
  const float desiredSpeed = m_aiRequest.HasDesiredSpeed() ? m_aiRequest.GetDesiredSpeed() : 0.0f;
  const Vec3 desiredMoveDir = m_aiRequest.HasMoveTarget() ? (m_aiRequest.GetMoveTarget() - worldPos).GetNormalizedSafe() : ZERO;
  const Vec3 desiredMoveDir2D = Vec3(desiredMoveDir.x, desiredMoveDir.y, 0.0f).GetNormalizedSafe();
	const Vec3 desiredVel = desiredMoveDir * desiredSpeed; 
	const Vec3 desiredVel2D(desiredVel.x, desiredVel.y, 0.0f);
  const Vec3 desiredLookDir = m_aiRequest.HasLookTarget() ? (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe() : desiredMoveDir;
  const Vec3 desiredLookDir2D = Vec3(desiredLookDir.x, desiredLookDir.y, 0.0f).GetNormalizedSafe();

  // Calculate the desired 2D velocity change
  Vec3 desiredVelChange2D = desiredVel2D - currentVel2D;

  static float anglePerVelDifference = 0.5f;
  float desiredTiltAngle = anglePerVelDifference * desiredVelChange2D.GetLength();
  static float maxAngle = DEG2RAD(60.0f);
  Limit(desiredTiltAngle, -maxAngle, maxAngle);

  if (desiredTiltAngle > 0.001f)
  {
    const Vec3 desiredWorldTiltAxis = Vec3(-desiredVelChange2D.y, desiredVelChange2D.x, 0.0f).GetNormalizedSafe();
    const Vec3 desiredLocalTiltAxis = worldMat.GetTransposed() * desiredWorldTiltAxis;

    float desiredPitch = desiredTiltAngle * desiredLocalTiltAxis.x;
    float desiredRoll = desiredTiltAngle * desiredLocalTiltAxis.y;

    static float scalePitch = 10.0f;
    static float scaleRoll  = 80.0f;

    m_movementAction.rotatePitch = scalePitch * (desiredPitch - worldAngles.x);
    m_movementAction.rotateRoll = scaleRoll * (desiredRoll - worldAngles.y);
  }

  float currentYawAngle = worldAngles.z;
  currentYawAngle += DEG2RAD(90.0f);
  float desiredYawAngle = atan2(desiredLookDir2D.y, desiredLookDir2D.x);
  float deltaYaw = (desiredYawAngle - currentYawAngle);
  if (deltaYaw > DEG2RAD(180.0f)) deltaYaw -= DEG2RAD(360.0f); else if (deltaYaw < -DEG2RAD(180.0f)) deltaYaw += DEG2RAD(360.0f);
  Limit(deltaYaw, -DEG2RAD(90.0f), DEG2RAD(90.0f));
  static float yawScale = -1.0f;
  m_movementAction.rotateYaw = yawScale * deltaYaw;

  static float timeScale = 5.0f;
  Vec3 guessedTargetPos = worldPos + desiredVel * timeScale;
	m_movementAction.power = m_powerPID.Update(worldPos.z, guessedTargetPos.z, -1.0f, 1.0f);
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::ProcessMovement(const float deltaTime)
{
	CVehicleMovementBase::ProcessMovement(deltaTime);

	assert(m_pEntity);

	m_pPhysics = m_pEntity->GetPhysics();
	assert(m_pPhysics);

	pe_action_impulse control;
	Vec3& impulse = control.impulse;
	Vec3& angImpulse = control.angImpulse;

	impulse.zero();
	angImpulse.zero();

	pe_status_dynamics dyn;
	m_pPhysics->GetStatus(&dyn);
	float& mass = dyn.mass;

	pe_simulation_params paramsSim;
	m_pPhysics->GetParams( &paramsSim );
	float gravity = abs(paramsSim.gravity.z);

	pe_status_dynamics	status;
	m_pPhysics->GetStatus(&status);
	Vec3&	velocity = status.v;
	Vec3& angularVelocity = status.w;

	Vec3 worldPos = m_pVehicle->GetEntity()->GetWorldPos();

	I3DEngine* p3DEngine = GetISystem()->GetI3DEngine();
	float altitude = worldPos.z - p3DEngine->GetTerrainElevation(worldPos.x, worldPos.y);

	Ang3 angles = m_pEntity->GetWorldAngles();
	angles.Snap180();

	float deltaX = 0.0f - angles.x;
	float deltaY = 0.0f - angles.y;

	Matrix33 tm;
	tm.SetRotationXYZ((angles));

	Vec3 forward = tm * Vec3(0.0f, 1.0f, 0.0f);
	Vec3 right = tm * Vec3(1.0f, 0.0f, 0.0f);
	Vec3 up = tm * Vec3(0.0f, 0.0f, 1.0f);

	if (m_isEnginePowered)
	{
		impulse += Vec3(0.0f, 0.0f, max(0.0f, up.z)) * (gravity * mass);
		impulse += Vec3(0.0f, 0.0f, up.z) * mass * m_liftForce * m_liftAction;
		impulse += forward * mass * m_forwardForce * m_forwardAction;
		//impulse += right * mass * m_strafeForce * m_strafeAction;

		angImpulse = right * mass * m_pitchSpeed * (-m_pitchAction * m_pitchActionMul);
		angImpulse += (up * mass * m_yawSpeed * -m_yawAction);
		angImpulse += (forward * mass * m_rollSpeed * m_rollAction);
		angImpulse += (right * deltaX * mass);

		float m_rollRelax = 2.5f;
		if (m_rollRelax > 0.0f)
			angImpulse += forward * deltaY * m_rollRelax * mass;

		impulse *= deltaTime;
		angImpulse *= deltaTime;

		if (m_dampInertia > 0.001f)
		{
			Vec3 fwdTemp(forward.x, forward.y, 0.0f);
			Vec3 dirTemp(dyn.v.x, dyn.v.y, 0.0f);
			Vec3 dirFixed(dirTemp);

			fwdTemp.normalize();
			dirTemp.normalize();

			float dotSide = 1.0f - fabs(fwdTemp.x*dirTemp.x + fwdTemp.y*dirTemp.y + fwdTemp.z*dirTemp.z);
			Vec3 energyAbsorb = dirFixed * mass * min(dotSide * m_dampInertia, 1.0f);
			float AbsorbModule = energyAbsorb.len();

			impulse -= fwdTemp * AbsorbModule;
			impulse -= energyAbsorb;
		}

		if (m_liftAction != 0.0f)
			impulse += Vec3(0.0f, 0.0f, -velocity.z) * mass * 0.0f;

		control.iSource = 3;
		m_pPhysics->Action(&control, 1);
	}	

	ICVar* profileVar = GetISystem()->GetIConsole()->GetCVar("v_profileMovement");
	if ((profileVar->GetIVal() == 1 && m_actor && m_actor->IsClient()) || profileVar->GetIVal() == 2)
	{
		IRenderer* pRenderer = GetISystem()->GetIRenderer();
		float color[4] = {1,1,1,1};

		Vec3 localAngles = m_pEntity->GetWorldAngles();

		pRenderer->Draw2dLabel(5.0f,   0.0f, 2.0f, color, false, "VTOL movement");
		pRenderer->Draw2dLabel(5.0f,  25.0f, 1.5f, color, false, "isInVerticalMode: %d", m_isInVerticalMode);
		pRenderer->Draw2dLabel(5.0f,  40.0f, 1.5f, color, false, "liftAction: %f", m_liftAction);
		pRenderer->Draw2dLabel(5.0f,  55.0f, 1.5f, color, false, "forwardAction: %f", m_forwardAction);
		pRenderer->Draw2dLabel(5.0f,  70.0f, 1.5f, color, false, "strafeAction: %f", m_strafeAction);

		pRenderer->Draw2dLabel(5.0f,  85.0f, 1.5f, color, false, "pitchAction: %f", m_pitchAction);
		pRenderer->Draw2dLabel(5.0f, 100.0f, 1.5f, color, false, "yawAction: %f", m_yawAction);
		pRenderer->Draw2dLabel(5.0f, 115.0f, 1.5f, color, false, "rollAction: %f", m_rollAction);

		pRenderer->Draw2dLabel(5.0f, 130.0f, 1.5f, color, false, "impulse: %f, %f, %f", impulse.x, impulse.y, impulse.z);
		pRenderer->Draw2dLabel(5.0f, 145.0f, 1.5f, color, false, "angImpulse: %f, %f, %f (%f)", angImpulse.x, angImpulse.y, angImpulse.z, angImpulse.GetLengthSquared());

		pRenderer->Draw2dLabel(5.0f, 175.0f, 1.5f, color, false, "angles: %f, %f, %f", RAD2DEG(angles.x), RAD2DEG(angles.y), RAD2DEG(angles.z));
		pRenderer->Draw2dLabel(5.0f, 190.0f, 1.5f, color, false, "velocity: %f, %f, %f (%f)", velocity.x, velocity.y, velocity.z, velocity.GetLength());
		pRenderer->Draw2dLabel(5.0f, 205.0f, 1.5f, color, false, "angular velocity: %f, %f, %f", angularVelocity.x, angularVelocity.y, angularVelocity.z);
		pRenderer->Draw2dLabel(5.0f, 220.0f, 1.5f, color, false, "deltaX: %f, deltaY: %f", RAD2DEG(deltaX), RAD2DEG(deltaY));
	}

	m_pitchAction = 0.0f;
	m_yawAction = 0.0f;
	m_rollAction = 0.0f;
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::ProcessActions(const float deltaTime)
{
	if (m_liftAction < 0.0f)
		m_liftAction /= 2.0f;
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::Update(const float deltaTime)
{
	CVehicleMovementBase::Update(deltaTime);

	IConsole* pConsole = GetISystem()->GetIConsole();
	if (ICVar* pVar = pConsole->GetCVar("v_invertPitchControl"))
	{
		if (pVar->GetIVal() == 1)
			m_pitchActionMul = -1.0f;
		else
			m_pitchActionMul = 1.0f;
	}

	UpdateSounds(deltaTime); 
}

//------------------------------------------------------------------------
void CVehicleMovementVTOL::UpdateSounds(const float deltaTime)
{ 
	if (m_isEnginePowered && !m_isEngineGoingOff)
	{ 
		float speedRatio = max(0.2f, min(1.f, m_statusDyn.v.len2() / sqr(m_maxSpeed)));

		// rpm_scale set to 1 after fadein (Joe)
		//m_rpmScale = 1.f;

		/*if (ISound* pRunSound = GetSound(eSID_Run))
		{      
		pRunSound->SetParam("rpm_scale", m_rpmScale, false);
		pRunSound->SetParam("speed", speedRatio, false);
		}    */
	}
}

//------------------------------------------------------------------------
bool CVehicleMovementVTOL::RequestMovement(CMovementRequest& movementRequest)
{
	if (!m_isEnginePowered)
		return false;

	if (movementRequest.HasLookTarget())
		m_aiRequest.SetLookTarget(movementRequest.GetLookTarget());

	if (movementRequest.HasMoveTarget())
		m_aiRequest.SetMoveTarget(movementRequest.GetMoveTarget());

	if (movementRequest.HasDesiredSpeed())
		m_aiRequest.SetDesiredSpeed(movementRequest.GetDesiredSpeed());

	return true;
}
