#include "StdAfx.h"
#include "PlayerMovement.h"
#include "GameUtils.h"

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
CPlayerMovement::CPlayerMovement( 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_movement(movement),
	m_player(player),
	m_velocity(player.m_velocity),
	m_upVector(player.m_upVector),
	m_detachLadder(false),
	m_onGroundWBoots(player.m_stats.onGroundWBoots),
	m_jumped(player.m_stats.jumped),
	m_actions(player.m_actions),
	m_thrusters(0.0f),
	m_turnTarget(player.m_turnTarget),
	m_thrusterSprint(player.m_stats.thrusterSprint)
{
	// derive some values that will be useful later
	m_worldPos = player.GetEntity()->GetWorldPos();


}

void CPlayerMovement::Process()
{
	if (m_stats.flyMode)
		ProcessFlyMode();
	else if (m_stats.isOnLadder)
		ProcessLadder();
	else if (m_stats.inAir && m_stats.inZeroG)
		ProcessFlyingZeroG();
	else if (m_stats.waterLevel < 0.0f)
		ProcessSwimming();
	else
		ProcessOnGroundOrJumping();

	if (!m_player.GetLinkedEntity())
		ProcessTurning();
}

void CPlayerMovement::Commit( CPlayer& player )
{
	if (player.m_pAnimatedCharacter)
		player.m_pAnimatedCharacter->AddMovement( m_request );

	if (m_detachLadder)
		player.CreateScriptEvent("detachLadder",0);
	if (m_thrusters > 0.01f)
		player.CreateScriptEvent("thrusters",(m_actions & ACTION_SPRINT)?1:0);

	player.m_velocity = m_velocity;
	player.m_stats.jumped = m_jumped;
	player.m_stats.onGroundWBoots = m_onGroundWBoots;
	player.m_turnTarget = m_turnTarget;
	player.m_lastRequestedVelocity = m_request.movement;
	player.m_stats.thrusterSprint = m_thrusterSprint;
}

//-----------------------------------------------------------------------------------------------
// utility functions
//-----------------------------------------------------------------------------------------------
static Vec3 ProjectPointToLine(const Vec3 &point,const Vec3 &lineStart,const Vec3 &lineEnd)
{
	Vec3 line((lineEnd-lineStart).GetNormalized());
	return (lineStart + line * (((point-lineStart) * line)/(line*line)));
}

static f32 GetYaw( const Vec3& v0, const Vec3& v1 )
{
	Vec3 cross  = v0%v1;
	f32 sign = (cross.z<0) ? -1.0f : 1.0f;
	return atan2f( sign*cross.GetLength(), v0|v1 );
}

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
void CPlayerMovement::ProcessFlyMode()
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_GAME);

	Vec3 move = m_viewMtx * m_movement.desiredVelocity;

	float zMove(0.0f);
	if (m_actions & ACTION_JUMP)
		zMove += 1.0f;
	if (m_actions & ACTION_CROUCH)
		zMove -= 1.0f;

	move += m_viewMtx.GetColumn(2) * zMove;

	//cap the movement vector to max 1
	float moveModule(move.len());

	if (moveModule > 1.0f)
	{
		move /= moveModule;
	}

	move *= 30.0f;

	if (m_actions & ACTION_SPRINT)
		move *= 10.0f;

	m_request.type = eCMT_Fly;
	m_request.movement = move;
}

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
void CPlayerMovement::ProcessLadder()
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_GAME);

	Vec3 mypos = m_worldPos;
	Vec3 move(m_params.ladderTop - m_params.ladderBottom);
	float maxDeltaSq(move.len2());
	move.NormalizeSafe();

	float dirDot(move * m_viewMtx.GetColumn(1));

	Vec3 projected = ProjectPointToLine(mypos,m_params.ladderBottom,m_params.ladderTop);

	float topDist = (m_params.ladderTop-projected).len2();
	float bottomDist = (m_params.ladderBottom-projected).len2();

	/*if (topDist + bottomDist > maxDeltaSq+0.01f)
	{
	if (topDist<bottomDist)
	projected = m_params.ladderTop;
	else
	projected = m_params.ladderBottom;

	move.Set(0,0,0);
	}*/

	//FIXME:make it more solid
	if ((topDist < 0.5f && m_movement.desiredVelocity.y > 0.01f) || (bottomDist < 0.5f && m_movement.desiredVelocity.y < -0.01f))
		m_detachLadder = true;

	/*GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawSphere(m_params.ladderBottom,0.12f,ColorB(0,255,0,100) );
	GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawSphere(m_params.ladderTop,0.12f,ColorB(0,255,0,100) );
	GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawSphere(projected,0.12f,ColorB(255,0,0,100) );*/

	move *= m_movement.desiredVelocity.y;// * (dirDot>0.0f?1.0f:-1.0f) * min(1.0f,fabs(dirDot)*5);
	move += (projected-mypos) * 10.0f;
	//cap the movement vector to max 1
	float moveModule(move.len());

	if (moveModule > 1.0f)
		move /= moveModule;

	move *= m_player.GetStanceMaxSpeed(STANCE_STAND)*0.5f;

	if (m_actions & ACTION_SPRINT)
		move *= m_params.sprintMultiplier;

	m_request.type = eCMT_Fly;
	m_request.movement = move;

	m_velocity.Set(0,0,0);
}

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
void CPlayerMovement::ProcessFlyingZeroG()
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_GAME);

	//movement
	Vec3 move(0,0,0);//usually 0, except AIs

	if (m_actions & ACTION_JUMP)
		m_movement.desiredVelocity.z += 1.0f; 
	if (m_actions & ACTION_CROUCH)
		m_movement.desiredVelocity.z -= 1.0f;

	move += m_baseMtx.GetColumn(0) * m_movement.desiredVelocity.x;
	move += m_baseMtx.GetColumn(1) * m_movement.desiredVelocity.y;
	move += m_baseMtx.GetColumn(2) * m_movement.desiredVelocity.z;

	//cap the movement vector to max 1
	float moveModule(move.len());

	if (moveModule > 1.0f)
	{
		move /= moveModule;
	}

	//afterburner
	if (m_actions & ACTION_SPRINT)
	{
		float mult(1.0f);
		if (m_thrusterSprint>0.001f)
			mult += (m_params.afterburnerMultiplier-1.0f) * m_thrusterSprint;

		if (move.len2()>0.01f)
			m_thrusterSprint = max(0.0f,m_thrusterSprint - m_frameTime * 5.0f);

		move *= mult;
	}

	AdjustMovementForEnvironment( move );

	m_thrusters = moveModule;

	float inertiaMul(1.0f);
	if (move.len2()>0.1)
	{
		inertiaMul = min(1.0f,max(0.0f,1.0f - move * m_stats.velocityUnconstrained));
		inertiaMul = 1.0f + inertiaMul * 2.0f;
	}

	move *= m_params.thrusterImpulse * m_frameTime;
	move -= m_stats.velocityUnconstrained * min(1.0f,m_frameTime*m_params.thrusterStabilizeImpulse*inertiaMul);

	if (m_actions & ACTION_GRAVITYBOOTS && m_movement.desiredVelocity.z<0.1f)
	{
		IPhysicalEntity *pSkip = m_player.GetEntity()->GetPhysics();
		ray_hit hit;

		Vec3 ppos(m_player.GetEntity()->GetWorldPos());

		int rayFlags = (COLLISION_RAY_PIERCABILITY & rwi_pierceability_mask);
		float rayLen(10.0f);
		if (GetISystem()->GetIPhysicalWorld()->RayWorldIntersection(ppos, m_baseMtx.GetColumn(2) * -rayLen, ent_terrain|ent_static|ent_rigid, rayFlags, &hit, 1, &pSkip, 1) && m_player.IsMaterialBootable(hit.surface_idx))
		{
			Vec3 delta(hit.pt - ppos);
			float len = delta.len();
			float lenMult(min(1.0f,len/rayLen));
			lenMult = 1.0f - lenMult*lenMult;

			/*Vec3 goalPos = hit.pt + hit.n * (len-rayLen*lenMult);
			GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawLine(hit.pt, ColorB(255,255,0,100), goalPos, ColorB(255,0,255,100));
			delta = goalPos - ppos;
			GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawLine(ppos, ColorB(0,255,0,100), ppos + delta, ColorB(255,0,0,100));*/

			delta /= max(0.001f,len);

			move += delta * (10.0f * lenMult * m_frameTime);

			m_gBootsSpotNormal = hit.n;
		}
		else
			m_gBootsSpotNormal.Set(0,0,0);
	}
	//

	//FIXME:pretty hacky, needed when passing from zeroG to normalG
	m_velocity = m_stats.velocityUnconstrained;
	m_velocity -= m_velocity * m_baseMtx * Matrix33::CreateScale(Vec3(0,0,1));

	m_request.type = eCMT_Impulse;
	m_request.movement = move * m_stats.mass;
}

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
void CPlayerMovement::ProcessSwimming()
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_GAME);

	//process movement
	Vec3 move(0,0,0);
	float upDown(0);

	if (m_actions & ACTION_JUMP && m_stats.inWater>0.1f)
		upDown += 1.0f;
	if (m_actions & ACTION_CROUCH)
		upDown -= 1.0f;

	if (m_movement.desiredVelocity.x || m_movement.desiredVelocity.y || upDown)
	{	
		//FIXME: strafe and backward multipler dont work for AIs since they use the m_input.movementVector
		float strafeMul(m_params.strafeMultiplier);
		float backwardMul(1.0f);

		//going back?
		if (m_movement.desiredVelocity.y<0.0f)
			backwardMul = m_params.backwardMultiplier;

		move += m_viewMtx.GetColumn(0) * m_movement.desiredVelocity.x * strafeMul;
		move += m_viewMtx.GetColumn(1) * m_movement.desiredVelocity.y * backwardMul;
		move += m_viewMtx.GetColumn(2) * upDown;
	}

	//ai can set a custom sprint value, so dont cap the movement vector
	if (m_movement.sprint<=0.0f)
	{
		//cap the movement vector to max 1
		float moveModule(move.len());

		if (moveModule > 1.0f)
		{
			move /= moveModule;
		}

		if (m_actions & ACTION_SPRINT)
			move *= m_params.sprintMultiplier;
	}

	//player movement dont need the m_frameTime, its handled already in the physics
	move *= m_player.GetStanceMaxSpeed(STANCE_STAND)* 0.5f; // was current stance... probably not needed

	//only the Z component of the basematrix, handy with flat speeds,jump and gravity
	Matrix33 baseMtxZ(m_baseMtx * Matrix33::CreateScale(Vec3(0,0,1)));

	//apply movement
	Vec3 desiredVel;

	desiredVel = move;
	m_request.type = eCMT_Fly;

	//add some up/down motion
	desiredVel.z += min(1.0f,-m_stats.waterLevel);

	if (m_velocity.len2()<=0.0f)
		m_velocity = desiredVel;

	if (m_stats.inWater < 0.15f)
		m_velocity.z = m_stats.velocityUnconstrained.z * 0.9f;

	if (m_actions & ACTION_JUMP && m_stats.inWater > 0.5f && m_stats.waterLevel>-0.1f)
	{
		m_velocity.z = 7.0f;
	}	

	Interpolate(m_velocity,desiredVel,3.0f,m_frameTime);

	m_request.type = eCMT_Normal;
	m_request.movement = m_velocity;
}

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
void CPlayerMovement::ProcessOnGroundOrJumping()
{
	//process movement
	Vec3 move(0,0,0);

	if (m_movement.desiredVelocity.x || m_movement.desiredVelocity.y)
	{	
		float strafeMul = m_params.strafeMultiplier;
		float backwardMul = 1.0f;

		//going back?
		if (m_movement.desiredVelocity.y < -0.01f)
			backwardMul = m_params.backwardMultiplier;

		move += m_baseMtx.GetColumn(0) * m_movement.desiredVelocity.x * strafeMul * backwardMul;
		move += m_baseMtx.GetColumn(1) * m_movement.desiredVelocity.y * backwardMul;
	}

	//ai can set a custom sprint value, so dont cap the movement vector
	if (m_movement.sprint<=0.0f)
	{
		//cap the movement vector to max 1
		float moveModule(move.len());

		if (moveModule > 1.0f)
		{
			move /= moveModule;
		}

		//move *= m_animParams.runSpeed/GetStanceMaxSpeed(m_stance);

		if (m_actions & ACTION_SPRINT && m_player.GetStance() == STANCE_STAND)
			move *= m_params.sprintMultiplier;
		else if ((m_actions & ACTION_SPRINT)==0 && m_player.GetStance() == STANCE_STAND) // VS2 BIG HACK
			if (m_player.IsClient())
				if (ICVar * pWalkMul = GetISystem()->GetIConsole()->GetCVar("g_walkMultiplier"))
					move *= pWalkMul->GetFVal();
	}

	//player movement dont need the m_frameTime, its handled already in the physics
	move *= m_player.GetStanceMaxSpeed(m_player.GetStance());

	//when using gravity boots speed can be slowed down
	if (m_stats.inZeroG && m_actions & ACTION_GRAVITYBOOTS)
		move *= m_params.gravityBootsMultipler;

	AdjustMovementForEnvironment( move );

	//only the Z component of the basematrix, handy with flat speeds,jump and gravity
	Matrix33 baseMtxZ(m_baseMtx * Matrix33::CreateScale(Vec3(0,0,1)));

	m_request.type = eCMT_Normal;

	Vec3 jumpVec(0,0,0);
	//jump?
	//FIXME: I think in zeroG should be possible to jump to detach from the ground, for now its like this since its the easiest fix
	if (m_movement.jump)
	{
		if (m_stats.jumpLock < 0.001f && m_stats.onGround && (m_player.GetStance() != STANCE_PRONE))
		{
			//float verticalMult(max(0.75f,1.0f-min(1.0f,m_stats.flatSpeed / GetStanceMaxSpeed(STANCE_STAND) * m_params.sprintMultiplier)));
			//mul * gravity * jump height
			jumpVec += m_baseMtx.GetColumn(2) * cry_sqrtf( 2.0f * m_stats.gravity.len() * m_params.jumpHeight );// * verticalMult;

			m_request.type = eCMT_JumpAccumulate;

			move = m_stats.velocityUnconstrained * 1.3f;
			move -= move * baseMtxZ;

			//with gravity boots on, jumping will disable them for a bit.
			if (m_onGroundWBoots)
			{
				m_onGroundWBoots = -0.5f;
				jumpVec += m_baseMtx.GetColumn(2) * cry_sqrtf( 2.0f * 9.81f * m_params.jumpHeight );
			}
			else
				m_jumped = true;
		}
	}

	//apply movement
	Vec3 desiredVel;

	if (m_stats.onGround)
	{
		desiredVel = move;

		if (m_stats.jumpLock>0.001f)
			desiredVel *= 0.3f;
	}
	else//"passive" air control, the player can air control as long as it is to decelerate
	{
		//TODO:optimize performance
		Vec3 normVel(m_stats.velocityUnconstrained);
		normVel -= normVel * baseMtxZ;
		float flatVelLen(normVel.len());

		if (flatVelLen > 1.0f)
			normVel /= flatVelLen;

		move -= move * baseMtxZ;
		float moveLen(move.len());

		if (moveLen>1.0f)
			move /= moveLen;

		float dotDir(fabs(move * normVel));

		move *= min(1.0f,max(dotDir,0.1f));
		move += m_stats.velocityUnconstrained;
		move -= move * baseMtxZ;

		moveLen = move.len();

		if (moveLen > 1.5f && moveLen > flatVelLen)
		{		
			move = move / moveLen * flatVelLen;
		}

		desiredVel = move;
		m_request.type = eCMT_Fly;
	}

	//be sure desired velocity is flat to the ground
	desiredVel -= desiredVel * baseMtxZ;

	if (m_request.type == eCMT_Fly)
	{
		if (m_velocity.len2()<=0.0f)
			m_velocity = desiredVel;

		m_velocity -= m_velocity * baseMtxZ;
		Interpolate(m_velocity,desiredVel,m_params.inertia*3.5f,m_frameTime);

		m_request.movement = m_velocity + jumpVec + (m_stats.velocityUnconstrained * baseMtxZ);
	}
	else
	{
		m_request.movement = desiredVel + jumpVec;
		m_velocity.Set(0,0,0);
	}

	//VS2 sound hack
	if(m_stats.onGround && !m_player.GetNanoSuit().GetSoundIsPlaying(SPEED_SOUND) && m_player.GetNanoSuit().GetSlotValue(NANOSLOT_SPEED) > 30
			&& m_request.movement.len() > 8.0f && m_movement.desiredVelocity.len() > 0.5f)
	{
		CNanoSuit* pNano = (CNanoSuit*)&(m_player.GetNanoSuit());
		pNano->PlaySound(SPEED_SOUND);
	}
}

void CPlayerMovement::AdjustMovementForEnvironment( Vec3& move )
{
	//nanoSuit
	move *= 1.0f+m_player.GetNanoSuit().GetSlotValue(NANOSLOT_SPEED)*0.01f;

	//player is slowed down by carrying heavy objects (max. 25%)
	move *= m_player.GetMassFactor();

	//	//hacky, for tweaking
	//	move *= pWalkMul->GetFVal();
}

//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
void CPlayerMovement::ProcessTurning()
{
	if (m_stats.isRagDoll || (m_player.m_stats.isFrozen && !m_player.IsPlayer()))
		return;

	static const bool ROTATION_AFFECTS_THIRD_PERSON_MODEL = true;
	static const float ROTATION_SPEED = 23.3f;

	Quat entityRot = m_player.GetEntity()->GetRotation();
	Quat inverseEntityRot = entityRot.GetInverted();

	// TODO: figure out a way to unify this
	if (m_player.IsClient())
	{
		Vec3 right = m_turnTarget.GetColumn0();
		Vec3 up = m_upVector.GetNormalized();
		Vec3 forward = (up % right).GetNormalized();
		m_turnTarget = GetQuatFromMat33( Matrix33::CreateFromVectors(forward%up, forward, up) );

		if (ROTATION_AFFECTS_THIRD_PERSON_MODEL)
		{
			if (m_stats.inAir && m_stats.inZeroG)
			{
				m_turnTarget = GetQuatFromMat33(m_baseMtx);
				m_request.turn = inverseEntityRot * m_turnTarget;
			}
			else
			{
				/*float turn = m_movement.deltaAngles.z;
				m_turnTarget = Quat::CreateRotationZ(turn) * m_turnTarget;
				Quat turnQuat = inverseEntityRot * m_turnTarget;

				Vec3 epos(m_player.GetEntity()->GetWorldPos()+Vec3(1,0,1));
				GetISystem()->GetIRenderer()->GetIRenderAuxGeom()->DrawLine(epos, ColorB(255,255,0,100), epos + m_turnTarget.GetColumn1()*2.0f, ColorB(255,0,255,100));*/

				/*
				float turnAngle = cry_acosf( CLAMP( FORWARD_DIRECTION.Dot(turn * FORWARD_DIRECTION), -1.0f, 1.0f ) );
				float turnRatio = turnAngle / (ROTATION_SPEED * m_frameTime);
				if (turnRatio > 1)
					turnQuat = Quat::CreateSlerp( Quat::CreateIdentity(), turnQuat, 1.0f/turnRatio );
				*/

				//float white[4] = {1,1,1,1};
				//GetISystem()->GetIRenderer()->Draw2dLabel( 100, 50, 4, white, false, "%f %f %f %f", turnQuat.w, turnQuat.v.x, turnQuat.v.y, turnQuat.v.z );

				//m_request.turn = turnQuat;
				m_request.turn = inverseEntityRot * Quat(m_baseMtx);
			}
		}
	}
	else
	{
		/*Vec3 right = entityRot.GetColumn0();
		Vec3 up = m_upVector.GetNormalized();
		Vec3 forward = (up % right).GetNormalized();
		m_turnTarget = GetQuatFromMat33( Matrix33::CreateFromVectors(forward%up, forward, up) );
		float turn = m_movement.deltaAngles.z;
		if (fabsf(turn) > ROTATION_SPEED * m_frameTime)
			turn = ROTATION_SPEED * m_frameTime * (turn > 0.0f? 1.0f : -1.0f);*/
		m_request.turn = inverseEntityRot * Quat(m_baseMtx);//(m_turnTarget * Quat::CreateRotationZ(turn));
	}

	m_request.turn.Normalize();
}
