#include "StdAfx.h"
#include "PlayerRotation.h"
#include "GameUtils.h"

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
CPlayerRotation::CPlayerRotation( const CPlayer& player, const SActorFrameMovementParams& movement, float m_frameTime ) : 
	m_frameTime(m_frameTime),
	m_params(player.m_params),
	m_stats(player.m_stats),
	m_viewMtx(player.m_viewMtx),
	m_baseMtx(player.m_baseMtx),
	m_viewMtxFinal(player.m_viewMtxFinal),
	m_player(player),
	m_viewRoll(player.m_viewRoll),
	m_upVector(player.m_upVector),
	m_viewAnglesOffset(player.m_viewAnglesOffset),
	m_desiredLean(movement.desiredLean),
	m_leanAmount(player.m_stats.leanAmount),
	m_actions(player.m_actions),	
  m_freezeDelta(0.f),
	m_absRoll(0.0f),
	m_angularImpulseTime(m_stats.angularImpulseTime),
	m_angularImpulse(m_stats.angularImpulse),
	m_angularImpulseDelta(m_stats.angularImpulse),
	m_angularVel(m_stats.angularVel)
{
	m_deltaAngles = movement.deltaAngles;
}

void CPlayerRotation::Process()
{
	ProcessAngularImpulses();
	ProcessFreezeShaking();

	if (m_stats.inAir && m_stats.inZeroG)
		ProcessFlyingZeroG();
	else
	{
		ProcessNormalRoll();
		ClampAngles();
		ProcessNormal();
	}

	m_viewMtxFinal = m_viewMtx * Matrix33::CreateRotationXYZ(m_player.m_viewAnglesOffset);

	ProcessLean();
}

void CPlayerRotation::Commit( CPlayer& player )
{
	player.m_baseMtx = m_baseMtx;
	player.m_viewMtx = m_viewMtx;
	player.m_viewMtxFinal = m_viewMtxFinal;
	player.m_viewRoll = m_viewRoll;
	player.m_upVector = m_upVector;
	player.m_viewAnglesOffset = m_viewAnglesOffset;
	player.m_stats.leanAmount = m_leanAmount;	
	player.m_stats.angularImpulseTime = m_angularImpulseTime;
	player.m_stats.angularImpulse = m_angularImpulse;
	player.m_stats.angularVel = m_angularVel;

	if (m_absRoll > 0.01f)
		player.CreateScriptEvent("thrusters",m_absRoll);

  CommitFreezeShaking(player);
 
}

void CPlayerRotation::GetStanceAngleLimits(float & minAngle,float & maxAngle)
{
	EStance stance = m_player.GetStance();

	minAngle=-(stance != STANCE_STAND?55.0f:80.0f) * gf_PI/180.0f;
	maxAngle=(stance == STANCE_PRONE?60.0f:89.0f) * gf_PI/180.0f;
}

void CPlayerRotation::ProcessFreezeShaking()
{ 
	// unfreeze with mouse shaking
	if (m_player.m_stats.isFrozen && m_player.IsPlayer())
	{ 
    static ICVar* pVarMult = GetISystem()->GetIConsole()->GetCVar("cl_frozenMult");
		m_freezeDelta = (abs(m_deltaAngles.z)+abs(m_deltaAngles.x)) * pVarMult->GetFVal(); 

    // remove viewSensitity (it's tedious when in zoom mode..) and mass factor
    m_freezeDelta /= m_player.m_params.viewSensitivity; 
    m_freezeDelta /= m_player.GetMassFactor();
    
		if (m_freezeDelta>0 && m_player.GetNanoSuit().IsActive())
		{
			float strength = m_player.GetNanoSuit().GetSlotValue(NANOSLOT_STRENGTH);
			strength = max(-0.75f, (strength-50)/50.f) * m_freezeDelta;
			m_freezeDelta += strength;

      static ICVar* pDebugVar = GetISystem()->GetIConsole()->GetCVar("cl_debugFreezeShake");
      if (pDebugVar->GetIVal())
      {
			  static float color[] = {1,1,1,1};
			  GetISystem()->GetIRenderer()->Draw2dLabel(100,150,1.5,color,false,"freezeDelta: %f (suit mod: %f)", m_freezeDelta, strength);
      }
		}
	}
}

void CPlayerRotation::CommitFreezeShaking( CPlayer& player )
{ 
  if (m_freezeDelta>0)
  {
    float prevAmt = player.GetFrozenAmount();  
    player.m_frozenAmount -= m_freezeDelta;

    static ICVar* pVar = GetISystem()->GetIConsole()->GetCVar("cl_frozenSoundDelta");
    
    if (m_freezeDelta > pVar->GetFVal() || player.GetFrozenAmount()!=prevAmt)
      player.CreateScriptEvent("unfreeze_shake", player.GetFrozenAmount()!=prevAmt ? m_freezeDelta : 0);    
  }
}

void CPlayerRotation::ProcessFlyingZeroG()
{
	//thats necessary when passing from groundG to normalG
	m_baseMtx = m_viewMtx;

	Ang3 desiredAngVel(m_deltaAngles.x,m_deltaAngles.y,m_deltaAngles.z);

	//view recoil in zeroG cause the player to rotate
	desiredAngVel.x += m_viewAnglesOffset.x * 0.1f;
	desiredAngVel.z += m_viewAnglesOffset.z * 0.1f;

	//so once used reset it.
	m_viewAnglesOffset.Set(0,0,0);

	//gyroscope: the gyroscope just apply the right roll speed to compensate the rotation, that way effects like
	//propulsion particles and such can be done easily just by using the angularVel
	float rotInertia(6.6f);

	if (m_actions & ACTION_GRAVITYBOOTS && m_stats.gBootsSpotNormal.len2()>0.01f)
	{
		Vec3 vRef(m_baseMtx.GetInverted() * m_stats.gBootsSpotNormal);
		Ang3 alignAngle(0,0,0);
		alignAngle.y = cry_atan2f(vRef.x,vRef.z);
		alignAngle.x = cry_atan2f(vRef.y,vRef.z);

		desiredAngVel.y += alignAngle.y * 0.05f;
		desiredAngVel.x -= alignAngle.x * 0.05f;
	}

	if (m_actions & ACTION_GYROSCOPE && desiredAngVel.y==0)
	{
		Vec3 vRef(m_baseMtx.GetInverted() * m_stats.gyroScopeNormal);
		Ang3 alignAngle(0,0,0);
		alignAngle.y = cry_atan2f(vRef.x,vRef.z);

		desiredAngVel.y += alignAngle.y * 0.05f;

		//rotInertia = 3.0f;
	}

	m_absRoll = fabs(desiredAngVel.y);

	Interpolate(m_angularVel,desiredAngVel,rotInertia,m_frameTime);
	Ang3 finalAngle(m_angularVel + m_angularImpulseDelta);

	Quat finalRot(Quat(m_baseMtx) * Quat::CreateRotationZ(finalAngle.z) * Quat::CreateRotationX(finalAngle.x) * Quat::CreateRotationY(finalAngle.y));
	finalRot.Normalize();
	m_baseMtx = Matrix33(finalRot);

	m_viewMtx = m_baseMtx;
	m_viewRoll = 0;
	m_upVector = m_baseMtx.GetColumn(2);
}

void CPlayerRotation::ProcessNormalRoll()
{
	//apply lean/roll
	float rollAngleGoal(0);
	float speed2(m_stats.velocity.len2());

	//add some leaning when strafing
	if (speed2 > 0.01f && m_stats.inAir)
	{
		Vec3 velVec(m_stats.velocity);
		float maxSpeed = m_player.GetStanceMaxSpeed(STANCE_STAND);
		if (maxSpeed > 0.01f)
			velVec /= maxSpeed;

		float dotSide(m_viewMtx.GetColumn(0) * velVec);

		rollAngleGoal -= dotSide * 1.5f * gf_PI/180.0f;
	}

	rollAngleGoal += m_leanAmount * m_params.leanAngle * gf_PI/180.0f;
	Interpolate(m_viewRoll,rollAngleGoal,5.0f,m_frameTime);

	Interpolate(m_angularVel,Ang3(0,0,0),6.6f,m_frameTime);
	m_deltaAngles += m_stats.angularVel + m_angularImpulseDelta;
}

void CPlayerRotation::ProcessAngularImpulses()
{
	//update angular impulse
	if (m_angularImpulseTime>0.001f)
	{
		m_angularImpulse *= min(m_angularImpulseTime / m_stats.angularImpulseTimeMax,1.0f);
		m_angularImpulseTime -= m_frameTime;
	}
	else if (m_stats.angularImpulseDeceleration>0.001f)
	{
		Interpolate(m_angularImpulse,ZERO,m_stats.angularImpulseDeceleration, m_frameTime);
	}
	m_angularImpulseDelta -= m_angularImpulse;
}

void CPlayerRotation::ClampAngles()
{
	{
		//cap up/down looking
		float minAngle,maxAngle;
		GetStanceAngleLimits(minAngle,maxAngle);

		float currentViewPitch=GetLocalPitch();
		float newPitch = currentViewPitch + m_deltaAngles.x;
		if (newPitch < minAngle)
			newPitch = minAngle;
		else if (newPitch > maxAngle)
			newPitch = maxAngle;
		m_deltaAngles.x = newPitch - currentViewPitch;
	}

	{
		//further limit the view if necessary
		float limitV = m_params.vLimitRangeV;
		float limitH = m_params.vLimitRangeH;

		if (m_player.m_stats.isFrozen)
		{ 
      float clampMin = GetISystem()->GetIConsole()->GetCVar("cl_frozenAngleMin")->GetFVal();
      float clampMax = GetISystem()->GetIConsole()->GetCVar("cl_frozenAngleMax")->GetFVal();
      float frozenLimit = DEG2RAD(clampMin + (clampMax-clampMin)*(1.f-m_player.GetFrozenAmount()));    

			if (limitV == 0 || limitV>frozenLimit)
				limitV = frozenLimit;
			if (limitH == 0 || limitH>frozenLimit)
				limitH = frozenLimit;

      if (GetISystem()->GetIConsole()->GetCVar("cl_debugFreezeShake")->GetIVal())
      {
        static float color[] = {1,1,1,1};    
        GetISystem()->GetIRenderer()->Draw2dLabel(100,200,1.5,color,false,"limit: %f", RAD2DEG(frozenLimit));
      }
		}

		if (limitH+limitV && m_params.vLimitDir.len2()>0.1f)
		{
			//A matrix is built around the view limit, and then the player view angles are checked with it.
			//Later, if necessary the upVector could be made customizable.
			Vec3 forward(m_params.vLimitDir);
			Vec3 up(m_baseMtx.GetColumn(2));
			Vec3 right(-(up % forward));

			Matrix33 limitMtx;
			limitMtx.SetFromVectors(right,up%right,up);
			//GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(0,0,255,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(0), ColorB(0,0,255,255));
			//GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(0,255,0,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(1), ColorB(0,255,0,255));
			//GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(255,0,0,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(2), ColorB(255,0,0,255));
			limitMtx.Invert();

			Vec3 localDir(limitMtx * m_viewMtx.GetColumn(1));
			Ang3 limit;

			if (limitV)
			{
				limit.x = asin(localDir.z) + m_deltaAngles.x;

				float deltaX(limitV - fabs(limit.x));
				if (deltaX < 0.0f)
					m_deltaAngles.x += deltaX*(limit.x>0.0f?1.0f:-1.0f);
			}

			if (limitH)
			{
				limit.z = cry_atan2f(-localDir.x,localDir.y) + m_deltaAngles.z;

				float deltaZ(limitH - fabs(limit.z));
				if (deltaZ < 0.0f)
					m_deltaAngles.z += deltaZ*(limit.z>0.0f?1.0f:-1.0f);		
			}
		}
	}
}

void CPlayerRotation::ProcessNormal()
{
	m_upVector = Vec3::CreateSlerp(m_upVector,m_stats.upVector,5.0f*m_frameTime);

	//create a matrix perpendicular to the ground
	Vec3 up = m_upVector;
	Vec3 right = m_baseMtx.GetColumn(0);
	Vec3 forward = (up % right).GetNormalized();

	m_baseMtx.SetFromVectors(forward % up,forward,up);

	m_baseMtx = m_baseMtx *	Matrix33::CreateRotationZ(m_deltaAngles.z);
	m_viewMtx = m_baseMtx * 
		Matrix33::CreateRotationX(GetLocalPitch() + m_deltaAngles.x) * 
		Matrix33::CreateRotationY(m_viewRoll);
}

void CPlayerRotation::ProcessLean()
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_GAME);

	float leanAmt(0.0f);

	if (!m_stats.inZeroG || m_stats.inAir<0.1f)
	{
		leanAmt = m_desiredLean;
	}

	EStance stance = m_player.GetStance();
	if (stance == STANCE_PRONE)
		leanAmt *= 0.65f;

	m_leanAmount = leanAmt;
	//Interpolate(m_leanAmount,leanAmt,10.0f,m_frameTime);

	//check if its possible
	if (m_leanAmount*m_leanAmount > 0.01f)
	{
		Vec3 headPos(m_player.GetEntity()->GetWorldPos() + m_baseMtx * m_player.GetStanceViewOffset(stance,false));
		Vec3 newPos(m_player.GetEntity()->GetWorldPos() + m_baseMtx * m_player.GetStanceViewOffset(stance));

		ray_hit hit;
		int rayFlags(COLLISION_RAY_PIERCABILITY & rwi_pierceability_mask);
		IPhysicalEntity *pSkip(m_player.GetEntity()->GetPhysics());

		float distMult(2.0f);

		if (GetISystem()->GetIPhysicalWorld()->RayWorldIntersection(headPos + m_viewMtx.GetColumn(1) * 0.25f, (newPos - headPos)*distMult, ent_terrain|ent_static|ent_rigid, rayFlags, &hit, 1, &pSkip, 1))
		{
			float dist((headPos - newPos).len2() * distMult);
			m_leanAmount *= ((hit.pt - headPos).len2() / dist) / distMult;
		}
	}
}
