/*************************************************************************
Crytek Source File.
Copyright (C), Crytek Studios, 2001-2005.
-------------------------------------------------------------------------
$Id$
$DateTime$
Description: Implements Hovercraft movement

-------------------------------------------------------------------------
History:
- 04:04:2005: Created by Michael Rauh

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

#include "IVehicleSystem.h"
#include "VehicleMovementHovercraft.h"

CVehicleMovementHovercraft::CVehicleMovementHovercraft()
: m_hoverHeight( 0.1f )
, m_numThrusters( 0 )
, m_thrustHeightCoeff( 1.1f )
, m_velMax( 20 )
, m_accel( 4 )
, m_turnRateMax( 1 )
, m_turnAccel( 1 )
, m_cornerForceCoeff( 1 )
, m_turnAccelCoeff( 2 )
, m_accelCoeff( 2 )
, m_stiffness( 1 )
, m_damping( 1 )
, m_bEngineAlwaysOn( false )
, m_thrusterTilt( 0 )
, m_dampLimitCoeff( 1 )
, m_pushTilt( 0 )
, m_pushOffset(ZERO)
, m_cornerTilt( 0 )
, m_cornerOffset(ZERO)
, m_stabRate(ZERO)
, m_turnDamping( 0 )
, m_massOffset(ZERO)
{
  m_Inertia.zero();
}


CVehicleMovementHovercraft::~CVehicleMovementHovercraft()
{}

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

//------------------------------------------------------------------------
void CVehicleMovementHovercraft::Physicalize()
{
	CVehicleMovementBase::Physicalize();

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

	float mass = m_pVehicle->GetMass();
	m_liftForce = Vec3(mass * paramsSim.gravity).GetLength();

	float fThrusterForce = m_liftForce / m_numThrusters;
	for (int i=0; i<m_numThrusters; ++i){
		m_vecThrusters[i]->maxForce = fThrusterForce;    
	}   
}


bool CVehicleMovementHovercraft::SetParams(const SmartScriptTable &table)
{  
  return true;
}

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

  m_pVehicle = pVehicle;
  m_pEntity = pVehicle->GetEntity();
  m_pPhysics = m_pEntity->GetPhysics();
   
  table->GetValue("hoverHeight", m_hoverHeight);  
  table->GetValue("thrustHeightCoeff", m_thrustHeightCoeff);   
  table->GetValue("stiffness", m_stiffness);     
  table->GetValue("damping", m_damping);
  table->GetValue("velMax", m_velMax);      
  table->GetValue("acceleration", m_accel);      
  table->GetValue("accelerationMultiplier", m_accelCoeff);     
  table->GetValue("pushOffset", m_pushOffset);     
  table->GetValue("pushTilt", m_pushTilt);     
  table->GetValue("turnRateMax", m_turnRateMax);
  table->GetValue("turnAccel", m_turnAccel);      
  table->GetValue("cornerForce", m_cornerForceCoeff);    
  table->GetValue("cornerOffset", m_cornerOffset);    
  table->GetValue("cornerTilt", m_cornerTilt);    
  table->GetValue("turnDamping", m_turnDamping); 
  table->GetValue("turnAccelMultiplier", m_turnAccelCoeff); 
  table->GetValue("bEngineAlwaysOn", m_bEngineAlwaysOn);
  table->GetValue("dampingLimit", m_dampLimitCoeff);
  table->GetValue("stabilizer", m_stabRate);
  
  m_maxSpeed = m_velMax;
 
  // init thrusters
  // put 1 on each corner of bbox and 1 in center. (equal mass distribution should be given)
  const Vec3 thrusterDir = Vec3(0,0,1);

	AABB bbox;
	if (IVehiclePart* massPart = pVehicle->GetPart("mass"))
	{
		bbox = massPart->GetLocalBounds();
	}
	else
	{
		GameWarning("[CVehicleMovementStdBoat]: initialization: No \"mass\" geometry found!");
		m_pEntity->GetLocalBounds(bbox);
	}
    
  // center
  Vec3 center = bbox.GetCenter();
  center.z = bbox.min.z;

  m_vecThrusters.push_back( new Thruster( center, thrusterDir ));  
  // min & max
  m_vecThrusters.push_back( new Thruster( bbox.min, thrusterDir ));  
  m_vecThrusters.push_back( new Thruster( Vec3( bbox.max.x, bbox.max.y, bbox.min.z ), thrusterDir ));
  // remaining corners
  m_vecThrusters.push_back( new Thruster( Vec3( bbox.min.x, bbox.max.y, bbox.min.z ), thrusterDir ));
  m_vecThrusters.push_back( new Thruster( Vec3( bbox.max.x, bbox.min.y, bbox.min.z ), thrusterDir ));
  // center along left and right side
  m_vecThrusters.push_back( new Thruster( Vec3( bbox.min.x, center.y, bbox.min.z ), thrusterDir ));
  m_vecThrusters.push_back( new Thruster( Vec3( bbox.max.x, center.y, bbox.min.z ), thrusterDir ));
  // middle along front and rear edge
  m_vecThrusters.push_back( new Thruster( Vec3( center.x, bbox.max.y, bbox.min.z ), thrusterDir ));
  m_vecThrusters.push_back( new Thruster( Vec3( center.x, bbox.min.y, bbox.min.z ), thrusterDir ));

  m_numThrusters = m_vecThrusters.size();

  // tilt thruster direction to outside   
  if (table->GetValue("thrusterTilt", m_thrusterTilt)){
    if (m_thrusterTilt >= 0 && m_thrusterTilt <= 90)
    {
      m_thrusterTilt = DEG2RAD(m_thrusterTilt);
      for (int i=0; i<m_numThrusters; ++i)
      {
        // tilt towards center.. rays are shot to -dir later
        if (m_vecThrusters[i]->pos == center){          
          continue;
        }
        Vec3 axis = Vec3(m_vecThrusters[i]->pos - center).Cross( thrusterDir );
        axis.Normalize();
        m_vecThrusters[i]->dir = Quat_tpl<float>::CreateRotationAA( m_thrusterTilt, axis ) * m_vecThrusters[i]->dir;
        m_vecThrusters[i]->tiltAngle = m_thrusterTilt;
      }
    }
  }

  // add thruster offset (so they don't touch ground intially)
  float thrusterBottomOffset = 0;
  if (table->GetValue("thrusterBottomOffset", thrusterBottomOffset))
  {    
    for (int i=0; i<m_numThrusters; ++i){
      m_vecThrusters[i]->pos.z += thrusterBottomOffset;
    }
    m_hoverHeight += thrusterBottomOffset; 
  }
   
  
  //pe_status_dynamics statusDyn;
  //m_pPhysics->GetStatus( &statusDyn );
	float mass = pVehicle->GetMass();
  
	// compute inertia [assumes box]
  float width = bbox.max.x - bbox.min.x;
  float length = bbox.max.y - bbox.min.y;
  float height = bbox.max.z - bbox.min.z;
  m_Inertia.x = mass * (sqr(length)+ sqr(height)) / 12;
  m_Inertia.y = mass * (sqr(width) + sqr(height)) / 12;
  m_Inertia.z = mass * (sqr(width) + sqr(length)) / 12;

  m_massOffset = bbox.GetCenter();
  CryLog("[Hovercraft movement]: got mass offset (%f, %f, %f)", m_massOffset.x, m_massOffset.y, m_massOffset.z);
  
  return true;
}

bool CVehicleMovementHovercraft::StartEngine(EntityId driverId)
{
	for (int i = 0; i < 10; i++)
	{
		//m_lastVals[i].vel = 0.0f;
		m_lastVals[i].dist= 0.0f;
	}		
  return CVehicleMovementBase::StartEngine(driverId);
}

void CVehicleMovementHovercraft::ProcessMovement(const float deltaTime)
{
  // moved to Update for testing
}

//------------------------------------------------------------------------
void CVehicleMovementHovercraft::Update(const float deltaTime)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

  if (!m_pEntity->IsActive())
   return;

  CVehicleMovementBase::Update(deltaTime);

  static const Vec3 pedalDir(0,1,0);  
  static const Vec3 thrusterForceDir(0,0,1); 
  static const Vec3 upwardDir(0,0,1);
  static const Vec3 vecZero(ZERO);  
  static const unsigned int objTypes = ent_terrain|ent_static|ent_rigid|ent_sleeping_rigid;
  //static const int flags = rwi_ignore_noncolliding|rwi_stop_at_pierceable;
  static const int flags = rwi_stop_at_pierceable|rwi_colltype_any|rwi_any_hit;  
  static const float minContacts = 0.3*m_numThrusters;
  static IPhysicalWorld* pWorld = GetISystem()->GetIPhysicalWorld();
    
  SmartScriptTable stateTable;  
  pe_action_impulse linearImp, angularImp, dampImp, stabImp;  
  pe_status_dynamics statusDyn;
  IPhysicalEntity *pSkip = m_pPhysics;
  ray_hit hit;
  int hits = 0;

	if (!m_isEnginePowered)
		return;

  float frameTime = min(deltaTime, 0.1f); // take care of high frametimes   
    
	if (!m_pPhysics->GetStatus( &statusDyn ))
	{
		CryError( "[VehicleMovementHovercraft]: '%s' is missing physics status", m_pEntity->GetName() );
		return;
	}
  
  const Matrix34& wTM = m_pEntity->GetWorldTM();

  Vec3 up = wTM.TransformVector(upwardDir); // check attitude
  if (up.z < 0)
    return;

  Matrix34 wTMInv = wTM.GetInvertedFast();
  Vec3 localVel = wTMInv.TransformVector( statusDyn.v );
  Vec3 localW = wTMInv.TransformVector( statusDyn.w );
  
  int contacts=0;

  if ( m_isEnginePowered || m_bEngineAlwaysOn )  
  {
    pe_action_impulse thrusterImp;
    Vec3 thrusterForceDirGlobal = wTM.TransformVector(thrusterForceDir);
    int i=0;

    std::vector<Thruster*>::const_iterator iter;
    for (iter=m_vecThrusters.begin(); iter!=m_vecThrusters.end(); ++iter)
    {                 
      Vec3 thrusterPos = wTM.TransformPoint( (*iter)->pos );
      Vec3 thrusterDir = wTM.TransformVector( (*iter)->dir );
   
      float cosAngle = cos((*iter)->tiltAngle);      
      bool bHit = false;
      if (hits = pWorld->RayWorldIntersection(thrusterPos, -1.1f*(m_hoverHeight/cosAngle)*thrusterDir, objTypes, flags, &hit, 1, &pSkip, 1))
      {
        bHit = true;
        // reset hit dist to vertical length
        hit.dist = hit.dist * cosAngle;
      }
      else 
      {        
        float delta = thrusterPos.z - GetISystem()->GetI3DEngine()->GetWaterLevel( &thrusterPos );
        if ( delta > 0 && delta < 1.1*m_hoverHeight ){
          bHit = true;
          hit.dist = delta;
        }
      }
      if (bHit)
      {
        ++contacts;
        
        if (hit.dist < 1.1f*m_hoverHeight) // max. height of spring/damper effect?
        {
          // positive force means upward push
          float basicForce = (*iter)->maxForce;
          float springForce = (m_hoverHeight-hit.dist)/m_hoverHeight * m_stiffness * basicForce;
                    
          // calc vel of thruster for damping 
          /*Vec3 angVel(ZERO);
          if (statusDyn.w.len2() > 1e-4){
            angVel = statusDyn.w.Cross( thrusterPos-wTM.GetTranslation() );
          }
          Vec3 pointVel = statusDyn.v + angVel;                */

          float relVel = (frameTime > 0) ? (hit.dist - m_lastVals[i].dist)/frameTime : 0;          
                    
          float dampForce = -1.f*CLAMP(relVel, -m_dampLimitCoeff, m_dampLimitCoeff) * m_damping * basicForce;
          if (abs(dampForce) < 0.05*basicForce)
            dampForce = 0;
        
          float force = basicForce + springForce + dampForce; 
                    
          thrusterImp.impulse = thrusterForceDirGlobal * force * frameTime;
          thrusterImp.point = thrusterPos; 
          //thrusterImp.iApplyTime = 2;
          m_pPhysics->Action(&thrusterImp);
         
          //m_lastVals[i].vel = pointVel.z;
          m_lastVals[i].dist= hit.dist;
          //lastVals[i].spring = springForce;
          //lastVals[i].damp = dampForce;                    
        }
      } 
      ++i;
    }
    // flight stabilization     
    if (contacts <= minContacts)
    {      
      Vec3 correction;
      for (int i=0; i<3; ++i){
        // apply stabRate as correction acceleration, but not more than current angular vel 
        correction[i] = -sgn(localW[i]) * min( abs(localW[i]), m_stabRate[i]*frameTime );
      }
      correction = correction.CompMul(m_Inertia);
      correction = wTM.TransformVector( correction );      
      stabImp.angImpulse = correction;
      stabImp.point = wTM.TransformPoint( m_massOffset );
      m_pPhysics->Action(&stabImp);
    }
  } 
  
    
  // apply driving force  
  float a = 0; 
  if (contacts > minContacts)  
  {   
    a = m_movementAction.power * m_accel;  
    if (sgn(a) * sgn(localVel.y) < 0){ // "braking"
      a *= m_accelCoeff;
    }
    if (abs(localVel.y) > m_velMax && sgn(a)*sgn(localVel.y)>0){ // check max vel
      a = 0;
    }
    Vec3 pushDir = pedalDir;
    if (a > 0){      
      // apply force downwards a bit for more realistic response  
      pushDir = Quat_tpl<float>::CreateRotationAA( DEG2RAD(m_pushTilt), Vec3(-1,0,0) ) * pushDir;
    }
    if (a != 0)
    {
      pushDir = wTM.TransformVector( pushDir );  

      linearImp.impulse = pushDir * statusDyn.mass * a * frameTime;       
      linearImp.point = m_pushOffset;
      linearImp.point.x += m_massOffset.x;
      linearImp.point.y += m_massOffset.y;
      linearImp.point = wTM.TransformPoint( linearImp.point );
      m_pPhysics->Action(&linearImp);
    }
  }  

  // apply steering 
  // (Momentum = alpha * I)  
  float turnAccel = 0;
  {        
    if (contacts > minContacts)
    {    
      Vec3 momentum(0,0,-1); // use momentum along -z to keep negative steering to the left      
      if (m_movementAction.rotateYaw != 0)
      {
        turnAccel = m_movementAction.rotateYaw * m_turnAccel * sgnnz(m_movementAction.power);
        
        // steering and current w in same direction?
        int sgnSteerW = sgn(m_movementAction.rotateYaw) * sgnnz(m_movementAction.power) * sgn(-localW.z);

        if (sgnSteerW < 0){ 
          turnAccel *= m_turnAccelCoeff; // "braking"      
        }
        else if (abs(localW.z) > m_turnRateMax){ 
          turnAccel = 0; // check max turn vel
        }
      }
      else 
      { 
        // if no steering, damp rotation                
        turnAccel = localW.z * m_turnDamping;
      }
      if (turnAccel != 0){
        momentum *= turnAccel * m_Inertia.z * frameTime;
        momentum = wTM.TransformVector( momentum );
        angularImp.angImpulse = momentum;
        angularImp.point = wTM.TransformPoint( m_massOffset );
        m_pPhysics->Action(&angularImp);
      }      
    }
  }

  // lateral force   
  if (localVel.x != 0 && contacts > minContacts)  
  {
    Vec3 cornerForce(0,0,0);
    cornerForce.x = -localVel.x * m_cornerForceCoeff * statusDyn.mass * frameTime;
    cornerForce = Quat_tpl<float>::CreateRotationAA( sgn(localVel.x)*DEG2RAD(m_cornerTilt), Vec3(0,1,0) ) * cornerForce;
    
    dampImp.impulse = wTM.TransformVector( cornerForce );

    dampImp.point = m_cornerOffset;    
    dampImp.point.x += m_massOffset.x;
    dampImp.point.y += m_massOffset.y;
    dampImp.point.x *= sgn(localVel.x);
    dampImp.point = wTM.TransformPoint( dampImp.point );
    
    m_pPhysics->Action(&dampImp);   
  }  

  //debugTable->SetValue("turnAccel", turnAccel );
  //debugTable->SetValue("accel", a );   
}




