/*************************************************************************
Crytek Source File.
Copyright (C), Crytek Studios, 2001-2005.
-------------------------------------------------------------------------
$Id$
$DateTime$
Description: Implements a standard wheel based vehicle movements

-------------------------------------------------------------------------
History:
- 04:04:2005: Created by Mathieu Pinard

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

#include "VehicleMovementStdWheeled.h"

#include "IVehicleSystem.h"
#include "Network/NetActionSync.h"
#include <IAgent.h>
#include "GameUtils.h"
#include "IGameTokens.h"
#include "VehicleSystem/Vehicle.h"
#include "VehicleSystem/VehiclePartSubPartWheel.h"


//------------------------------------------------------------------------
CVehicleMovementStdWheeled::CVehicleMovementStdWheeled()
{
	m_steerSpeed = 40.0f;
	m_steerMax = 20.0f;
	m_steerSpeedMin = 90.0f;
	m_steerSpeedScale = 0.8f;
	m_kvSteerMax = 10.0f;
	m_steerSpeedScaleMin = 1.0f;
	m_v0SteerMax = 30.0f;
	m_steerRelaxation = 90.0f;
  m_vMaxSteerMax = 20.f;
  m_pedalLimitMax = 0.3f;  
  m_suspDampingMin = 0.f;
  m_suspDampingMax = 0.f;
  m_speedSuspUpdated = 0.f;
  m_suspDamping = 0.f;
  m_rpmTarget = 0.f;
  m_engineMaxRPM = m_engineIdleRPM = m_engineShiftDownRPM = 0.f;
  m_rpmRelaxSpeed = 4.f;
  m_rpmInterpSpeed = 4.f;

  m_prevAngle = 0.0f;
  m_wheelParams = 0;
  m_lastSteerUpdateTime = GetISystem()->GetITimer()->GetFrameStartTime();
	m_netActionSync.PublishActions( CNetworkMovementStdWheeled(this) );
}

//------------------------------------------------------------------------
CVehicleMovementStdWheeled::~CVehicleMovementStdWheeled()
{
  if (m_wheelParams)
    delete[] m_wheelParams;
}

//------------------------------------------------------------------------
bool CVehicleMovementStdWheeled::Init(IVehicle* pVehicle, const SmartScriptTable &table)
{
	if (!CVehicleMovementBase::Init(pVehicle, table))
	{
		assert(0);
	}

	m_pPhysics = NULL;

	if (!table->GetValue("Wheeled", m_wheeledTable))
		return false;

  table->GetValue("steerSpeed", m_steerSpeed);
  table->GetValue("steerSpeedMin", m_steerSpeedMin);
  table->GetValue("steerSpeedScale", m_steerSpeedScale);
  table->GetValue("steerSpeedScaleMin", m_steerSpeedScaleMin);
  table->GetValue("kvSteerMax", m_kvSteerMax);
  table->GetValue("v0SteerMax", m_v0SteerMax);
  table->GetValue("steerRelaxation", m_steerRelaxation);
  table->GetValue("vMaxSteerMax", m_vMaxSteerMax);
  table->GetValue("pedalLimitMax", m_pedalLimitMax);
  table->GetValue("rpmRelaxSpeed", m_rpmRelaxSpeed);
  table->GetValue("rpmInterpSpeed", m_rpmInterpSpeed);

	m_action.steer = 0.0f;
	m_action.pedal = 0.0f;
	m_action.dsteer = 0.0f;

	// Initialise the direction PID.
	m_direction = 0.0f;
	m_dirPID.Reset();
	m_dirPID.m_kP = 0.6f;
	m_dirPID.m_kD = 0.1f;
	m_dirPID.m_kI = 0.01f;

	// Initialise the steering history.
	m_steering = 0.0f;
  m_prevAngle = 0.0f;

	// Initialise damage
	m_damageLevel = 0.0f;

	m_rpmScale = 0.0f;
	m_currentGear = 0;

	m_damageMul = 1.0f;

	if (!table->GetValue("isBreakingOnIdle", m_isBreakingOnIdle))
		m_isBreakingOnIdle = false;

	return true;
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::PostInit()
{   
}

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

	SEntityPhysicalizeParams& physicsParams = m_pVehicle->GetPhysicsParams();

	physicsParams.type = PE_WHEELEDVEHICLE;
	physicsParams.pCar = new pe_params_car;

	pe_params_car& carParams = *physicsParams.pCar;

	bool invalid=!m_wheeledTable;
	if (!invalid)
	{
		m_wheeledTable->GetValue("axleFriction", carParams.axleFriction);
		m_wheeledTable->GetValue("brakeTorque", carParams.brakeTorque );
		m_wheeledTable->GetValue("clutchSpeed", carParams.clutchSpeed );
		m_wheeledTable->GetValue("damping", carParams.damping );
		m_wheeledTable->GetValue("engineIdleRPM", carParams.engineIdleRPM );
		m_wheeledTable->GetValue("engineMaxRPM", carParams.engineMaxRPM );
		m_wheeledTable->GetValue("engineMinRPM", carParams.engineMinRPM );
		m_wheeledTable->GetValue("enginePower", carParams.enginePower );
		m_wheeledTable->GetValue("engineShiftDownRPM", carParams.engineShiftDownRPM );
		m_wheeledTable->GetValue("engineShiftUpRPM", carParams.engineShiftUpRPM );
		m_wheeledTable->GetValue("engineStartRPM", carParams.engineStartRPM );
		m_wheeledTable->GetValue("integrationType", carParams.iIntegrationType );
		m_wheeledTable->GetValue("stabilizer", carParams.kStabilizer );
		m_wheeledTable->GetValue("minBrakingFriction", carParams.minBrakingFriction );
		m_wheeledTable->GetValue("maxBrakingFriction", carParams.maxBrakingFriction );
		m_wheeledTable->GetValue("maxSteer", carParams.maxSteer );    
		m_wheeledTable->GetValue("maxTimeStep", carParams.maxTimeStep );
		m_wheeledTable->GetValue("minEnergy", carParams.minEnergy );
		m_wheeledTable->GetValue("slipThreshold", carParams.slipThreshold );
		m_wheeledTable->GetValue("gearDirSwitchRPM", carParams.gearDirSwitchRPM );
		m_wheeledTable->GetValue("dynFriction", carParams.kDynFriction );
		m_wheeledTable->GetValue("steerTrackNeutralTurn", carParams.steerTrackNeutralTurn);  
		m_wheeledTable->GetValue("suspDampingMin", m_suspDampingMin);
		m_wheeledTable->GetValue("suspDampingMax", m_suspDampingMax);
    m_wheeledTable->GetValue("maxSpeed", m_maxSpeed );

		carParams.enginePower *= 1000.0f;

		float pullTilt = 0;
		m_wheeledTable->GetValue("pullTilt", pullTilt); 

		if (pullTilt)
			carParams.pullTilt = DEG2RAD(pullTilt);

		std::vector<float> gearRatios;
		gearRatios.resize(0);
		SmartScriptTable gearRatiosTable;

		if (m_wheeledTable->GetValue( "gearRatios", gearRatiosTable))
		{
			gearRatios.reserve(gearRatiosTable->Count());
			for (int i=1; i<=gearRatiosTable->Count(); i++)
			{
				float value;
				if (!gearRatiosTable->GetAt(i, value))
				{
					break;
				}
				gearRatios.push_back(value);
			}

			carParams.nGears = gearRatios.size();
			carParams.gearRatios = new float[gearRatios.size()];
			for (int i=0; i < carParams.nGears; i++)
			{
				carParams.gearRatios[i] = gearRatios[i];
			}
		}
	}

	carParams.nWheels = m_pVehicle->GetWheelCount();

	m_pEntity->Physicalize(physicsParams);

	m_engineMaxRPM = carParams.engineMaxRPM;
	m_engineIdleRPM = carParams.engineIdleRPM;
	m_engineShiftDownRPM = carParams.engineShiftDownRPM;

	m_pPhysics = m_pEntity->GetPhysics();
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::InitWheeledParticles()
{ 
  pe_status_nparts tmpStatus;
  int numParts = m_pPhysics->GetStatus(&tmpStatus);
  int numWheels = m_pVehicle->GetWheelCount();

  m_paStats.envStats.emitters.clear();

  // for each wheelgroup, add 1 particleemitter. the position is the wheels' 
  // center in xy-plane and minimum on z-axis
  // direction is upward
  SEnvironmentParticles* envParams = m_pPaParams->GetEnvironmentParticles();
  
  for (int iLayer=0; iLayer<envParams->GetLayerCount(); ++iLayer)
  {
    const SEnvironmentLayer& layer = envParams->GetLayer(iLayer);
    
    m_paStats.envStats.emitters.reserve(m_paStats.envStats.emitters.size() + layer.GetGroupCount());

    for (int i=0; i<layer.GetGroupCount(); ++i)
    { 
      Matrix34 tm;
      tm.SetIdentity();
      
      if (layer.GetHelperCount() == layer.GetGroupCount() && strlen(layer.GetHelper(i))>0)
      {
        // use helper pos if specified
        tm = m_pVehicle->GetHelperLocalTM(layer.GetHelper(i), false, false);
      }
      else
      {
        // else use wheels' center
        Vec3 pos(ZERO);

        for (int w=0; w<layer.GetWheelCount(i); ++w)
        {       
          int ipart = numParts - numWheels + layer.GetWheelAt(i,w)-1; // wheels are last

          if (ipart < 0)
          {
            assert(ipart>=0);
            continue;
          }

          pe_status_pos spos;
          spos.ipart = ipart;
          if (m_pPhysics->GetStatus(&spos))
          {
            spos.pos.z += spos.BBox[0].z;
            pos = (pos.IsZero()) ? spos.pos : 0.5f*(pos + spos.pos);        
          }
        }
        tm = Matrix34::CreateRotationX(DEG2RAD(90.f));      
        tm.SetTranslation( m_pEntity->GetWorldTM().GetInverted().TransformPoint(pos) );
      }     

      // dummy effect for loading
      IParticleEffect* pEff = m_p3DEngine->FindParticleEffect("vehicle_fx.vehicles_surface_fx.soil_dust");

      SpawnParams sp;         
      sp.fCountScale = 0.f;

      int slot = m_pEntity->LoadParticleEmitter(-1, pEff, &sp);
      if (slot > -1) 
      { 
        m_pEntity->SetSlotLocalTM(slot, tm);

        TEnvEmitter emitter;
        emitter.layer = iLayer;        
        emitter.slot = slot;
        emitter.group = i;
        m_paStats.envStats.emitters.push_back(emitter);

        if (DebugParticles())
        {
          const Vec3 loc = tm.GetTranslation();
          CryLog("WheelGroup %i local pos: %.1f %.1f %.1f", i, loc.x, loc.y, loc.z);
          CryLog("WheelGroup %i loaded emitter in slot %i", i, slot);
        }      
      }
    }
  }
  
  m_paStats.envStats.initalized = true;  
}

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

	m_direction = 0;
	m_steering = 0;
	m_dirPID.Reset();
  m_prevAngle = 0.0f;

	m_damageLevel = 0.0f;
	m_damageMul = 1.0f;

	m_rpmScale = 0.0f;
	m_currentGear = 0;
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::Release()
{
	CVehicleMovementBase::Release();

	delete this;
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::StopEngine()
{
	CVehicleMovementBase::StopEngine(); 
	m_movementAction.Clear(true);

  m_pGameTokenSystem->SetOrCreateToken("vehicle.rpmNorm", TFlowInputData(0.f, true));
}

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

	if (event == eVME_Damage)
	{
		m_damageLevel = max(m_damageLevel, value);
		if (m_damageLevel >= 1.0f)
		{
			m_damageMul = 0.0f;
		}
		else
		{
			m_damageMul = (1.0f - m_damageLevel) * 0.6f;
			m_damageMul += 0.4f;
		}
	}
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::ProcessMovement(const float deltaTime)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

  LastMinuteInits(); // fix this (proper physicalization needed in movement init)

  m_netActionSync.UpdateObject(this);

	m_pPhysics->GetStatus(&m_vehicleStatus);

  UpdateSuspension();

	if (m_movementAction.brake)
		m_action.bHandBrake = 1;
	else
		m_action.bHandBrake = 0;

	if (m_isBreakingOnIdle && m_movementAction.power == 0.0f)
	{
		m_action.bHandBrake = 1;
	}
	
  // speed ratio
  float speed = m_vehicleStatus.vel.len();
	float speedDelta = min(speed, m_vMaxSteerMax) / m_vMaxSteerMax;

  // reduce max steering angle with increasing speed
  m_steerMax = m_v0SteerMax - (m_kvSteerMax * speedDelta);
	float steerMax = DEG2RAD(m_steerMax);
  
  // reduce steer speed with increasing speed
  float steerDelta = m_steerSpeed - m_steerSpeedMin;
	float steerSpeed = m_steerSpeedMin + steerDelta * speedDelta;
	
  // additionally adjust sensitivity based on speed
  float steerScaleDelta = m_steerSpeedScale - m_steerSpeedScaleMin;
	float sensivity = m_steerSpeedScaleMin + steerScaleDelta * speedDelta;
	
  // calc steer error
	float steering = m_movementAction.rotateYaw;
	float steerError = steering * steerMax - m_action.steer;
  steerError = (fabs(steerError)<0.01) ? 0 : steerError;

  if (fabs(m_movementAction.rotateYaw) > 0.005f)
  {
    // adjust steering based on current error
    //m_action.steer = m_action.steer + steerError * (DEG2RAD(steerSpeed) * deltaTime * sensivity);
    m_action.steer = m_action.steer + sgn(steerError) * DEG2RAD(steerSpeed) * deltaTime * sensivity;  
    m_action.steer = CLAMP(m_action.steer, -steerMax, steerMax);
  }
  else
	{
    // relax to center
		float d = -m_action.steer;
		float a = min(DEG2RAD(deltaTime * m_steerRelaxation), 1.0f);
		m_action.steer = m_action.steer + d * a;
	}

	if (fabs(m_action.steer) < 0.01f)
		m_action.steer = 0.0f;

  // reduce actual pedal with increasing steering and velocity
  float maxPedal = 1 - (speedDelta * abs(steering) * m_pedalLimitMax);  
  m_action.pedal = CLAMP(m_movementAction.power, -maxPedal, maxPedal ) * m_damageMul;

	m_pPhysics->Action(&m_action, 1);
  
  if (IsProfilingMovement())
  {
    IRenderer* pRenderer = GetISystem()->GetIRenderer();
    static float color[4] = {1,1,1,1};
    float y = 50.f, step1 = 15.f, step2 = 20.f;

    pRenderer->Draw2dLabel(5.0f,   y, 2.0f, color, false, "Car movement");
    pRenderer->Draw2dLabel(5.0f,  y+=step2, 1.5f, color, false, "Speed: %.1f (%.1f km/h)", speed, speed*3.6f);
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "RPM:   %.0f", m_vehicleStatus.engineRPM); 
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "rpm_scale:   %.2f", m_rpmScale); 
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "Gear:  %i", m_vehicleStatus.iCurGear-1);
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "Clutch:  %.2f", m_vehicleStatus.clutch);
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "Dampers:  %.2f", m_suspDamping);

    pRenderer->Draw2dLabel(5.0f,  y+=step2, 2.0f, color, false, "Driver input");
    pRenderer->Draw2dLabel(5.0f,  y+=step2, 1.5f, color, false, "power: %.2f", m_movementAction.power);
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "steer: %.2f", m_movementAction.rotateYaw); 
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "brake: %i", m_movementAction.brake);

    pRenderer->Draw2dLabel(5.0f,  y+=step2, 2.0f, color, false, "Car action");
    pRenderer->Draw2dLabel(5.0f,  y+=step2, 1.5f, color, false, "pedal: %.2f", m_action.pedal);
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "steer: %.2f", m_action.steer); 
    pRenderer->Draw2dLabel(5.0f,  y+=step1, 1.5f, color, false, "brake: %i", m_action.bHandBrake);

    pe_status_wheel ws;
    pe_status_pos wp;

    pe_status_nparts tmpStatus;
    int numParts = m_pPhysics->GetStatus(&tmpStatus);

    int count = m_pVehicle->GetWheelCount();
    for (int i=0; i<count; ++i)
    {
      ws.iWheel = i;
      wp.ipart = numParts - count + i;

      if (!m_pPhysics->GetStatus(&ws) || !m_pPhysics->GetStatus(&wp))
        continue;
      
      float slip = ws.velSlip.len();
      if (slip>0.01f)
      { 
        IRenderAuxGeom* pGeom = GetISystem()->GetIRenderer()->GetIRenderAuxGeom();
        static ColorB col(255,0,0,255);
        pGeom->DrawLine(wp.pos, col, wp.pos+ws.velSlip, col);
      }
    }
  }

	if (m_netActionSync.PublishActions( CNetworkMovementStdWheeled(this) ))
		m_pVehicle->GetGameObject()->ChangedNetworkState( eEA_GameClientDynamic );
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::LastMinuteInits()
{ 
  if (!m_wheelParams)
  {
    int numWheels = m_pVehicle->GetWheelCount();

    m_wheelParams = new pe_params_wheel[numWheels];
    for (int i=0; i<numWheels; ++i)
    {
      m_wheelParams[i].iWheel = i;
      if (!m_pPhysics->GetParams(&m_wheelParams[i]))
        assert(0 && "getting wheelparams failed!");
    }

    int nParts = m_pVehicle->GetPartCount();
    IVehiclePart** parts = new IVehiclePart*[nParts];
    m_pVehicle->GetParts(parts, nParts);
    
    for (int i=0; i<nParts; ++i)
    {
      if (parts[i]->GetType() == IVehiclePart::eVPT_WHEEL)
        m_wheelParts.push_back((CVehiclePartSubPartWheel*)parts[i]);
    }
    assert(m_wheelParts.size() == numWheels);
    delete[] parts;

    if (m_maxSpeed == 0.f)
    {
      pe_status_vehicle_abilities ab;
      m_pPhysics->GetStatus(&ab);
      m_maxSpeed = ab.maxVelocity * 0.5f; // fixme! maxVelocity too high
      CryLog("%s maxSpeed: %f", m_pVehicle->GetEntity()->GetClass()->GetName(), m_maxSpeed);
    }
  }
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::Update(const float deltaTime)
{
  FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );
    
  m_pPhysics->GetStatus(&m_vehicleStatus);

	CVehicleMovementBase::Update(deltaTime);

  UpdateSounds(deltaTime);  
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::UpdateGameTokens(const float deltaTime)
{
  CVehicleMovementBase::UpdateGameTokens(deltaTime);

  if (m_pVehicle->IsPlayerDriving())
  { 
    m_pGameTokenSystem->SetOrCreateToken("vehicle.rpmNorm", TFlowInputData(m_rpmScale, true));
  }    
}


//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::UpdateSounds(const float deltaTime)
{
  // update engine sound
  if (m_isEnginePowered && !m_isEngineGoingOff)
  {
    float rpmScale = min(m_vehicleStatus.engineRPM / m_engineMaxRPM, 1.f);
    
    if (m_vehicleStatus.bHandBrake)
    {
      Interpolate(m_rpmScale, rpmScale, 2.5f, deltaTime);
      m_rpmTarget = 0.f;
    }
    else if (m_rpmTarget)
    {
      Interpolate(m_rpmScale, m_rpmTarget, m_rpmRelaxSpeed, deltaTime);

      if (abs(m_rpmScale-m_rpmTarget)<0.02)
      {
        m_rpmTarget = 0.f;

        if (m_currentGear >= 3) // only from 1st gear upward
        {          
          string soundName = string("Sounds/Vehicles:") + string(m_pEntity->GetClass()->GetName()).MakeLower() + string(":gear");
          m_pEntitySoundsProxy->PlaySound(soundName.c_str(), m_enginePos, FORWARD_DIRECTION, FLAG_SOUND_DEFAULT_3D);        
        }
      }
    }
    else
      Interpolate(m_rpmScale, rpmScale, m_rpmInterpSpeed, deltaTime);
    
    SetSoundParam(eSID_Run, "rpm_scale", m_rpmScale);

    if (m_currentGear != m_vehicleStatus.iCurGear)
    { 
      // when shifting up from 1st upward, set sound target to low rpm to simulate dropdown 
      // during clutch disengagement
      if (m_currentGear >= 2 && m_vehicleStatus.iCurGear>m_currentGear)
      {
        m_rpmTarget = m_engineShiftDownRPM/m_engineMaxRPM;
        //m_rpmScale += 0.05f;
      }

      if (!m_rpmTarget && !(m_currentGear<=2 && m_vehicleStatus.iCurGear<=2))
      {
        // do gearshift sound only for gears higher than 1st forward
        // in case rpmTarget has been set, shift is played upon reaching it
        string soundName = string("Sounds/Vehicles:") + string(m_pEntity->GetClass()->GetName()).MakeLower() + string(":gear");
        m_pEntitySoundsProxy->PlaySound(soundName.c_str(), m_enginePos, FORWARD_DIRECTION, FLAG_SOUND_DEFAULT_3D);        
      }

      m_currentGear = m_vehicleStatus.iCurGear;
    }
   
  }
}


//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::UpdateSuspension()
{
  if (m_suspDampingMax == 0.f || m_suspDampingMin == 0.f)
    return;

  float speed = m_statusDyn.v.len();
  
  if (abs(m_speedSuspUpdated-speed) > 1.5f) // only update when speed changes
  {
    float diff = m_suspDampingMax - m_suspDampingMin;
    float speedNorm = min(1.f, speed/(0.15f*m_maxSpeed));
    m_suspDamping = m_suspDampingMin + (speedNorm * diff);

    pe_params_wheel params;

    int count = m_pVehicle->GetWheelCount();
    for (int i=0; i<count; ++i)
    {
      params.iWheel = i;
      params.kDamping = m_suspDamping;
      m_pPhysics->SetParams(&params, 1);
    }
    m_speedSuspUpdated = speed;
  }
}

//------------------------------------------------------------------------
bool CVehicleMovementStdWheeled::RequestMovement(CMovementRequest& movementRequest)
{
	SmartScriptTable stateTable;
	if (!m_pEntity->GetScriptTable()->GetValue("State", stateTable))
	{
		CryError( "Vehicle '%s' is missing 'State' LUA table. Required by AI", m_pEntity->GetName() );
		return false;
	}

	Vec3 worldPos = m_pEntity->GetWorldPos();

	float inputSpeed;
	if (movementRequest.HasDesiredSpeed())
		inputSpeed = movementRequest.GetDesiredSpeed();
	else
		inputSpeed = 0.0f;

	Vec3 moveDir;
	if (movementRequest.HasMoveTarget())
		moveDir = (movementRequest.GetMoveTarget() - worldPos).GetNormalizedSafe();
	else
		moveDir.zero();

	// If the movement vector is nonzero there is a target to drive at.
	if (moveDir.GetLengthSquared() > 0.01f)
	{
		SmartScriptTable movementAbilityTable;
		if (!m_pEntity->GetScriptTable()->GetValue("AIMovementAbility", movementAbilityTable))
		{
			CryError("Vehicle '%s' is missing 'AIMovementAbility' LUA table. Required by AI", m_pEntity->GetName());
			return false;
		}

		float	maxSpeed = 0.0f;
		if( !movementAbilityTable->GetValue( "maxSpeed", maxSpeed ) )
		{
			CryError("Vehicle '%s' is missing 'MovementAbility.maxSpeed' LUA variable. Required by AI", m_pEntity->GetName());
			return false;
		}

		Matrix33 entRotMat(m_pEntity->GetRotation());
		
		Vec3 forwardDir = entRotMat.TransformVector(FORWARD_DIRECTION);
		forwardDir.z = 0.0f;
    forwardDir.NormalizeSafe(Vec3Constants<float>::fVec3_OneX);
		Vec3 rightDir(forwardDir.y, -forwardDir.x, 0.0f);

		// Normalize the movement dir so the dot product with it can be used to lookup the angle.
		Vec3 targetDir = moveDir;
		targetDir.z = 0.0f;
		targetDir.NormalizeSafe(Vec3Constants<float>::fVec3_OneX);

    /// component parallel to target dir
		float	cosAngle = forwardDir.Dot(targetDir);
    Limit(cosAngle, -1.0f, 1.0f);
		float angle = RAD2DEG((float)acos(cosAngle));

    if (targetDir.Dot(rightDir) < 0.0f)
			angle = -angle;
    if ( !(angle < 181.0f && angle > -181.0f) )
      angle = 0.0f;

		// Danny no need for PID - just need to get the proportional constant right
		// to turn angle into range 0-1. Predict the angle(=error) between now and 
		// next step - in fact over-predict to account for the lag in steering
    CTimeValue curTime = GetISystem()->GetITimer()->GetFrameStartTime();
    float dt = (curTime - m_lastSteerUpdateTime).GetSeconds();
    if (dt > 0.0f)
    {
      // this time prediction is to take into account the steering lag and prevent
      // oscillations - really it should be vehicle dependant. 
		  static float steerTimescale = 0.1f;
		  static float steerConst = 0.05f;

		  float predAngle = angle + (angle - m_prevAngle) * steerTimescale / dt;
      m_lastSteerUpdateTime = curTime;
		  m_prevAngle = angle;
      if (m_steerMax > 0.0f)
        m_steering = predAngle / m_steerMax; // nearly exact for real wheeled vehicles - approx for tank
      else
    		m_steering = steerConst * predAngle; // shouldn't really get used
    }  		

    Limit(m_steering, -1.0f, 1.0f);

		// DEBUG: temporary hack to get the speed of the vehicle.
		pe_status_vehicle status;
		IPhysicalEntity* pPhysics = m_pEntity->GetPhysics();
		pPhysics->GetStatus(&status);

		Vec3 vel = status.vel;
		float	speed = forwardDir.Dot(vel);

    if (!(speed < 100.0f && speed > -100.0f))
    {
      GameWarning("[CVehicleMovementStdWheeled]: Bad speed from physics: %5.2f", speed);
      speed = 0.0f;
    }

		// If the desired speed is negative, it means that the maximum speed of the vehicle
		// should be used.
		float	desiredSpeed;
		if (movementRequest.HasDesiredSpeed())
			desiredSpeed = movementRequest.GetDesiredSpeed();
		else
			desiredSpeed = 0.0f;
		
    // desiredSpeed is in m/s
		if (desiredSpeed > maxSpeed)
			desiredSpeed = maxSpeed;

		// Allow breaking if the error is too high.
		float	clampMin = 0.0f;
		if (abs(desiredSpeed - speed) > desiredSpeed * 0.5f)
			clampMin = -1.0f;

    static bool usePID = false;
    if (usePID)
    {
  		m_direction = m_dirPID.Update(speed/maxSpeed, desiredSpeed/maxSpeed, clampMin, 1.0f);
    }
    else
    {
      static float accScale = 0.5f;
      m_direction = (desiredSpeed - speed) * accScale;
      Limit(m_direction, clampMin, 1.0f);
    }
    static bool dumpSpeed = false;
    if (dumpSpeed)
      GetISystem()->GetILog()->Log("speed = %5.2f desiredSpeed = %5.2f power = %5.2f", speed, desiredSpeed, m_direction);

    // let's break - we don't want to move anywhere
    if (fabs(desiredSpeed) < 0.001f)
    {
		  stateTable->SetValue("braking", 1);
		  m_movementAction.brake = true;
    }
    else
    {
		  stateTable->SetValue("braking", 0);
		  m_movementAction.brake = false;
    }

		stateTable->SetValue("direction", m_direction);
		stateTable->SetValue("steering", m_steering);
		m_movementAction.power = m_direction;
		m_movementAction.rotateYaw = m_steering;
		m_movementAction.isAI = true;
	}
	else
	{
		// let's break - we don't want to move anywhere
		stateTable->SetValue("braking", 1);
		m_movementAction.brake = true;
		m_movementAction.rotateYaw = m_steering;
	}

	return true;
}

void CVehicleMovementStdWheeled::GetMovementState(SMovementState& movementState)
{
	if (!m_pPhysics)
		return;

  if (m_maxSpeed == 0.f)
  {
    pe_status_vehicle_abilities ab;
    m_pPhysics->GetStatus(&ab);
    m_maxSpeed = ab.maxVelocity * 0.5f; // fixme
  }  

	movementState.minSpeed = 0.0f;
	movementState.maxSpeed = m_maxSpeed;
	movementState.normalSpeed = movementState.maxSpeed;
}

//------------------------------------------------------------------------
CNetworkMovementStdWheeled::CNetworkMovementStdWheeled()
: m_steer(0.0f),
	m_pedal(0.0f),
	m_brake(false)
{
}

//------------------------------------------------------------------------
CNetworkMovementStdWheeled::CNetworkMovementStdWheeled(CVehicleMovementStdWheeled *pMovement)
{
	m_steer = pMovement->m_movementAction.rotateYaw;
	m_pedal = pMovement->m_movementAction.power;
	m_brake = pMovement->m_movementAction.brake;
}

//------------------------------------------------------------------------
void CNetworkMovementStdWheeled::UpdateObject(CVehicleMovementStdWheeled *pMovement)
{
	pMovement->m_movementAction.rotateYaw = m_steer;
	pMovement->m_movementAction.power = m_pedal;
	pMovement->m_movementAction.brake = m_brake;
}

//------------------------------------------------------------------------
void CNetworkMovementStdWheeled::Serialize(TSerialize ser, unsigned aspects)
{
	if (aspects & eEA_GameClientDynamic)
	{
		ser.Value("steer", m_steer, NSerPolicy::AC_SimpleFloat(-1.0f, 1.0f, 12, 0.049f, 0));
		ser.Value("pedal", m_pedal, NSerPolicy::Q_FloatAsInt(-1.0f, 1.0f, 10));
		ser.Value("brake", m_brake, NSerPolicy::A_Bool());
	}
}

//------------------------------------------------------------------------
void CVehicleMovementStdWheeled::UpdateEnvParticles(const float deltaTime)
{ 
  FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

  // fixme: physics init not complete in movement init
  if (!m_paStats.envStats.initalized)
    InitWheeledParticles();

  // process wheeled particles
  SEnvironmentParticles* envParams = m_pPaParams->GetEnvironmentParticles();
  SEnvParticleStatus::TEnvEmitters::iterator emitterIt = m_paStats.envStats.emitters.begin();

  for (; emitterIt!=m_paStats.envStats.emitters.end(); ++emitterIt)
  { 
    if (emitterIt->layer < 0)
    {
      assert(0);
      continue;
    }

    // scaling for each wheelgroup is based on vehicle speed + avg. slipspeed
    float slipAvg = 0; 
    int cnt = 0;
    bool bContact = false;
    int matId = 0;

    const SEnvironmentLayer& layer = envParams->GetLayer(emitterIt->layer);
    
    int wheelCount = layer.GetWheelCount(emitterIt->group);    
    for (int w=0; w<wheelCount; ++w)
    {
      // all wheels in group
      ++cnt;
      pe_status_wheel wheelStats;
      wheelStats.iWheel = layer.GetWheelAt(emitterIt->group, w) - 1;
      
      if (!m_pPhysics->GetStatus(&wheelStats))
        continue;

      if (wheelStats.bContact)
      {
        bContact = true;
        
        if (wheelStats.contactSurfaceIdx > matId)
          matId = wheelStats.contactSurfaceIdx;

        if (wheelStats.bSlip)
          slipAvg += wheelStats.velSlip.len();
      }
    }

    if (!bContact && !emitterIt->bContact)
      continue;

    emitterIt->bContact = bContact;
    
    float vel = m_statusDyn.v.len() + (slipAvg/=cnt);        
    
    float countScale = 1;
    float sizeScale = 1;

    if (vel < layer.minSpeed || !bContact || matId == 0)
    {   
      countScale = 0;          
    }
    else
    { 
      float speedDiff = max(0.f, layer.maxSpeed - layer.minSpeed);        

      if (speedDiff)
      {
        float speedNorm = CLAMP(vel, layer.minSpeed, layer.maxSpeed) / speedDiff;                                

        float countDiff = max(0.f, layer.maxSpeedCountScale - layer.minSpeedCountScale);          
        if (countDiff)          
          countScale *= (speedNorm * countDiff) + layer.minSpeedCountScale;

        float sizeDiff  = max(0.f, layer.maxSpeedSizeScale - layer.minSpeedSizeScale);
        if (sizeDiff)                      
          sizeScale *= (speedNorm * sizeDiff) + layer.minSpeedSizeScale;                  
      }
    }

    SEntitySlotInfo info;
    info.pParticleEmitter = 0;
    if (m_pEntity->GetSlotInfo(emitterIt->slot, info) && info.pParticleEmitter)
    {       
      if (matId != emitterIt->matId)
      {
        // change effect                
        const char* effect = GetEffectByIndex( GetEffectNode(matId), emitterIt->layer );
        IParticleEffect* pEff = 0;        
        
        if (effect && (pEff = m_p3DEngine->FindParticleEffect(effect)))
        {           
          if (DebugParticles())
          {
            CryLog("%s changes sfx to %s (slot %i)", m_pEntity->GetName(), effect, emitterIt->slot);
          }
          info.pParticleEmitter->SetEffect(pEff);
          //info.pParticleEmitter->Activate(true);
        }
        else 
        {
          if (DebugParticles())
            CryLog("%s found no effect for %i", m_pEntity->GetName(), matId);

          // effect not available, disable
          //info.pParticleEmitter->Activate(false);
          countScale = 0.f; 
        }        
      }
      emitterIt->matId = matId;
         
      SpawnParams sp;
      sp.fSizeScale = sizeScale;
      sp.fCountScale = countScale;
      info.pParticleEmitter->SetSpawnParams(sp);

      if (DebugParticles())
      {
        float color[] = {1,1,1,1};
        GetISystem()->GetIRenderer()->Draw2dLabel(200, 300+25*emitterIt->group, 1.5f, color, false, "group %i: sizeScale %f, countScale %f", emitterIt->group, sizeScale, countScale);
      }           
    }      
  }
  
}


