/*
===========================================================================

Doom 3 GPL Source Code
Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.

This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").

Doom 3 Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Doom 3 Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.

If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.

===========================================================================
*/

#include "sys/platform.h"
#include "Entity.h"
#include "Actor.h"

#include "physics/Physics_Monster.h"

CLASS_DECLARATION( idPhysics_Actor, idPhysics_Monster )
END_CLASS

const float OVERCLIP = 1.001f;

/*
=====================
idPhysics_Monster::CheckGround
=====================
*/
void idPhysics_Monster::CheckGround( monsterPState_t &state ) {
	trace_t groundTrace;
	idVec3 down;

	if ( gravityNormal == vec3_zero ) {
		state.onGround = false;
		groundEntityPtr = NULL;
		return;
	}

	down = state.origin + gravityNormal * CONTACT_EPSILON;
	gameLocal.clip.Translation( groundTrace, state.origin, down, clipModel, clipModel->GetAxis(), clipMask, self );

	if ( groundTrace.fraction == 1.0f ) {
		state.onGround = false;
		groundEntityPtr = NULL;
		return;
	}

	groundEntityPtr = gameLocal.entities[ groundTrace.c.entityNum ];

	if ( ( groundTrace.c.normal * -gravityNormal ) < minFloorCosine ) {
		state.onGround = false;
		return;
	}

	state.onGround = true;

	// let the entity know about the collision
	self->Collide( groundTrace, state.velocity );

	// apply impact to a non world floor entity
	if ( groundTrace.c.entityNum != ENTITYNUM_WORLD && groundEntityPtr.GetEntity() ) {
		impactInfo_t info;
		groundEntityPtr.GetEntity()->GetImpactInfo( self, groundTrace.c.id, groundTrace.c.point, &info );
		if ( info.invMass != 0.0f ) {
			groundEntityPtr.GetEntity()->ApplyImpulse( self, 0, groundTrace.c.point, state.velocity  / ( info.invMass * 10.0f ) );
		}
	}
}

/*
=====================
idPhysics_Monster::SlideMove
=====================
*/
monsterMoveResult_t idPhysics_Monster::SlideMove( idVec3 &start, idVec3 &velocity, const idVec3 &delta ) {
	int i;
	trace_t tr;
	idVec3 move;

	blockingEntity = NULL;
	move = delta;
	for( i = 0; i < 3; i++ ) {
		gameLocal.clip.Translation( tr, start, start + move, clipModel, clipModel->GetAxis(), clipMask, self );

		start = tr.endpos;

		if ( tr.fraction == 1.0f ) {
			if ( i > 0 ) {
				return MM_SLIDING;
			}
			return MM_OK;
		}

		if ( tr.c.entityNum != ENTITYNUM_NONE ) {
			blockingEntity = gameLocal.entities[ tr.c.entityNum ];
		}

		// clip the movement delta and velocity
		move.ProjectOntoPlane( tr.c.normal, OVERCLIP );
		velocity.ProjectOntoPlane( tr.c.normal, OVERCLIP );
	}

	return MM_BLOCKED;
}

/*
=====================
idPhysics_Monster::StepMove

  move start into the delta direction
  the velocity is clipped conform any collisions
=====================
*/
monsterMoveResult_t idPhysics_Monster::StepMove( idVec3 &start, idVec3 &velocity, const idVec3 &delta ) {
	trace_t tr;
	idVec3 up, down, noStepPos, noStepVel, stepPos, stepVel;
	monsterMoveResult_t result1, result2;
	float	stepdist;
	float	nostepdist;

	if ( delta == vec3_origin ) {
		return MM_OK;
	}

	// try to move without stepping up
	noStepPos = start;
	noStepVel = velocity;
	result1 = SlideMove( noStepPos, noStepVel, delta );
	if ( result1 == MM_OK ) {
		velocity = noStepVel;
		if ( gravityNormal == vec3_zero ) {
			start = noStepPos;
			return MM_OK;
		}

		// try to step down so that we walk down slopes and stairs at a normal rate
		down = noStepPos + gravityNormal * maxStepHeight;
		gameLocal.clip.Translation( tr, noStepPos, down, clipModel, clipModel->GetAxis(), clipMask, self );
		if ( tr.fraction < 1.0f ) {
			start = tr.endpos;
			return MM_STEPPED;
		} else {
			start = noStepPos;
			return MM_OK;
		}
	}

	if ( blockingEntity && blockingEntity->IsType( idActor::Type ) ) {
		// try to step down in case walking into an actor while going down steps
		down = noStepPos + gravityNormal * maxStepHeight;
		gameLocal.clip.Translation( tr, noStepPos, down, clipModel, clipModel->GetAxis(), clipMask, self );
		start = tr.endpos;
		velocity = noStepVel;
		return MM_BLOCKED;
	}

	if ( gravityNormal == vec3_zero ) {
		return result1;
	}

	// try to step up
	up = start - gravityNormal * maxStepHeight;
	gameLocal.clip.Translation( tr, start, up, clipModel, clipModel->GetAxis(), clipMask, self );
	if ( tr.fraction == 0.0f ) {
		start = noStepPos;
		velocity = noStepVel;
		return result1;
	}

	// try to move at the stepped up position
	stepPos = tr.endpos;
	stepVel = velocity;
	result2 = SlideMove( stepPos, stepVel, delta );
	if ( result2 == MM_BLOCKED ) {
		start = noStepPos;
		velocity = noStepVel;
		return result1;
	}

	// step down again
	down = stepPos + gravityNormal * maxStepHeight;
	gameLocal.clip.Translation( tr, stepPos, down, clipModel, clipModel->GetAxis(), clipMask, self );
	stepPos = tr.endpos;

	// if the move is further without stepping up, or the slope is too steap, don't step up
	nostepdist = ( noStepPos - start ).LengthSqr();
	stepdist = ( stepPos - start ).LengthSqr();
	if ( ( nostepdist >= stepdist ) || ( ( tr.c.normal * -gravityNormal ) < minFloorCosine ) ) {
		start = noStepPos;
		velocity = noStepVel;
		return MM_SLIDING;
	}

	start = stepPos;
	velocity = stepVel;

	return MM_STEPPED;
}

/*
================
idPhysics_Monster::Activate
================
*/
void idPhysics_Monster::Activate( void ) {
	current.atRest = -1;
	self->BecomeActive( TH_PHYSICS );
}

/*
================
idPhysics_Monster::Rest
================
*/
void idPhysics_Monster::Rest( void ) {
	current.atRest = gameLocal.time;
	current.velocity.Zero();
	self->BecomeInactive( TH_PHYSICS );
}

/*
================
idPhysics_Monster::PutToRest
================
*/
void idPhysics_Monster::PutToRest( void ) {
	Rest();
}

/*
================
idPhysics_Monster::idPhysics_Monster
================
*/
idPhysics_Monster::idPhysics_Monster( void ) {

	memset( &current, 0, sizeof( current ) );
	current.atRest = -1;
	saved = current;

	delta.Zero();
	maxStepHeight = 18.0f;
	minFloorCosine = 0.7f;
	moveResult = MM_OK;
	forceDeltaMove = false;
	fly = false;
	useVelocityMove = false;
	noImpact = false;
	blockingEntity = NULL;
}

/*
================
idPhysics_Monster_SavePState
================
*/
void idPhysics_Monster_SavePState( idSaveGame *savefile, const monsterPState_t &state ) {
	savefile->WriteVec3( state.origin );
	savefile->WriteVec3( state.velocity );
	savefile->WriteVec3( state.localOrigin );
	savefile->WriteVec3( state.pushVelocity );
	savefile->WriteBool( state.onGround );
	savefile->WriteInt( state.atRest );
}

/*
================
idPhysics_Monster_RestorePState
================
*/
void idPhysics_Monster_RestorePState( idRestoreGame *savefile, monsterPState_t &state ) {
	savefile->ReadVec3( state.origin );
	savefile->ReadVec3( state.velocity );
	savefile->ReadVec3( state.localOrigin );
	savefile->ReadVec3( state.pushVelocity );
	savefile->ReadBool( state.onGround );
	savefile->ReadInt( state.atRest );
}

/*
================
idPhysics_Monster::Save
================
*/
void idPhysics_Monster::Save( idSaveGame *savefile ) const {

	idPhysics_Monster_SavePState( savefile, current );
	idPhysics_Monster_SavePState( savefile, saved );

	savefile->WriteFloat( maxStepHeight );
	savefile->WriteFloat( minFloorCosine );
	savefile->WriteVec3( delta );

	savefile->WriteBool( forceDeltaMove );
	savefile->WriteBool( fly );
	savefile->WriteBool( useVelocityMove );
	savefile->WriteBool( noImpact );

	savefile->WriteInt( (int)moveResult );
	savefile->WriteObject( blockingEntity );
}

/*
================
idPhysics_Monster::Restore
================
*/
void idPhysics_Monster::Restore( idRestoreGame *savefile ) {

	idPhysics_Monster_RestorePState( savefile, current );
	idPhysics_Monster_RestorePState( savefile, saved );

	savefile->ReadFloat( maxStepHeight );
	savefile->ReadFloat( minFloorCosine );
	savefile->ReadVec3( delta );

	savefile->ReadBool( forceDeltaMove );
	savefile->ReadBool( fly );
	savefile->ReadBool( useVelocityMove );
	savefile->ReadBool( noImpact );

	savefile->ReadInt( (int &)moveResult );
	savefile->ReadObject( reinterpret_cast<idClass *&>( blockingEntity ) );
}

/*
================
idPhysics_Monster::SetDelta
================
*/
void idPhysics_Monster::SetDelta( const idVec3 &d ) {
	delta = d;
	if ( delta != vec3_origin ) {
		Activate();
	}
}

/*
================
idPhysics_Monster::SetMaxStepHeight
================
*/
void idPhysics_Monster::SetMaxStepHeight( const float newMaxStepHeight ) {
	maxStepHeight = newMaxStepHeight;
}

/*
================
idPhysics_Monster::GetMaxStepHeight
================
*/
float idPhysics_Monster::GetMaxStepHeight( void ) const {
	return maxStepHeight;
}

/*
================
idPhysics_Monster::OnGround
================
*/
bool idPhysics_Monster::OnGround( void ) const {
	return current.onGround;
}

/*
================
idPhysics_Monster::GetSlideMoveEntity
================
*/
idEntity *idPhysics_Monster::GetSlideMoveEntity( void ) const {
	return blockingEntity;
}

/*
================
idPhysics_Monster::GetMoveResult
================
*/
monsterMoveResult_t idPhysics_Monster::GetMoveResult( void ) const {
	return moveResult;
}

/*
================
idPhysics_Monster::ForceDeltaMove
================
*/
void idPhysics_Monster::ForceDeltaMove( bool force ) {
	forceDeltaMove = force;
}

/*
================
idPhysics_Monster::UseFlyMove
================
*/
void idPhysics_Monster::UseFlyMove( bool force ) {
	fly = force;
}

/*
================
idPhysics_Monster::UseVelocityMove
================
*/
void idPhysics_Monster::UseVelocityMove( bool force ) {
	useVelocityMove = force;
}

/*
================
idPhysics_Monster::EnableImpact
================
*/
void idPhysics_Monster::EnableImpact( void ) {
	noImpact = false;
}

/*
================
idPhysics_Monster::DisableImpact
================
*/
void idPhysics_Monster::DisableImpact( void ) {
	noImpact = true;
}

/*
================
idPhysics_Monster::Evaluate
================
*/
bool idPhysics_Monster::Evaluate( int timeStepMSec, int endTimeMSec ) {
	idVec3 masterOrigin, oldOrigin;
	idMat3 masterAxis;
	float timeStep;

	timeStep = MS2SEC( timeStepMSec );

	moveResult = MM_OK;
	blockingEntity = NULL;
	oldOrigin = current.origin;

	// if bound to a master
	if ( masterEntity ) {
		self->GetMasterPosition( masterOrigin, masterAxis );
		current.origin = masterOrigin + current.localOrigin * masterAxis;
		clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() );
		current.velocity = ( current.origin - oldOrigin ) / timeStep;
		masterDeltaYaw = masterYaw;
		masterYaw = masterAxis[0].ToYaw();
		masterDeltaYaw = masterYaw - masterDeltaYaw;
		return true;
	}

	// if the monster is at rest
	if ( current.atRest >= 0 ) {
		return false;
	}

	ActivateContactEntities();

	// move the monster velocity into the frame of a pusher
	current.velocity -= current.pushVelocity;

	clipModel->Unlink();

	// check if on the ground
	idPhysics_Monster::CheckGround( current );

	// if not on the ground or moving upwards
	float upspeed;
	if ( gravityNormal != vec3_zero ) {
		upspeed = -( current.velocity * gravityNormal );
	} else {
		upspeed = current.velocity.z;
	}
	if ( fly || ( !forceDeltaMove && ( !current.onGround || upspeed > 1.0f ) ) ) {
		if ( upspeed < 0.0f ) {
			moveResult = MM_FALLING;
		}
		else {
			current.onGround = false;
			moveResult = MM_OK;
		}
		delta = current.velocity * timeStep;
		if ( delta != vec3_origin ) {
			moveResult = idPhysics_Monster::SlideMove( current.origin, current.velocity, delta );
			delta.Zero();
		}

		if ( !fly ) {
			current.velocity += gravityVector * timeStep;
		}
	} else {
		if ( useVelocityMove ) {
			delta = current.velocity * timeStep;
		} else {
			current.velocity = delta / timeStep;
		}

		current.velocity -= ( current.velocity * gravityNormal ) * gravityNormal;

		if ( delta == vec3_origin ) {
			Rest();
		} else {
			// try moving into the desired direction
			moveResult = idPhysics_Monster::StepMove( current.origin, current.velocity, delta );
			delta.Zero();
		}
	}

	clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() );

	// get all the ground contacts
	EvaluateContacts();

	// move the monster velocity back into the world frame
	current.velocity += current.pushVelocity;
	current.pushVelocity.Zero();

	if ( IsOutsideWorld() ) {
		gameLocal.Warning( "clip model outside world bounds for entity '%s' at (%s)", self->name.c_str(), current.origin.ToString(0) );
		Rest();
	}

	return ( current.origin != oldOrigin );
}

/*
================
idPhysics_Monster::UpdateTime
================
*/
void idPhysics_Monster::UpdateTime( int endTimeMSec ) {
}

/*
================
idPhysics_Monster::GetTime
================
*/
int idPhysics_Monster::GetTime( void ) const {
	return gameLocal.time;
}

/*
================
idPhysics_Monster::GetImpactInfo
================
*/
void idPhysics_Monster::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const {
	info->invMass = invMass;
	info->invInertiaTensor.Zero();
	info->position.Zero();
	info->velocity = current.velocity;
}

/*
================
idPhysics_Monster::ApplyImpulse
================
*/
void idPhysics_Monster::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) {
	if ( noImpact ) {
		return;
	}
	current.velocity += impulse * invMass;
	Activate();
}

/*
================
idPhysics_Monster::IsAtRest
================
*/
bool idPhysics_Monster::IsAtRest( void ) const {
	return current.atRest >= 0;
}

/*
================
idPhysics_Monster::GetRestStartTime
================
*/
int idPhysics_Monster::GetRestStartTime( void ) const {
	return current.atRest;
}

/*
================
idPhysics_Monster::SaveState
================
*/
void idPhysics_Monster::SaveState( void ) {
	saved = current;
}

/*
================
idPhysics_Monster::RestoreState
================
*/
void idPhysics_Monster::RestoreState( void ) {
	current = saved;

	clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() );

	EvaluateContacts();
}

/*
================
idPhysics_Player::SetOrigin
================
*/
void idPhysics_Monster::SetOrigin( const idVec3 &newOrigin, int id ) {
	idVec3 masterOrigin;
	idMat3 masterAxis;

	current.localOrigin = newOrigin;
	if ( masterEntity ) {
		self->GetMasterPosition( masterOrigin, masterAxis );
		current.origin = masterOrigin + newOrigin * masterAxis;
	}
	else {
		current.origin = newOrigin;
	}
	clipModel->Link( gameLocal.clip, self, 0, newOrigin, clipModel->GetAxis() );
	Activate();
}

/*
================
idPhysics_Player::SetAxis
================
*/
void idPhysics_Monster::SetAxis( const idMat3 &newAxis, int id ) {
	clipModel->Link( gameLocal.clip, self, 0, clipModel->GetOrigin(), newAxis );
	Activate();
}

/*
================
idPhysics_Monster::Translate
================
*/
void idPhysics_Monster::Translate( const idVec3 &translation, int id ) {

	current.localOrigin += translation;
	current.origin += translation;
	clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() );
	Activate();
}

/*
================
idPhysics_Monster::Rotate
================
*/
void idPhysics_Monster::Rotate( const idRotation &rotation, int id ) {
	idVec3 masterOrigin;
	idMat3 masterAxis;

	current.origin *= rotation;
	if ( masterEntity ) {
		self->GetMasterPosition( masterOrigin, masterAxis );
		current.localOrigin = ( current.origin - masterOrigin ) * masterAxis.Transpose();
	}
	else {
		current.localOrigin = current.origin;
	}
	clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() * rotation.ToMat3() );
	Activate();
}

/*
================
idPhysics_Monster::SetLinearVelocity
================
*/
void idPhysics_Monster::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) {
	current.velocity = newLinearVelocity;
	Activate();
}

/*
================
idPhysics_Monster::GetLinearVelocity
================
*/
const idVec3 &idPhysics_Monster::GetLinearVelocity( int id ) const {
	return current.velocity;
}

/*
================
idPhysics_Monster::SetPushed
================
*/
void idPhysics_Monster::SetPushed( int deltaTime ) {
	// velocity with which the monster is pushed
	current.pushVelocity += ( current.origin - saved.origin ) / ( deltaTime * idMath::M_MS2SEC );
}

/*
================
idPhysics_Monster::GetPushedLinearVelocity
================
*/
const idVec3 &idPhysics_Monster::GetPushedLinearVelocity( const int id ) const {
	return current.pushVelocity;
}

/*
================
idPhysics_Monster::SetMaster

  the binding is never orientated
================
*/
void idPhysics_Monster::SetMaster( idEntity *master, const bool orientated ) {
	idVec3 masterOrigin;
	idMat3 masterAxis;

	if ( master ) {
		if ( !masterEntity ) {
			// transform from world space to master space
			self->GetMasterPosition( masterOrigin, masterAxis );
			current.localOrigin = ( current.origin - masterOrigin ) * masterAxis.Transpose();
			masterEntity = master;
			masterYaw = masterAxis[0].ToYaw();
		}
		ClearContacts();
	}
	else {
		if ( masterEntity ) {
			masterEntity = NULL;
			Activate();
		}
	}
}

const float	MONSTER_VELOCITY_MAX			= 4000;
const int	MONSTER_VELOCITY_TOTAL_BITS		= 16;
const int	MONSTER_VELOCITY_EXPONENT_BITS	= idMath::BitsForInteger( idMath::BitsForFloat( MONSTER_VELOCITY_MAX ) ) + 1;
const int	MONSTER_VELOCITY_MANTISSA_BITS	= MONSTER_VELOCITY_TOTAL_BITS - 1 - MONSTER_VELOCITY_EXPONENT_BITS;

/*
================
idPhysics_Monster::WriteToSnapshot
================
*/
void idPhysics_Monster::WriteToSnapshot( idBitMsgDelta &msg ) const {
	msg.WriteFloat( current.origin[0] );
	msg.WriteFloat( current.origin[1] );
	msg.WriteFloat( current.origin[2] );
	msg.WriteFloat( current.velocity[0], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	msg.WriteFloat( current.velocity[1], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	msg.WriteFloat( current.velocity[2], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( current.origin[0], current.localOrigin[0] );
	msg.WriteDeltaFloat( current.origin[1], current.localOrigin[1] );
	msg.WriteDeltaFloat( current.origin[2], current.localOrigin[2] );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	msg.WriteInt( current.atRest );
	msg.WriteBits( current.onGround, 1 );
}

/*
================
idPhysics_Monster::ReadFromSnapshot
================
*/
void idPhysics_Monster::ReadFromSnapshot( const idBitMsgDelta &msg ) {
	current.origin[0] = msg.ReadFloat();
	current.origin[1] = msg.ReadFloat();
	current.origin[2] = msg.ReadFloat();
	current.velocity[0] = msg.ReadFloat( MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	current.velocity[1] = msg.ReadFloat( MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	current.velocity[2] = msg.ReadFloat( MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	current.localOrigin[0] = msg.ReadDeltaFloat( current.origin[0] );
	current.localOrigin[1] = msg.ReadDeltaFloat( current.origin[1] );
	current.localOrigin[2] = msg.ReadDeltaFloat( current.origin[2] );
	current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS );
	current.atRest = msg.ReadLong();
	current.onGround = msg.ReadBits( 1 ) != 0;
}