#include "TaskVehiclePark.h"

// Rage headers
#include "grcore/debugdraw.h"

// Framework headers
#include "fwmaths/geomutil.h"

// Game headers
#include "Vehicles/Planes.h"
#include "Vehicles/Submarine.h"
#include "Vehicles/heli.h"
#include "vehicleAi/driverpersonality.h"
#include "VehicleAi/task/TaskVehicleGoTo.h"
#include "vehicleAi/task/TaskVehicleGoToAutomobile.h"
#include "VehicleAi/task/TaskVehicleCruise.h"
#include "VehicleAi/task/TaskVehicleTempAction.h"
#include "VehicleAi/VehicleIntelligence.h"
#include "Vehicles/Trailer.h"
#include "Peds/PedIntelligence.h"
#include "Task/Scenario/ScenarioChainingTests.h"
#include "Task/Scenario/ScenarioManager.h"
#include "Task/Scenario/Types/TaskVehicleScenario.h"
#include "Task/Vehicle/TaskExitVehicle.h"
#include "scene/world/gameWorld.h"


AI_OPTIMISATIONS()
AI_VEHICLE_OPTIMISATIONS()

///////////////////////////////////////////////////////////////////////////////

CTaskVehicleStop::CTaskVehicleStop(u32 Stopflags)
: m_StopFlags(Stopflags)
{
	m_Params.m_fCruiseSpeed = 0.0f;

	if (Stopflags & SF_DontTerminateWhenStopped)
	{
		m_Params.m_iDrivingFlags.SetFlag(DF_DontTerminateTaskWhenAchieved);
	}

	SetInternalTaskType(CTaskTypes::TASK_VEHICLE_STOP);
}


///////////////////////////////////////////////////////////////////////////////

CTask::FSM_Return	CTaskVehicleStop::Stop_OnUpdate					(CVehicle* pVehicle)
{
	if(m_StopFlags&SF_SupressBrakeLight)
	{
		pVehicle->m_nVehicleFlags.bSuppressBrakeLight = true;
	}

	const bool bDontResetSteerAngle = (m_StopFlags&SF_DontResetSteerAngle) != 0;
	if(!pVehicle->InheritsFromBike() && !pVehicle->InheritsFromQuadBike() && !pVehicle->InheritsFromAmphibiousQuadBike() && !bDontResetSteerAngle)
	{
		pVehicle->SetSteerAngle(0.0f);
	}

	float fThrottle = 0.0f;

	bool bOppressor2 = MI_BIKE_OPPRESSOR2.IsValid() && pVehicle->GetModelIndex() == MI_BIKE_OPPRESSOR2;
	if (bOppressor2)
	{
		// Brake / handbrake have virtually no effect on a hover bike. In order for this task to work, apply some throttle in the opposite direction to our movement.
		float fSpeed = DotProduct(pVehicle->GetVelocityIncludingReferenceFrame(), VEC3V_TO_VECTOR3(pVehicle->GetTransform().GetB()));
		TUNE_GROUP_FLOAT(VEHICLE_STOP, OPPRESSOR2_OPPOSITE_THROTTLE, 0.25f, 0.0f, 1.0f, 0.01f);
		fThrottle = fSpeed > 0.0f ? -OPPRESSOR2_OPPOSITE_THROTTLE : OPPRESSOR2_OPPOSITE_THROTTLE;
	}

	pVehicle->SetThrottle(fThrottle);

	bool bShouldUseFullBrake = (m_StopFlags & SF_UseFullBrake) || bOppressor2;
	pVehicle->SetBrake(bShouldUseFullBrake ? 1.0f : 0.5f);

	pVehicle->SetHandBrake(true);

	pVehicle->GetIntelligence()->UpdateCarHasReasonToBeStopped();

	m_bMissionAchieved = true;
	pVehicle->m_nVehicleFlags.bIsThisAParkedCar = true;

	VehicleType vehicleType = pVehicle->GetVehicleType();
	if( vehicleType == VEHICLE_TYPE_SUBMARINECAR && !pVehicle->IsInSubmarineMode())
	{
		vehicleType = VEHICLE_TYPE_CAR;
	}

	if( vehicleType == VEHICLE_TYPE_AMPHIBIOUS_AUTOMOBILE || 
		vehicleType == VEHICLE_TYPE_AMPHIBIOUS_QUADBIKE )
	{
		vehicleType = VEHICLE_TYPE_CAR;
	}

	bool mightDoTimeslicing = true;
	switch(vehicleType)
	{
	case VEHICLE_TYPE_HELI:
	case VEHICLE_TYPE_BLIMP:
		((CHeli*)pVehicle)->SetYawControl(0.0f);
		((CHeli*)pVehicle)->SetPitchControl(0.0f);
		((CHeli*)pVehicle)->SetRollControl(0.0f);
		if( pVehicle->HasContactWheels() ||
			( pVehicle->pHandling->GetSeaPlaneHandlingData() && pVehicle->m_nFlags.bPossiblyTouchesWater ) )
		{
			( (CHeli*)pVehicle )->SetThrottleControl( 0.0f );
		}
		else
			((CHeli*)pVehicle)->SetThrottleControl(0.95f);
		mightDoTimeslicing = false;
		break;
	case VEHICLE_TYPE_BOAT:
		pVehicle->SetThrottle(ComputeThrottleForSpeed(*pVehicle, 0.0f));
		pVehicle->SetBrake(1.0f);
		break;
	case VEHICLE_TYPE_PLANE:
		((CPlane*)pVehicle)->SetYawControl(0.0f);
		((CPlane*)pVehicle)->SetPitchControl(0.0f);
		((CPlane*)pVehicle)->SetRollControl(0.0f);
		((CPlane*)pVehicle)->SetThrottleControl(0.0f);
		mightDoTimeslicing = false;
		break;
	case VEHICLE_TYPE_SUBMARINE:
	case VEHICLE_TYPE_SUBMARINECAR:
		{
			{
				CSubmarineHandling* pSubHandling = ((CSubmarine*)pVehicle)->GetSubHandling();
				Assertf(pSubHandling, "Sub handling should be valid");

				pSubHandling->SetPitchControl(0.0f);

				Vector3 vVelocity = pVehicle->GetVelocity();			
				vVelocity.Normalize();
				Vector3 vDirection = VEC3V_TO_VECTOR3(pVehicle->GetVehicleForwardDirection());
				float fVelocityDot = vDirection.Dot(vVelocity);
				if(fVelocityDot > 0.9f)
				{
					pVehicle->SetThrottle(ComputeThrottleForSpeed(*pVehicle, 0.0f));
				}
				else
				{
					// B*2023637: This behaviour is only for AI drivers tasked with stopping their vehicle.
					// Don't do it for players as it makes subs spin around oddly when the ped exits the vehicle.
					if (pVehicle->GetDriver() && !pVehicle->IsDriverAPlayer())
					{
						//Rotate to align with velocity
						Vector3 vehDriveDirXY(vDirection.x, vDirection.y, 0.0f);
						vehDriveDirXY.Normalize();
						const Vector3 steeringDeltaXY(vVelocity.x, vVelocity.y, 0.0f);
						Vector3 steeringDirXY(steeringDeltaXY);
						steeringDirXY.Normalize();

						Vector3 turnVec(0.0f, 0.0f, 0.0f);
						turnVec.Cross(vehDriveDirXY, steeringDirXY);
						float leftRightControl = turnVec.z * HALF_PI;
						float fYawControl = rage::Clamp(leftRightControl, -HALF_PI, HALF_PI);
						pSubHandling->SetYawControl(fYawControl);
					}

					static float s_DiveScaler = 0.05f;
					float fDiveControl = Clamp( -vVelocity.z * s_DiveScaler, -1.0f, 1.0f);
					pSubHandling->SetDiveControl(fDiveControl);
					pVehicle->SetThrottle(0.0f);
				}
			}
		}
	default:
		break;
	}

	if(pVehicle->GetAiXYSpeed() < 0.5f)//are we stopped? or close enough :)
	{
		if(!(m_StopFlags&SF_DontTerminateWhenStopped))
		{
			return FSM_Quit;//if so quit
		}

		if(mightDoTimeslicing && (m_StopFlags & SF_EnableTimeslicingWhenStill))
		{
			// We are basically sitting still, allow timeslicing.
			pVehicle->m_nVehicleFlags.bLodCanUseTimeslicing = true;
			pVehicle->m_nVehicleFlags.bLodShouldUseTimeslicing = true;
		}
	}

	return FSM_Continue;//keep braking
}


//////////////////////////////////////////////////////////////////////

CTask::FSM_Return CTaskVehicleStop::UpdateFSM( const s32 iState, const FSM_Event iEvent )
{
	CVehicle *pVehicle = GetVehicle(); //Get the vehicle ptr.

	FSM_Begin
	// Stop
	FSM_State(State_Stop)
		FSM_OnUpdate
		return Stop_OnUpdate(pVehicle);
	FSM_End
}

CTask::FSM_Return	CTaskVehicleStop::ProcessPreFSM()
{
	CVehicle *pVehicle = GetVehicle(); //Get the vehicle ptr.

	// stopping cars can become become dummies. Don't know why the law enforcement check is here, this was copied from old logic in the vehicle LOD manager
	if (!pVehicle->IsLawEnforcementCar())
	{
		pVehicle->m_nVehicleFlags.bTasksAllowDummying = true;
	}

	return FSM_Continue;
}

void CTaskVehicleStop::CloneUpdate(CVehicle* pVehicle)
{
    CTaskVehicleMissionBase::CloneUpdate(pVehicle);

    pVehicle->m_nVehicleFlags.bTasksAllowDummying = true;
}

void CTaskVehicleStop::CleanUp()
{
	CVehicle *pVehicle = GetVehicle(); //Get the vehicle ptr.
	if ( pVehicle )
	{
		// prevent boats having negative throttle
		pVehicle->SetThrottle(0.0f);
	}

}

float CTaskVehicleStop::ComputeThrottleForSpeed(CVehicle& in_Vehicle, float in_Speed)
{
	Vector3 vVehicleVelocity = in_Vehicle.GetVelocity();
	Vector3 vVehicleForward = VEC3V_TO_VECTOR3(in_Vehicle.GetTransform().GetForward());
	float fSpeed = vVehicleVelocity.XYMag();

	float fForwardSpeed = 0.0f;
	if (fSpeed > SMALL_FLOAT)
	{
		Vector3 vVehicleDirection = vVehicleVelocity * 1.0f / fSpeed;
		fForwardSpeed = vVehicleDirection.Dot(vVehicleForward) * fSpeed;
	}

	float fDeltaSpeed = in_Speed - fForwardSpeed;

	static float s_SpeedScalar = 0.05f;
	return Clamp( fDeltaSpeed * s_SpeedScalar, -1.0f, 1.0f);
}


//////////////////////////////////////////////////////////////////////

#if !__FINAL
const char * CTaskVehicleStop::GetStaticStateName( s32 iState  )
{
	Assert(iState>=State_Stop&&iState<=State_Stop);
	static const char* aStateNames[] = 
	{
		"State_Stop"
	};

	return aStateNames[iState];
}
#endif


//////////////////////////////////////////////////////////////////////

CTaskVehiclePullOver::CTaskVehiclePullOver(bool bForcePullOverRightAway, ForcePulloverDirection forceDirection, bool bJustPullOverToSideOfCurrentLane)
: m_bForcePullOverRightAway(bForcePullOverRightAway)
, m_ForcePulloverInDirection(forceDirection)
, m_bJustPullOverToSideOfCurrentLane(bJustPullOverToSideOfCurrentLane)
{
	m_bRightSideOfRoad = false;
	m_bFirstUpdateInCruise = true;

	m_pFollowRoute = rage_new CVehicleFollowRouteHelper();

	SetInternalTaskType(CTaskTypes::TASK_VEHICLE_PULL_OVER);
}

CTask::FSM_Return CTaskVehiclePullOver::ProcessPreFSM()
{
	UpdateIndicatorsForCar(GetVehicle());
	return FSM_Continue;
}

void CTaskVehiclePullOver::CleanUp()
{
	if (GetVehicle())
	{
		GetVehicle()->m_nVehicleFlags.bLeftIndicator	= false;
		GetVehicle()->m_nVehicleFlags.bRightIndicator	= false;
	}
}

//////////////////////////////////////////////////////////////////////

CTask::FSM_Return CTaskVehiclePullOver::UpdateFSM( const s32 iState, const FSM_Event iEvent )
{
	CVehicle *pVehicle = GetVehicle(); //Get the vehicle ptr.

	FSM_Begin

		// Cruise
		FSM_State(State_Cruise)
			FSM_OnEnter
				Cruise_OnEnter(pVehicle);
			FSM_OnUpdate
				return Cruise_OnUpdate(pVehicle);

		// HeadForTarget
		FSM_State(State_HeadForTarget)
			FSM_OnEnter
				HeadForTarget_OnEnter(pVehicle);
			FSM_OnUpdate
				return HeadForTarget_OnUpdate(pVehicle);

		FSM_State(State_Slowdown)
			FSM_OnEnter
				Slowdown_OnEnter(pVehicle);
			FSM_OnUpdate
				return Slowdown_OnUpdate(pVehicle);

		// Stop
		FSM_State(State_Stop)
			FSM_OnEnter
				Stop_OnEnter(pVehicle);
			FSM_OnUpdate
				return Stop_OnUpdate(pVehicle);
	FSM_End
}


//////////////////////////////////////////////////////////////////////
//Cruise
//////////////////////////////////////////////////////////////////////
void		CTaskVehiclePullOver::Cruise_OnEnter(CVehicle* pVehicle)
{
	//remember whether this is the first update in this state or not, so we don't
	//exit early for not having a node list
	m_bFirstUpdateInCruise = true;

	SetNewTask( CVehicleIntelligence::CreateCruiseTask(*pVehicle, m_Params));
}


//////////////////////////////////////////////////////////////////////

CTask::FSM_Return	CTaskVehiclePullOver::Cruise_OnUpdate(CVehicle* pVehicle)
{
	if (m_bFirstUpdateInCruise)
	{
		m_bFirstUpdateInCruise = false;
		return FSM_Continue;
	}

	CTaskVehicleCruiseNew* pSubtask = static_cast<CTaskVehicleCruiseNew*>(FindSubTaskOfType(CTaskTypes::TASK_VEHICLE_CRUISE_NEW));
	CVehicleNodeList * pNodeList = pSubtask ? pSubtask->GetNodeList() : NULL;
	if(!pNodeList)
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	Vector3	TargetCoors;
	CNodeAddress oldNode, newNode, futureNode;

	Assert(pNodeList->GetTargetNodeIndex() > 0);	//this should never be 0
	oldNode = pNodeList->GetPathNodeAddr(pNodeList->GetTargetNodeIndex()-1);
	newNode = pNodeList->GetPathNodeAddr(pNodeList->GetTargetNodeIndex());
	futureNode = pNodeList->GetPathNodeAddr(rage::Min(pNodeList->GetTargetNodeIndex()+1, CVehicleNodeList::CAR_MAX_NUM_PATHNODES_STORED-1));

	if(oldNode.IsEmpty() || newNode.IsEmpty() ||(!ThePaths.IsRegionLoaded(oldNode)) ||(!ThePaths.IsRegionLoaded(newNode)))
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	const CPathNode *pOldNode = ThePaths.FindNodePointer(oldNode);
	const CPathNode *pNewNode = ThePaths.FindNodePointer(newNode);

	s16 iLink = -1;
	const bool bLinkFound = ThePaths.FindNodesLinkIndexBetween2Nodes(oldNode, newNode, iLink);
	if(!bLinkFound)//can't find the link so just stop.
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	const CPathNodeLink *pLink = &ThePaths.GetNodesLink(pOldNode, iLink);

	const CPathNode *pFutureNode = 0;
	if(!futureNode.IsEmpty() && ThePaths.IsRegionLoaded(futureNode))
	{
		pFutureNode = ThePaths.FindNodePointer(futureNode);
	}

	Vector2 oldNodeCoors;
	Vector2 newNodeCoors;

	pOldNode->GetCoors2(oldNodeCoors);
	pNewNode->GetCoors2(newNodeCoors);

	Vector2	diff = newNodeCoors - oldNodeCoors;
	diff.Normalize();

	// First we wait until we have a sufficiently straight bit of road.
	// Also, don't pull over on a sliplane
	//if(pOldNode->FindNumberNonSpecialNeighbours() == 2 && pNewNode->FindNumberNonSpecialNeighbours() == 2 &&((!pFutureNode)||(pFutureNode->FindNumberNonSpecialNeighbours() == 2)))
	const CJunction* pOldJunction = !pOldNode->IsJunctionNode() ? NULL : CJunctions::FindJunctionFromNode(oldNode);
	const CJunction* pNewJunction = !pNewNode->IsJunctionNode() ? NULL : CJunctions::FindJunctionFromNode(newNode);
	const CJunction* pFutureJunction = !pFutureNode || !pFutureNode->IsJunctionNode() ? NULL : CJunctions::FindJunctionFromNode(futureNode);
	const bool bOldNodeIsNonFakeJunction = pOldJunction && !pOldJunction->IsOnlyJunctionBecauseHasSwitchedOffEntrances();
	const bool bNewNodeIsNonFakeJunction = pNewJunction && !pNewJunction->IsOnlyJunctionBecauseHasSwitchedOffEntrances();
	const bool bFutureNodeIsNonFakeJunction = pFutureJunction && !pFutureJunction->IsOnlyJunctionBecauseHasSwitchedOffEntrances();
	if (m_bForcePullOverRightAway || (!bOldNodeIsNonFakeJunction && !pOldNode->IsSlipLane() 
		&& !bNewNodeIsNonFakeJunction && !pNewNode->IsSlipLane()
		&& !pLink->IsDontUseForNavigation()
		&& (!pFutureNode || (!bFutureNodeIsNonFakeJunction && !pFutureNode->IsSlipLane()))))
	{
		m_bRightSideOfRoad = true;
		// Only if we are on a one way street will we consider stopping on the left side of the road.
		//TODO: differentiate between one way roads and roads that have been split
		if(pLink->m_1.m_LanesFromOtherNode == 0)
		{
			// Bit of a fudge to get taxis to stop near the player
			Vector3	playerCoors = CGameWorld::FindLocalPlayerCoors();
			const Vector3 vVehiclePosition = VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition());
			if(CVehicle::IsTaxiModelId(pVehicle->GetModelId()) &&((playerCoors - vVehiclePosition).Mag2() < 40.0f * 40.0f))
			{
				if(diff.Cross(Vector2(playerCoors, Vector2::kXY)-oldNodeCoors) > 0)
				{
					m_bRightSideOfRoad = false;
				}
			}
			else if(diff.Cross(Vector2(vVehiclePosition, Vector2::kXY)-oldNodeCoors) > 0)
			{
				m_bRightSideOfRoad = false;
			}
		}

		//override
		if (m_ForcePulloverInDirection == Force_Left)
		{
			m_bRightSideOfRoad = false;
		}
		else if (m_ForcePulloverInDirection == Force_Right)
		{
			m_bRightSideOfRoad = true;
		}
		CopyNodeList(pNodeList);
		SetState(State_HeadForTarget);
		return FSM_Continue;
	}

	return FSM_Continue;
}

void CTaskVehiclePullOver::CopyNodeList(const CVehicleNodeList * pNodeList)
{
	if(pNodeList)
	{
		//Assert(m_pNodeList);
		sysMemCpy(&m_NodeList, pNodeList, sizeof(CVehicleNodeList));

		Assert(m_pFollowRoute);
		m_pFollowRoute->ConstructFromNodeList(GetVehicle(), m_NodeList);
	}
}

//////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////
float CTaskVehiclePullOver::GetPointToAimFor(CVehicle *pVehicle, Vector3 *pPointToAimFor, float& fDotProductOut)
{
	CVehicleNodeList * pNodeList = pVehicle->GetIntelligence()->GetNodeList();
	Assertf(pNodeList, "CTaskVehiclePullOver::GetPointToAimFor - vehicle has no node list");
	if(!pNodeList)
		return 0.0f;

	Vector3	TargetCoors;
	CNodeAddress oldNode, newNode, futureNode;

	Assert(pNodeList->GetTargetNodeIndex() > 0);	//this should never be 0
	oldNode = pNodeList->GetPathNodeAddr(pNodeList->GetTargetNodeIndex()-1);
	newNode = pNodeList->GetPathNodeAddr(pNodeList->GetTargetNodeIndex());
	futureNode = pNodeList->GetPathNodeAddr(rage::Min(pNodeList->GetTargetNodeIndex()+1, CVehicleNodeList::CAR_MAX_NUM_PATHNODES_STORED-1));

	if(oldNode.IsEmpty() || newNode.IsEmpty() ||(!ThePaths.IsRegionLoaded(oldNode)) ||(!ThePaths.IsRegionLoaded(newNode)))
	{
		SetState(State_Stop);
		return 0.0f;
	}

	const CPathNode *pOldNode = ThePaths.FindNodePointer(oldNode);
	const CPathNode *pNewNode = ThePaths.FindNodePointer(newNode);

	s16 iLink = -1;
	const bool bLinkFound = ThePaths.FindNodesLinkIndexBetween2Nodes(oldNode, newNode, iLink);
	if(!bLinkFound)//can't find the link so just stop.
	{
		SetState(State_Stop);
		return 0.0f;
	}

	const CPathNodeLink *pLink = &ThePaths.GetNodesLink(pOldNode, iLink);

	Vector2 oldNodeCoors;
	Vector2 newNodeCoors;

	pOldNode->GetCoors2(oldNodeCoors);
	pNewNode->GetCoors2(newNodeCoors);

	Vector2	diff = newNodeCoors - oldNodeCoors;
	diff.Normalize();

	Vector3 vCarForward = VEC3V_TO_VECTOR3(pVehicle->GetVehicleForwardDirection());
	vCarForward.z = 0.0f;
	fDotProductOut = vCarForward.Dot(Vector3(diff.x, diff.y, 0.0f));

	// Find the coordinates of the curb(where we want to stop)
	float roadWidthOnLeft, roadWidthOnRight;
#if __DEV
	const bool bAllLanesThoughCentre = ThePaths.bMakeAllRoadsSingleTrack ||(pLink->m_1.m_Width == ALL_LANES_THROUGH_CENTRE_FLAG_VAL);
#else
	const bool bAllLanesThoughCentre = (pLink->m_1.m_Width == ALL_LANES_THROUGH_CENTRE_FLAG_VAL);
#endif // __DEV
	ThePaths.FindRoadBoundaries(pLink->m_1.m_LanesToOtherNode, pLink->m_1.m_LanesFromOtherNode, static_cast<float>(pLink->m_1.m_Width), pLink->m_1.m_NarrowRoad, bAllLanesThoughCentre, roadWidthOnLeft, roadWidthOnRight);

	float	centreToCurbDist;
	const float fCarWidth = pVehicle->GetBoundingBoxMax().x;
	if(m_bRightSideOfRoad)
	{
		centreToCurbDist = roadWidthOnRight;
		centreToCurbDist -= fCarWidth;
		centreToCurbDist = rage::Max(0.0f, centreToCurbDist);
	}
	else
	{
		centreToCurbDist = -roadWidthOnLeft;
		centreToCurbDist += fCarWidth;
		centreToCurbDist = rage::Min(0.0f, centreToCurbDist);
	}

	Vector2 curbPoint = oldNodeCoors;
	curbPoint.x += centreToCurbDist * diff.y;
	curbPoint.y -= centreToCurbDist * diff.x;

	//draw the curb line
	//Vector3 vCurbStart(curbPoint.x, curbPoint.y, pOldNode->GetPos().z);
	//CVehicle::ms_debugDraw.AddLine(VECTOR3_TO_VEC3V(vCurbStart), VECTOR3_TO_VEC3V(vCurbStart + Vector3(diff.x, diff.y, 0.0f)), Color_yellow);

	// Find our cars' coordinate projected onto the curb line
	const Vector3 vVehiclePosition = VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition());
	Vector2	carPos = Vector2(vVehiclePosition, Vector2::kXY);
	float	distAlongLine = diff.Dot(carPos - curbPoint);
	Vector2 carOnCurbLine;
	carOnCurbLine.x = curbPoint.x + distAlongLine * diff.x;
	carOnCurbLine.y = curbPoint.y + distAlongLine * diff.y;

	//CVehicle::ms_debugDraw.AddSphere(Vec3V(carOnCurbLine.x, carOnCurbLine.y, pOldNode->GetPos().z), 0.33f, Color_green);

	float	distFromCurbLine =(carPos - carOnCurbLine).Mag();

	//...hack!
	//if we want to just pull over to the side of our current lane
	//subtract the width of a lane until distFromCurbLine < lane width
	if (m_bJustPullOverToSideOfCurrentLane)
	{
		while (distFromCurbLine > pLink->GetLaneWidth())
		{
			distFromCurbLine -= pLink->GetLaneWidth();
		}
	}

	// We try to aim for a point a bit further on the line.
	float	forwardAim = rage::Max(4.0f, distFromCurbLine * 2.3f);
	if (m_bForcePullOverRightAway && m_bJustPullOverToSideOfCurrentLane)
	{
		static dev_float fCopSirenPulloverMult = 2.0f;
		forwardAim *= fCopSirenPulloverMult;
	}

	Vector2 pointToAimFor = carOnCurbLine + diff * forwardAim;

	pPointToAimFor->x = pointToAimFor.x;
	pPointToAimFor->y = pointToAimFor.y; 
	pPointToAimFor->z = 0.0f;

	//CVehicle::ms_debugDraw.AddSphere(Vec3V(pointToAimFor.x, pointToAimFor.y, pOldNode->GetPos().z), 0.33f, Color_purple);

	// Print a bunch of debug lines.
#if __DEV
	if(CVehicleIntelligence::m_bDisplayCarAiDebugInfo && CVehicleIntelligence::AllowCarAiDebugDisplayForThisVehicle(pVehicle))
	{
		float	displayZ = vVehiclePosition.z + 2.0f;
		grcDebugDraw::Line(Vector3(carOnCurbLine.x, carOnCurbLine.y, displayZ), 
			Vector3(carOnCurbLine.x, carOnCurbLine.y, displayZ), 
			Color32(255, 0, 0, 255));
		grcDebugDraw::Line(Vector3(pPointToAimFor->x, pPointToAimFor->y, displayZ), 
			Vector3(pPointToAimFor->x, pPointToAimFor->y, displayZ), 
			Color32(255, 255, 0, 255));
		grcDebugDraw::Line(Vector3(vVehiclePosition.x, vVehiclePosition.y, displayZ), 
			Vector3(pPointToAimFor->x, pPointToAimFor->y, displayZ), 
			Color32(255, 255, 255, 255));

	}
#endif // __DEV

	return distFromCurbLine;
}

//
//
// HeadForTarget
void		CTaskVehiclePullOver::HeadForTarget_OnEnter(CVehicle* pVehicle)
{
	Vector3 pointToAimFor;
	float fDot = 0.0f;
	GetPointToAimFor(pVehicle, &pointToAimFor, fDot);

	sVehicleMissionParams params;
	params.m_fCruiseSpeed = GetCruiseSpeed();
	params.SetTargetPosition(pointToAimFor);
	params.m_fTargetArriveDist = 0.0f;
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundStationaryCars);	//don't steer around parked cars if there's one in the way

	SetNewTask(rage_new CTaskVehicleGoToPointWithAvoidanceAutomobile(params));
}

bool	CTaskVehiclePullOver::HeadingIsWithinToleranceForVehicle(CVehicle* pVehicle, float fDot) const
{
	const float fDrivingAbility = CDriverPersonality::FindDriverAbility(pVehicle->GetDriver(), pVehicle);

	//simple case-based logic
	if (fDrivingAbility < 0.35f)
	{	
		return true;
	}
	else if (fDrivingAbility < 0.75f)
	{
		return fDot > 0.96f;
	}
	else
	{
		return fDot > 0.99f;
	}
}

//////////////////////////////////////////////////////////////////////
CTask::FSM_Return	CTaskVehiclePullOver::HeadForTarget_OnUpdate(CVehicle* pVehicle)
{
	Vector3 pointToAimFor;
	float fDot = 0.0f;
	float distFromCurbLine = GetPointToAimFor(pVehicle, &pointToAimFor, fDot);

	// Limit the speed depending on our distance to the curb.
	SetCruiseSpeed(rage::Min(GetCruiseSpeed(), 10.0f));
	if(distFromCurbLine < 2.0f)
	{
		SetCruiseSpeed(rage::Min(GetCruiseSpeed(), 5.0f));
	}

	//get the goto task and update it with the latest point to aim for.
	if(GetSubTask() && GetSubTask()->GetTaskType()==CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE)
	{
		CTaskVehicleGoTo * pGoToTask = (CTaskVehicleGoToPointWithAvoidanceAutomobile*)GetSubTask();

		pGoToTask->SetTargetPosition(&pointToAimFor);
	}

	//check whether we have reached our goal
	if(pVehicle->GetAiXYSpeed() < 0.5f || 
		(distFromCurbLine < 0.5f && HeadingIsWithinToleranceForVehicle(pVehicle, fDot)))
	{
		SetState(State_Slowdown);
	}

	return FSM_Continue;
}

//stop, but in a way that allows us to keep steering
void	CTaskVehiclePullOver::Slowdown_OnEnter(CVehicle* pVehicle)
{
	Vector3 pointToAimFor;
	float fDot = 0.0f;
	GetPointToAimFor(pVehicle, &pointToAimFor, fDot);

	sVehicleMissionParams params;
	params.m_fCruiseSpeed = 0.0f;
	params.SetTargetPosition(pointToAimFor);
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundStationaryCars);	//don't steer around parked cars if there's one in the way
	SetNewTask(rage_new CTaskVehicleGoToPointWithAvoidanceAutomobile(params));
}

aiTask::FSM_Return CTaskVehiclePullOver::Slowdown_OnUpdate(CVehicle* pVehicle)
{
	if (pVehicle->GetAiXYSpeed() < 0.25f || GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE))
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	//get the goto task and update it with the latest point to aim for.
	if(GetSubTask() && GetSubTask()->GetTaskType()==CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE)
	{
		CTaskVehicleGoTo * pGoToTask = (CTaskVehicleGoToPointWithAvoidanceAutomobile*)GetSubTask();

		Vector3 pointToAimFor;
		float fDot = 0.0f;
		GetPointToAimFor(pVehicle, &pointToAimFor, fDot);

		pGoToTask->SetTargetPosition(&pointToAimFor);
	}

	//CVehicle::ms_debugDraw.AddLine(pVehicle->GetVehiclePosition(), pVehicle->GetVehiclePosition() + Vec3V(0.0f, 0.0f, 5.0f), Color_orange);

	return FSM_Continue;
}

//////////////////////////////////////////////////////////////////////
//State_Stop
//////////////////////////////////////////////////////////////////////
void		CTaskVehiclePullOver::Stop_OnEnter(CVehicle* UNUSED_PARAM(pVehicle))
{
	SetNewTask(rage_new CTaskVehicleStop());
}

/////////////////////////////////////////////////////////////////////////////////

CTask::FSM_Return	CTaskVehiclePullOver::Stop_OnUpdate(CVehicle* UNUSED_PARAM(pVehicle))
{
	if (GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_STOP))
	{
		return FSM_Quit;
	}
	return FSM_Continue;
}

/////////////////////////////////////////////////////////////////////////////////
// FUNCTION : UpdateIndicatorsForCar
// PURPOSE :  Sets the indicator bits in the Vehicle flags if the car is planning
//			  to turn off and not on a mission.
/////////////////////////////////////////////////////////////////////////////////
void CTaskVehiclePullOver::UpdateIndicatorsForCar(CVehicle* pVeh)
{
	Assert(pVeh);

	if(m_bRightSideOfRoad)//!m_bCommitted 
	{
		pVeh->m_nVehicleFlags.bRightIndicator = true;
		pVeh->m_nVehicleFlags.bLeftIndicator	= false;
	}
	else
	{
		pVeh->m_nVehicleFlags.bLeftIndicator = true;
		pVeh->m_nVehicleFlags.bRightIndicator	= false;
	}
}



//////////////////////////////////////////////////////////////////////

#if !__FINAL
const char * CTaskVehiclePullOver::GetStaticStateName( s32 iState  )
{
	Assert(iState>=State_Cruise&&iState<=State_Stop);
	static const char* aStateNames[] = 
	{
		"State_Cruise",
		"State_HeadForTarget",
		"State_Slowdown",
		"State_Stop",
	};

	return aStateNames[iState];
}
#endif

//////////////////////////////////////////////////////////////////////

CTaskVehiclePassengerExit::CTaskVehiclePassengerExit(const Vector3& target)
: m_vTarget(target)
, m_uNumPassengersToExit(0)
{
	SetInternalTaskType(CTaskTypes::TASK_VEHICLE_PASSENGER_EXIT);
}

CTask::FSM_Return CTaskVehiclePassengerExit::ProcessPreFSM()
{
	return FSM_Continue;
}

CTask::FSM_Return CTaskVehiclePassengerExit::UpdateFSM( const s32 iState, const FSM_Event iEvent )
{
	CVehicle *pVehicle = GetVehicle(); //Get the vehicle ptr.

	FSM_Begin
		FSM_State(State_GoToTarget)
		FSM_OnEnter
		GoToTarget_OnEnter(pVehicle);
	FSM_OnUpdate
		return GoToTarget_OnUpdate(pVehicle);

	FSM_State(State_Stop)
		FSM_OnEnter
		Stop_OnEnter(pVehicle);
	FSM_OnUpdate
		return Stop_OnUpdate(pVehicle);

	FSM_State(State_WaitForPassengersToExit)
		FSM_OnEnter
		WaitForPassengersToExit_OnEnter(pVehicle);
	FSM_OnUpdate
		return WaitForPassengersToExit_OnUpdate(pVehicle);
	FSM_End
}

void CTaskVehiclePassengerExit::GoToTarget_OnEnter(CVehicle* UNUSED_PARAM(pVehicle))
{
	sVehicleMissionParams params = m_Params;
	params.m_fTargetArriveDist = 1.f;
	params.SetTargetPosition(m_vTarget);
	SetNewTask(rage_new CTaskVehicleGoToPointWithAvoidanceAutomobile(params));
}

CTask::FSM_Return	CTaskVehiclePassengerExit::GoToTarget_OnUpdate(CVehicle* UNUSED_PARAM(pVehicle))
{
	if (GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE))
	{
		SetState(State_Stop);
	}

	return FSM_Continue;
}

void	CTaskVehiclePassengerExit::Stop_OnEnter(CVehicle* UNUSED_PARAM(pVehicle))
{
	SetNewTask(rage_new CTaskVehicleStop());
}

CTask::FSM_Return	CTaskVehiclePassengerExit::Stop_OnUpdate(CVehicle* UNUSED_PARAM(pVehicle))
{
	if (GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_STOP))
	{
		SetState(State_WaitForPassengersToExit);
	}
	return FSM_Continue;
}

void				CTaskVehiclePassengerExit::WaitForPassengersToExit_OnEnter(CVehicle* pVehicle)
{
	//Figure out how many passengers should exit.
	// We'll use this scenario point's probability to determine how many passengers leave, 
	// i.e. 50% = half of passengers get off.
	m_uNumPassengersToExit = 0;
	s32 iTotalPassengers = pVehicle->GetNumberOfPassenger();
	if (iTotalPassengers > 0)
	{
		CScenarioPoint* pCurrentPoint = GetDriverScenarioPoint(pVehicle);
		if (pCurrentPoint)
		{
			float fPercentageToExit;
			if (pCurrentPoint->HasProbabilityOverride())
			{
				fPercentageToExit = pCurrentPoint->GetProbabilityOverride();
			}
			else
			{
				const float fMinPercentage = 0.25f;
				const float fMaxPercentage = 0.75f;
				fPercentageToExit = fwRandom::GetRandomNumberInRange(fMinPercentage, fMaxPercentage);
			}
			m_uNumPassengersToExit = static_cast<u32>(ceilf(fPercentageToExit * static_cast<float>(iTotalPassengers)));

		}
	}

	//Tell the passengers to get off.
	if (m_uNumPassengersToExit > 0)
	{
		m_exitingPeds.Reserve(m_uNumPassengersToExit);
		atArray<int> aiSeatIndecesToExit;
		pVehicle->PickRandomPassengers(m_uNumPassengersToExit, aiSeatIndecesToExit);

		for (int iSeatIndex = 0; iSeatIndex < aiSeatIndecesToExit.size(); ++iSeatIndex)
		{
			CPed* pPedToExit = pVehicle->GetPedInSeat(aiSeatIndecesToExit[iSeatIndex]);
			GivePedExitTask(pPedToExit, pVehicle);
			RegdPed regdExitingPed(pPedToExit);
			m_exitingPeds.Push(regdExitingPed);
		}
	}
}

CTask::FSM_Return  CTaskVehiclePassengerExit::WaitForPassengersToExit_OnUpdate(CVehicle* UNUSED_PARAM(pVehicle))
{
	// Keep track of passengers as they leave.
	for (int scan = m_exitingPeds.size() - 1; scan >= 0; --scan)
	{
		if (m_exitingPeds[scan] == NULL)
		{
			m_exitingPeds.DeleteFast(scan);
		}
		else if ( !m_exitingPeds[scan]->GetPedIntelligence()->HasEventOfType(EVENT_GIVE_PED_TASK) &&
			NULL == m_exitingPeds[scan]->GetPedIntelligence()->FindTaskActiveByType(CTaskTypes::TASK_EXIT_VEHICLE) )
		{
			// This ped has exited, so delete it off the list.
			m_exitingPeds.DeleteFast(scan);
		}
	}

	if (m_exitingPeds.size() > 0)
	{	
		return FSM_Continue;
	}
	else
	{
		return FSM_Quit;
	}
}

CScenarioPoint* CTaskVehiclePassengerExit::GetDriverScenarioPoint(const CVehicle* pVehicle) const
{
	if (pVehicle)
	{
		CPed* pDriver = pVehicle->GetDriver();
		if (pDriver && pDriver->GetPedIntelligence())
		{
			CTaskUseVehicleScenario* pVehicleScenarioTask = static_cast<CTaskUseVehicleScenario*>(pDriver->GetPedIntelligence()->FindTaskByType(CTaskTypes::TASK_USE_VEHICLE_SCENARIO));
			if (pVehicleScenarioTask)
			{
				return pVehicleScenarioTask->GetScenarioPoint();
			}
		}
	}
	return NULL;
}

void CTaskVehiclePassengerExit::GivePedExitTask(CPed* pPed, CVehicle* pVehicle) const
{
	if (aiVerify(pPed && pPed->GetPedIntelligence()))
	{
		VehicleEnterExitFlags vehicleFlags;
		vehicleFlags.BitSet().Set(CVehicleEnterExitFlags::ExitToWalk);
		CTask* pLeaveCarTask = rage_new CTaskExitVehicle(pVehicle, vehicleFlags);
		CEventGivePedTask givePedTask(PED_TASK_PRIORITY_EVENT_RESPONSE_TEMP, pLeaveCarTask);
		pPed->GetPedIntelligence()->AddEvent(givePedTask);
	}
}

/////////////////////////////////////////////////////////////////////
//	CTaskVehicleParkNew
/////////////////////////////////////////////////////////////////////

CTaskVehicleParkNew::Tunables CTaskVehicleParkNew::sm_Tunables;
IMPLEMENT_VEHICLE_AI_TASK_TUNABLES(CTaskVehicleParkNew, 0xff036ca8);

// TODO: Would be nice to move these into CTaskVehicleParkNew::Tunables.
dev_float CTaskVehicleParkNew::ms_fCruiseSpeedToParkBoat = 6.0f;
dev_float CTaskVehicleParkNew::ms_fCruiseSpeedToPark = 4.0f;
dev_float CTaskVehicleParkNew::ms_fParkingSpaceHalfLength = 2.0f;
dev_float CTaskVehicleParkNew::ms_fParkingSpaceHalfWidth = 1.2f;
dev_float CTaskVehicleParkNew::ms_fPositionXTolerance = 0.55f;
dev_float CTaskVehicleParkNew::ms_fPositionYTolerance = 0.75f;
dev_float CTaskVehicleParkNew::ms_fPositionXToleranceBike = 1.1f;
dev_float CTaskVehicleParkNew::ms_fPositionYToleranceBike = 1.5f;
dev_float CTaskVehicleParkNew::ms_fPositionXTolerancePlane = 4.0f;
dev_float CTaskVehicleParkNew::ms_fPositionYTolerancePlane = 16.0f;
dev_u16 CTaskVehicleParkNew::ms_iParkingStuckTime = 1000;

CTaskVehicleParkNew::CTaskVehicleParkNew(const sVehicleMissionParams& params, const Vector3& direction, const ParkType parkType
	, const float toleranceRadians, const bool bKeepLightsOn)
: CTaskVehicleMissionBase(params)
, m_SpaceDir(direction)
, m_ParkType(parkType)
, m_HeadingToleranceRadians(toleranceRadians)
, m_NumTimesParkingSpaceBlocked(0)
, m_bKeepLightsOnAfterParking(bKeepLightsOn)
, m_iMaxPathSearchDistance(-1)
, m_bDoTestsForBlockedSpace(false)
{
	SetInternalTaskType(CTaskTypes::TASK_VEHICLE_PARK_NEW);
}

CTaskVehicleParkNew::~CTaskVehicleParkNew()
{
}

aiTask* CTaskVehicleParkNew::Copy() const
{
	CTaskVehicleParkNew* pTaskCopy = rage_new CTaskVehicleParkNew(m_Params, m_SpaceDir, m_ParkType, m_HeadingToleranceRadians, m_bKeepLightsOnAfterParking);
	if(pTaskCopy)
	{
		pTaskCopy->SetMaxPathSearchDistance(GetMaxPathSearchDistance());
		pTaskCopy->SetDoTestsForBlockedSpace(GetDoTestsForBlockedSpace());
	}
	return pTaskCopy;
}

CTask::FSM_Return CTaskVehicleParkNew::ProcessPreFSM()
{
	CVehicle *pVehicle = GetVehicle(); //Get the vehicle ptr.
	if (pVehicle)
	{
		pVehicle->m_nVehicleFlags.bPreventBeingSuperDummyThisFrame = true;
	}

	return FSM_Continue;
}

CTask::FSM_Return CTaskVehicleParkNew::UpdateFSM(const s32 iState, const FSM_Event iEvent)
{
	CVehicle *pVehicle = GetVehicle(); 

	FSM_Begin
		// Start
		FSM_State(State_Start)
			FSM_OnEnter
				Start_OnEnter(pVehicle);
			FSM_OnUpdate
				return Start_OnUpdate(pVehicle);

		// Navigate
		FSM_State(State_Navigate)
			FSM_OnEnter
				Navigate_OnEnter(pVehicle);
			FSM_OnUpdate
				return Navigate_OnUpdate(pVehicle);

		// ForwardIntoSpace
		FSM_State(State_ForwardIntoSpace)
			FSM_OnEnter
				ForwardIntoSpace_OnEnter(pVehicle);
			FSM_OnUpdate
				return ForwardIntoSpace_OnUpdate(pVehicle);

		// BackIntoSpace
		FSM_State(State_BackIntoSpace)
			FSM_OnEnter
				BackIntoSpace_OnEnter(pVehicle);
			FSM_OnUpdate
				return BackIntoSpace_OnUpdate(pVehicle);

		// BackAwayFromSpace
		FSM_State(State_BackAwayFromSpace)
			FSM_OnEnter
				BackAwayFromSpace_OnEnter(pVehicle);
			FSM_OnUpdate
				return BackAwayFromSpace_OnUpdate(pVehicle);

		// PullForwardParallel
		FSM_State(State_PullForward)
			FSM_OnEnter
				PullForward_OnEnter(pVehicle);
			FSM_OnUpdate
				return PullForward_OnUpdate(pVehicle);

		FSM_State(State_PullOver)
			FSM_OnEnter
				PullOver_OnEnter(pVehicle);
			FSM_OnUpdate
				return PullOver_OnUpdate(pVehicle);

		FSM_State(State_PassengerExit)
			FSM_OnEnter
				PassengerExit_OnEnter(pVehicle);
			FSM_OnUpdate
				return PassengerExit_OnUpdate(pVehicle);

		// Stop
		FSM_State(State_Stop)
			FSM_OnEnter
				Stop_OnEnter(pVehicle);
			FSM_OnUpdate
				return Stop_OnUpdate(pVehicle);

		// Failed
		FSM_State(State_Failed)
			FSM_OnEnter
				Failed_OnEnter();
			FSM_OnUpdate
				return Failed_OnUpdate();

	FSM_End
}

#if !__FINAL
const char * CTaskVehicleParkNew::GetStaticStateName( s32 iState  )
{
	Assert(iState>=State_Start&&iState<=State_Failed);
	static const char* aStateNames[] = 
	{
		"State_Start",
		"State_Navigate",
		"State_ForwardIntoSpace",
		"State_BackIntoSpace",
		"State_BackAwayFromSpace",
		"State_PullForward",		//before backing in, pull up in front of the space
		"State_PullOver",
		"State_PassengerExit",
		"State_Stop",
		"State_Failed"
	};

	return aStateNames[iState];
}

void CTaskVehicleParkNew::Debug() const
{
#if DEBUG_DRAW
	//draw the parking space
	Vector3 vSpaceNormal(m_SpaceDir.y, -m_SpaceDir.x, 0.0f);
	Vector3 vSpaceNormalScaled = vSpaceNormal * ms_fParkingSpaceHalfWidth;
	Vector3 vSpaceDirScaled = m_SpaceDir * ms_fParkingSpaceHalfLength;
	Vector3 p1 = m_Params.GetTargetPosition() + vSpaceDirScaled + vSpaceNormalScaled;
	Vector3 p2 = m_Params.GetTargetPosition() + -vSpaceDirScaled + vSpaceNormalScaled;
	Vector3 p3 = m_Params.GetTargetPosition() + -vSpaceDirScaled + -vSpaceNormalScaled;
	Vector3 p4 = m_Params.GetTargetPosition() + vSpaceDirScaled + -vSpaceNormalScaled;

	grcDebugDraw::Line(p1, p2, Color_green3);
	grcDebugDraw::Line(p2, p3, Color_green3);
	grcDebugDraw::Line(p3, p4, Color_green3);
	grcDebugDraw::Line(p4, p1, Color_green3);

	//look through our subtasks and find a point to draw if we're navigating toward one
	aiTask* pSubtask = FindSubTaskOfType(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE);
	if (pSubtask)
	{
		const Vector3* pTargetPos = static_cast<CTaskVehicleGoToPointWithAvoidanceAutomobile*>(pSubtask)->GetTargetPosition();
		if (pTargetPos)
		{
			grcDebugDraw::Sphere(*pTargetPos, 0.5f, Color_WhiteSmoke, false);
		}
	}

#endif //DEBUG_DRAW
}
#endif	//!__FINAL

//Start
void		CTaskVehicleParkNew::Start_OnEnter(CVehicle* /*pVehicle*/)
{
}

CTask::FSM_Return	CTaskVehicleParkNew::Start_OnUpdate(CVehicle* /*pVehicle*/)
{
	m_SpaceDir.NormalizeSafe(ORIGIN);
	SetState(State_Navigate);
	return FSM_Continue;
}

void		CTaskVehicleParkNew::PullOver_OnEnter(CVehicle* /*pVehicle*/)
{
	const bool bPullOverImmediate = m_ParkType == Park_PullOverImmediate;
	SetNewTask(rage_new CTaskVehiclePullOver(bPullOverImmediate));
}

CTask::FSM_Return	CTaskVehicleParkNew::PullOver_OnUpdate(CVehicle* /*pVehicle*/)
{
	if (!GetSubTask() || GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_PULL_OVER))
	{
		SetState(State_Stop);
	}

	return FSM_Continue;
}

void CTaskVehicleParkNew::PassengerExit_OnEnter(CVehicle* pVehicle)
{
	Vector3 vTargetPos;
	FindTargetCoors(pVehicle, vTargetPos);
	SetNewTask(rage_new CTaskVehiclePassengerExit(vTargetPos));
}

CTask::FSM_Return	CTaskVehicleParkNew::PassengerExit_OnUpdate(CVehicle* UNUSED_PARAM(pVehicle))
{
	if (!GetSubTask() || GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_PASSENGER_EXIT))
	{
		SetState(State_Stop);
	}
	return FSM_Continue;
}

static dev_float s_fCoarseNavigationDistanceXY = 20.0f;
static dev_float s_fCoarseNavigationDistanceZ = 10.0f;

//normalize the space's heading vector, and align it with the car
void CTaskVehicleParkNew::FixUpParkingSpaceDirection(CVehicle* pVehicle)
{
	if (m_ParkType == Park_Perpendicular_NoseIn
		|| m_ParkType == Park_Perpendicular_BackIn
		)
	{
		Vector3 vDeltaToCar = m_Params.GetTargetPosition() - VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition());

		//m_SpaceDir's direction may be forward or backward
		if (m_SpaceDir.Dot(vDeltaToCar) < 0.0f)
		{
			m_SpaceDir.Negate();
		}
	}
	else if (m_ParkType == Park_Parallel
			|| m_ParkType == Park_LeaveParallelSpace
			|| m_ParkType == Park_PullOver
			|| m_ParkType == Park_PullOverImmediate
			|| m_ParkType == Park_BackOutPerpendicularSpace)
	{
		Vector3 vCarHeading = VEC3V_TO_VECTOR3(pVehicle->GetVehicleForwardDirection());
		if (m_SpaceDir.Dot(vCarHeading) < 0.0f)
		{
			m_SpaceDir.Negate();
		}
	}
}

// Navigate
void		CTaskVehicleParkNew::Navigate_OnEnter(CVehicle* /*pVehicle*/)
{
	//drive using the road network until we're relatively close
	sVehicleMissionParams params = m_Params;
	params.m_fTargetArriveDist = 0.0f;
	CTaskVehicleMissionBase* pSubTask = CVehicleIntelligence::CreateAutomobileGotoTask(params, 0);
	if(pSubTask && pSubTask->GetTaskType() == CTaskTypes::TASK_VEHICLE_GOTO_AUTOMOBILE_NEW && m_iMaxPathSearchDistance >= 0)
	{
		CTaskVehicleGoToAutomobileNew* pGoToTask = static_cast<CTaskVehicleGoToAutomobileNew*>(pSubTask);
		pGoToTask->SetMaxPathSearchDistance(m_iMaxPathSearchDistance);
	}
	SetNewTask(pSubTask);

	// Reset the test helper in case it was doing something already for some reason.
	m_SpaceFreeFromObstructionsHelper.ResetTest();

	// Set up a new test, if enabled.
	CVehicle *pVehicle = GetVehicle();
	if(m_bDoTestsForBlockedSpace && pVehicle)
	{
		CBaseModelInfo* pModelInfo = pVehicle->GetBaseModelInfo();
		if(pModelInfo)
		{
			Mat34V mtrx;
			const Vec3V posV = VECTOR3_TO_VEC3V(m_Params.GetTargetPosition());
			const Vec3V dirOrigV = VECTOR3_TO_VEC3V(m_SpaceDir);
			CParkingSpaceFreeFromObstructionsTestHelper::ComputeParkingSpaceMatrix(mtrx, posV, dirOrigV);

			CParkingSpaceFreeTestVehInfo vehInfo;
			vehInfo.m_ModelInfo = pModelInfo;

			m_SpaceFreeFromObstructionsHelper.StartTest(vehInfo, mtrx, pVehicle);
		}
	}
}

CTask::FSM_Return	CTaskVehicleParkNew::Navigate_OnUpdate(CVehicle* pVehicle)
{
	Vector3 vTargetPos;
	FindTargetCoors(pVehicle, vTargetPos);

	// Update the blocked space test, if there is one.
	if(m_bDoTestsForBlockedSpace && m_SpaceFreeFromObstructionsHelper.IsTestActive())
	{
		m_SpaceFreeFromObstructionsHelper.Update();
		bool spaceFree = false;
		if(!m_SpaceFreeFromObstructionsHelper.GetTestResults(spaceFree))
		{
			// Waiting for the test to be done, don't allow us to
			// switch states just yet.
			return FSM_Continue;
		}
	}

	const Vector3 vBonnetPos = VEC3V_TO_VECTOR3(CVehicleFollowRouteHelper::GetVehicleBonnetPosition(pVehicle, IsDrivingFlagSet(DF_DriveInReverse)));
	if (!GetSubTask() || GetIsSubtaskFinished(CVehicleIntelligence::GetAutomobileGotoTaskType())
		|| ((vTargetPos - vBonnetPos).XYMag2() < s_fCoarseNavigationDistanceXY*s_fCoarseNavigationDistanceXY
		&& fabsf(vTargetPos.z - vBonnetPos.z) < s_fCoarseNavigationDistanceZ )
		)
	{

		// Handle test results.
		if(m_bDoTestsForBlockedSpace && m_SpaceFreeFromObstructionsHelper.IsTestActive())
		{
			bool spaceFree = false;
			if(m_SpaceFreeFromObstructionsHelper.GetTestResults(spaceFree))
			{
				// Check if the space is blocked.
				if(!spaceFree)
				{
					// If we have already done enough attempts, fail.
					if(m_NumTimesParkingSpaceBlocked >= sm_Tunables.m_ParkingSpaceBlockedMaxAttempts)
					{
						SetState(State_Failed);
						return FSM_Continue;
					}

					// Increase the blocked count and hang out in the Stop state until
					// the next attempt.
					m_NumTimesParkingSpaceBlocked++;
					SetState(State_Stop);
					return FSM_Continue;
				}
			}
		}

		//taskAssertf( vBonnetPos.Dist2(vTargetPos) < s_iCoarseNavigationDistance*s_iCoarseNavigationDistance
		//	, "CTaskVehicleParkNew::Navigate_OnUpdate--GotoPoint finished without getting us within the specified distance to target!");

		FixUpParkingSpaceDirection(pVehicle);

		//aiTask* pTask = NULL;
		//CTaskVehicleGoToAutomobileNew* pGotoTaskNew = NULL;

		switch (m_ParkType)
		{
		case Park_Parallel:
			if (HelperIsAnythingBlockingEntryToSpace(pVehicle))
			{
				SetState(State_PullForward);
			}
			else
			{
				SetState(State_ForwardIntoSpace);
			}
			break;
		case Park_Perpendicular_NoseIn:
			SetState(State_PullForward);
			break;
		case Park_Perpendicular_BackIn:
			SetState(State_PullForward);
			break;
		case Park_PullOver:
		case Park_PullOverImmediate:

// 			pTask = FindSubTaskOfType(CVehicleIntelligence::GetAutomobileGotoTaskType());
// 	
// 			pGotoTaskNew = pTask ? static_cast<CTaskVehicleGoToAutomobileNew*>(pTask) : NULL;
// 			if (pGotoTaskNew)
// 			{
// 				pGotoTaskNew->CopyNodeList(m_pNodeList);
// 			}
			

			SetState(State_PullOver);
			break;
		case Park_LeaveParallelSpace:
			SetState(State_BackAwayFromSpace);
			break;
		case Park_BackOutPerpendicularSpace:
			SetState(State_BackAwayFromSpace);
			break;
		case Park_PassengerExit:
			SetState(State_PassengerExit);
			break;
		default:
			taskAssertf(0, "Invalid or Non-supported ParkType!");
			break;
		}
	}

	return FSM_Continue;
}

//ForwardIntoSpace
void		CTaskVehicleParkNew::ForwardIntoSpace_OnEnter(CVehicle* pVehicle)
{
	Vector3 vSteerPos;
	HelperGetPositionToSteerTo(pVehicle, vSteerPos);

	sVehicleMissionParams params;
	params.m_fCruiseSpeed = GetCruiseSpeedForDistanceToTarget(pVehicle);
	params.m_iDrivingFlags = GetDrivingFlags();
	params.m_iDrivingFlags.ClearFlag(DF_StopForCars);
	params.m_iDrivingFlags.ClearFlag(DF_StopForPeds);
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundObjects);
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundStationaryCars);
	params.m_iDrivingFlags.ClearFlag(DF_SwerveAroundAllCars);
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundPeds);
	params.m_fTargetArriveDist = 0.0f;
	params.SetTargetPosition(vSteerPos);
	SetNewTask(rage_new  CTaskVehicleGoToPointWithAvoidanceAutomobile(params, false));


	if ( pVehicle->InheritsFromBoat() )
	{
		if ( pVehicle->GetIntelligence()->GetBoatAvoidanceHelper() )
		{
			pVehicle->GetIntelligence()->GetBoatAvoidanceHelper()->SetSlowDownEnabled(false);
			pVehicle->GetIntelligence()->GetBoatAvoidanceHelper()->SetSteeringEnabled(false);
		}
	}
}

CTask::FSM_Return	CTaskVehicleParkNew::ForwardIntoSpace_OnUpdate(CVehicle* pVehicle)
{
	if ((!GetSubTask() || GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE))
		&& GetTimeInState() > 0.0f)
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	CTaskVehicleGoToPointWithAvoidanceAutomobile* pGoToTask = NULL;
	if(GetSubTask() && GetSubTask()->GetTaskType()==CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE)
	{
		pGoToTask = static_cast<CTaskVehicleGoToPointWithAvoidanceAutomobile*>(GetSubTask());
	}

	//if we're pulling forward in a parallel park, allow us to go in front of the line a bit
	//so we can smooth out our position, then back up straight into it
	const float fAmountAllowedInFrontOfSpace = m_ParkType == Park_Parallel && !IsCurrentPositionWithinXTolerance(pVehicle)
		? pVehicle->GetBoundingBoxMax().y
		: 0.0f;


	bool bAllowStop = true;

	if ( pVehicle->InheritsFromBoat() )
	{
		if (pVehicle->GetIntelligence()->GetBoatAvoidanceHelper() )
		{
			if ( pVehicle->GetIntelligence()->GetBoatAvoidanceHelper()->IsBlocked() )
			{
				bAllowStop = false;
				if ( pVehicle->GetIntelligence()->GetBoatAvoidanceHelper()->IsBeached() )
				{
					bAllowStop = true;
				}
			}
		}
	}

	//bool bFacingForward = false;
	const float fDistInFrontOfSpace = GetDistanceInFrontOfSpace(pVehicle);
	if (bAllowStop
		&& (fDistInFrontOfSpace > fAmountAllowedInFrontOfSpace 
		|| (HelperIsVehicleStopped(pVehicle) && pGoToTask && pGoToTask->GetCruiseSpeed() <= 0.0f)
		|| pVehicle->GetIntelligence()->MillisecondsNotMoving > ms_iParkingStuckTime) )
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	if (pGoToTask)
	{
		pGoToTask->SetCruiseSpeed(GetCruiseSpeedForDistanceToTarget(pVehicle));

		Vector3 vSteerPos;
		HelperGetPositionToSteerTo(pVehicle, vSteerPos);
		pGoToTask->SetTargetPosition(&vSteerPos);
	}

// #if __DEV
// 
// 	//debugging
// 	Vector3 vDest(Vector3::ZeroType);
// 	CTaskVehicleGoToPointWithAvoidanceAutomobile* pSubtask1 = static_cast<CTaskVehicleGoToPointWithAvoidanceAutomobile*>
// 		(FindSubTaskOfType(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE));
// 	if (pSubtask1)
// 	{
// 		vDest = *pSubtask1->GetTargetPosition();
// 		grcDebugDraw::Sphere(vDest, 0.5f, Color_red4, false);
// 		grcDebugDraw::Sphere(m_vTargetPos, 0.5f, Color_green2, false);
// 
// 		Vector3 frontPos, rearPos;
// 		HelperGetParkingSpaceFrontPosition(frontPos);
// 		HelperGetParkingSpaceRearPosition(rearPos);
// 		grcDebugDraw::Sphere(frontPos, 0.3f, Color_plum4, false);
// 		grcDebugDraw::Sphere(rearPos, 0.3f, Color_blue3, false);
// 	}
// 
// #endif //__DEV

	return FSM_Continue;
}

//BackIntoSpace
void		CTaskVehicleParkNew::BackIntoSpace_OnEnter(CVehicle* pVehicle)
{
	Vector3 vSteerPos;
	HelperGetPositionToSteerTo(pVehicle, vSteerPos);

	sVehicleMissionParams params;
	params.m_fCruiseSpeed = GetCruiseSpeedForDistanceToTarget(pVehicle);
	params.m_iDrivingFlags = GetDrivingFlags()|DF_DriveInReverse;
	params.m_iDrivingFlags.ClearFlag(DF_StopForCars);
	params.m_iDrivingFlags.ClearFlag(DF_StopForPeds);
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundObjects);
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundStationaryCars);
	params.m_iDrivingFlags.ClearFlag(DF_SwerveAroundAllCars);
	params.m_iDrivingFlags.ClearFlag(DF_SteerAroundPeds);
	params.m_fTargetArriveDist = 0.0f;
	params.SetTargetPosition(vSteerPos);
	SetNewTask(rage_new CTaskVehicleGoToPointWithAvoidanceAutomobile(params, false));
}

CTask::FSM_Return	CTaskVehicleParkNew::BackIntoSpace_OnUpdate(CVehicle* pVehicle)
{
	if ((!GetSubTask() || GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE))
		&& GetTimeInState() > 0.0f)
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	CTaskVehicleGoToPointWithAvoidanceAutomobile * pGoToTask = NULL;
	if(GetSubTask() && GetSubTask()->GetTaskType()==CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE)
	{
		pGoToTask = static_cast<CTaskVehicleGoToPointWithAvoidanceAutomobile*>(GetSubTask());
	}

	if ((pGoToTask && pGoToTask->GetCruiseSpeed() <= 0.0f && HelperIsVehicleStopped(pVehicle))
		|| pVehicle->GetIntelligence()->MillisecondsNotMoving > ms_iParkingStuckTime )
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	if (pGoToTask)
	{
		pGoToTask->SetCruiseSpeed(GetCruiseSpeedForDistanceToTarget(pVehicle));

		Vector3 vSteerPos;
		HelperGetPositionToSteerTo(pVehicle, vSteerPos);
		pGoToTask->SetTargetPosition(&vSteerPos);
	}
	return FSM_Continue;
}

//BackAwayFromSpace
void		CTaskVehicleParkNew::BackAwayFromSpace_OnEnter(CVehicle* pVehicle)
{
	Vector3 vBackUpPosition(Vector3::ZeroType);
	HelperGetBackAwayFromSpacePosition(pVehicle, vBackUpPosition);

	sVehicleMissionParams params;
	params.m_fCruiseSpeed = GetCruiseSpeedForDistanceToTarget(pVehicle);
	params.m_iDrivingFlags = GetDrivingFlags()|DF_DriveInReverse;
	params.m_fTargetArriveDist = 1.0f;
	params.SetTargetPosition(vBackUpPosition);
	SetNewTask(rage_new  CTaskVehicleGoToPointWithAvoidanceAutomobile(params, false));
}

CTask::FSM_Return	CTaskVehicleParkNew::BackAwayFromSpace_OnUpdate(CVehicle* pVehicle)
{
	if ((!GetSubTask() || GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE))
		&& GetTimeInState() > 0.0f)
	{
		SetState(State_Stop);
		return FSM_Continue;
	}

	if(GetSubTask() && GetSubTask()->GetTaskType()==CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE)
	{
		CTaskVehicleGoToPointWithAvoidanceAutomobile * pGoToTask = (CTaskVehicleGoToPointWithAvoidanceAutomobile*)GetSubTask();
		pGoToTask->SetCruiseSpeed(GetCruiseSpeedForDistanceToTarget(pVehicle));

		Vector3 vBackUpPosition(Vector3::ZeroType);
		HelperGetBackAwayFromSpacePosition(pVehicle, vBackUpPosition);
		pGoToTask->SetTargetPosition(&vBackUpPosition);
	}

	if (m_ParkType == Park_Perpendicular_NoseIn)
	{
		//early out if we're roughly halfway out of the space and facing the right direction
		//bool bFacingForward = false;
		const float fDistanceInFrontOfSpace = GetDistanceInFrontOfSpace(pVehicle);
		if ((IsCurrentHeadingWithinTolerance(pVehicle) 
			&& fDistanceInFrontOfSpace < -ms_fParkingSpaceHalfLength
			&& IsCurrentPositionWithinXTolerance(pVehicle))
			|| fDistanceInFrontOfSpace < -(2.0f * ms_fParkingSpaceHalfLength + 3.0f))
		{
			SetState(State_Stop);
			return FSM_Continue;
		}
	}

// #if __DEV
// 
// 	//debugging
// 	Vector3 vDest(Vector3::ZeroType);
// 	CTaskVehicleGoToPointWithAvoidanceAutomobile* pSubtask1 = static_cast<CTaskVehicleGoToPointWithAvoidanceAutomobile*>
// 		(FindSubTaskOfType(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE));
// 	if (pSubtask1)
// 	{
// 		vDest = *pSubtask1->GetTargetPosition();
// 		grcDebugDraw::Sphere(vDest, 0.5f, Color_red4, false);
// 	}
// 
// #endif //__DEV

	return FSM_Continue;
}

//PullForwardParallel
void		CTaskVehicleParkNew::PullForward_OnEnter(CVehicle* pVehicle)
{
	static dev_float s_fParallelZOffset = 9.0f;
	static dev_float s_fParallelXOffset = 3.5f;
	//Approach an intermediate position before pulling into the space
	Vector3 vApproachPosition(Vector3::ZeroType);
	const bool bShouldUseBackInOffsets = m_ParkType == Park_Perpendicular_BackIn && GetPreviousState() == State_Navigate;
	const float fApproachTolerance = pVehicle->InheritsFromPlane() ? 5.0f : 1.0f;

	if (m_ParkType == Park_Perpendicular_NoseIn || bShouldUseBackInOffsets)
	{
		//ok, new algorithm. use however far away we are by the time we get in here as our Z offset
		//and the X offset should be the minimum between the X offset from the space and (trig incoming)
		//(Z offset) / tan(max steering angle at current speed)
		Vector3 vRearPos;
		HelperGetParkingSpaceRearPosition(vRearPos);
		const Vector3 vBonnetPos = VEC3V_TO_VECTOR3(CVehicleFollowRouteHelper::GetVehicleBonnetPosition(pVehicle, false));
 		const float fActualZOffset = (vBonnetPos - vRearPos).Dot(-m_SpaceDir);
		const float fTurnRadius = pVehicle->GetIntelligence()->GetTurnRadiusAtCurrentSpeed();

		//get a vector normal to the parking space, and point it toward the car
		Vector3 vSpaceNormal(m_SpaceDir.y, -m_SpaceDir.x, 0.0f);
		Vector3 vDeltaToCar = vBonnetPos - vRearPos;
		if (vSpaceNormal.Dot(vDeltaToCar) < 0.0f)
		{
			vSpaceNormal.Negate();
		}
		const float fActualXOffset = (vBonnetPos - vRearPos).Dot(vSpaceNormal);

		//add the Z direction
		vApproachPosition = vRearPos + (-m_SpaceDir * fActualZOffset);

		//add the X direction if needed. if the z offset is large enough we won't need any
		//X offset
		//const float fMaxXOffset = rage::Max(fTurnRadius - fApproachTolerance, 0.0f);
		const float fMinXOffsetRequired = rage::Max(fTurnRadius - fActualZOffset, ms_fParkingSpaceHalfWidth);

		vApproachPosition += vSpaceNormal * rage::Min(fActualXOffset, fMinXOffsetRequired);
	}
	else if (m_ParkType == Park_Perpendicular_BackIn)
	{
		HelperGetBackAwayFromSpacePosition(pVehicle, vApproachPosition);
	}
	else if (m_ParkType == Park_Parallel)
	{
		vApproachPosition = m_Params.GetTargetPosition() + (m_SpaceDir * s_fParallelZOffset);
		Vector3 vSpaceNormal(m_SpaceDir.y, -m_SpaceDir.x, 0.0f);
		Vector3 vDeltaToCar = VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition()) - vApproachPosition;
		if (vSpaceNormal.Dot(vDeltaToCar) < 0.0f)
		{
			vSpaceNormal.Negate();
		}
		vApproachPosition += (vSpaceNormal * s_fParallelXOffset);
	}
	else
	{
		taskAssertf(0, "Parktype not supported yet :(");
	}

	//drive to the point
	sVehicleMissionParams params;
	params.m_fCruiseSpeed = GetCruiseSpeedForDistanceToTarget(pVehicle);
	params.m_iDrivingFlags = GetDrivingFlags();
	params.m_fTargetArriveDist = fApproachTolerance;
	params.SetTargetPosition(vApproachPosition);
	SetNewTask(rage_new  CTaskVehicleGoToPointWithAvoidanceAutomobile(params));
}

CTask::FSM_Return	CTaskVehicleParkNew::PullForward_OnUpdate(CVehicle* pVehicle)
{
	if (!GetSubTask() || GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE))
	{
		if (m_ParkType == Park_Perpendicular_NoseIn)
		{
			SetState(State_ForwardIntoSpace);
			return FSM_Continue;
		}
		else if (m_ParkType == Park_Parallel)
		{
			SetState(State_BackIntoSpace);
			return FSM_Continue;
		}
		else if (m_ParkType == Park_Perpendicular_BackIn)
		{
			SetState(State_BackIntoSpace);
			return FSM_Continue;
		}
	}

	if (m_ParkType == Park_Perpendicular_BackIn)
	{
		//early out if we're roughly halfway out of the space and facing the right direction
		//bool bFacingForward = false;
		if ((IsCurrentHeadingWithinTolerance(pVehicle) 
			&& GetDistanceInFrontOfSpace(pVehicle) < -ms_fParkingSpaceHalfLength
			&& IsCurrentPositionWithinXTolerance(pVehicle))
			|| pVehicle->GetIntelligence()->MillisecondsNotMoving > ms_iParkingStuckTime
			/*|| GetDistanceInFrontOfSpace(pVehicle) < -(2.0f * ms_fParkingSpaceHalfLength + 3.0f)*/)
		{
			SetState(State_BackIntoSpace);
			return FSM_Continue;
		}
	}

	if(GetSubTask() && GetSubTask()->GetTaskType()==CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE)
	{
		CTaskVehicleGoToPointWithAvoidanceAutomobile * pGoToTask = (CTaskVehicleGoToPointWithAvoidanceAutomobile*)GetSubTask();
		pGoToTask->SetCruiseSpeed(GetCruiseSpeedForDistanceToTarget(pVehicle));
	}

// #if __DEV
// 
// 	
// 	//debugging
// 	Vector3 vDest(Vector3::ZeroType);
// 	CTaskVehicleGoToPointWithAvoidanceAutomobile* pSubtask1 = static_cast<CTaskVehicleGoToPointWithAvoidanceAutomobile*>
// 		(FindSubTaskOfType(CTaskTypes::TASK_VEHICLE_GOTO_POINT_WITH_AVOIDANCE_AUTOMOBILE));
// 	if (pSubtask1)
// 	{
// 		vDest = *pSubtask1->GetTargetPosition();
// 	}
// 
// 	Vector3 vDest2(Vector3::ZeroType);
// 	CTaskVehicleGoToPointAutomobile* pSubtask2 = static_cast<CTaskVehicleGoToPointAutomobile*>
// 		(FindSubTaskOfType(CTaskTypes::TASK_VEHICLE_GOTO_POINT_AUTOMOBILE));
// 	if (pSubtask2)
// 	{
// 		vDest2 = *pSubtask2->GetTargetPosition();
// 	}
// 
// 	grcDebugDraw::Sphere(vDest, 0.5f, Color_red4, false);
// 	grcDebugDraw::Sphere(m_vTargetPos, 0.5f, Color_green3, false);
// 	grcDebugDraw::Sphere(vDest2, 0.5f, Color_yellow1, false);
// 
// #endif //__DEV

	return FSM_Continue;
}

//Stop
void		CTaskVehicleParkNew::Stop_OnEnter(CVehicle* /*pVehicle*/)
{
	SetNewTask(rage_new CTaskVehicleStop(CTaskVehicleStop::SF_DontResetSteerAngle));
}

CTask::FSM_Return	CTaskVehicleParkNew::Stop_OnUpdate(CVehicle* pVehicle)
{
	static dev_float s_fMinimumTimeBeforeTryingAgainSeconds = 0.2f;
	const bool bTimeElapsed = GetTimeInState() >= s_fMinimumTimeBeforeTryingAgainSeconds;

	if (bTimeElapsed && GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_STOP))
	{
		if (IsReadyToQuit(pVehicle))
		{
			//for now just hang out in this state so dudes don't go back to
			//cruising after they park
			//......that sounded really dirty
			//return FSM_Continue;

			//not sure if PullOver implies idling on the curb or not--
			//existing code does that so we'll do the same for now
			if (m_ParkType != Park_PullOver 
				&& m_ParkType != Park_PullOverImmediate
				&& m_ParkType != Park_LeaveParallelSpace
				&& m_ParkType != Park_BackOutPerpendicularSpace)
			{
				if (ShouldTurnOffEngineAndLights(pVehicle) && !m_bKeepLightsOnAfterParking)
				{
					pVehicle->m_nVehicleFlags.bLightsOn = false;
					pVehicle->SwitchEngineOff();
				}
				CScenarioManager::GiveScenarioToDrivingCar(pVehicle);
			}
			
			if (m_ParkType == Park_Parallel)
			{
				pVehicle->m_nVehicleFlags.bIsParkedParallelly = true;
			}
			else if (m_ParkType == Park_Perpendicular_NoseIn)
			{
				pVehicle->GetIntelligence()->bCarHasToReverseFirst = true;
				pVehicle->m_nVehicleFlags.bIsParkedPerpendicularly = true;
			}
			else if (m_ParkType == Park_Perpendicular_BackIn)
			{
				pVehicle->m_nVehicleFlags.bIsParkedPerpendicularly = true;
			}

			m_bMissionAchieved = true;
			return FSM_Quit;
		}	
		else if (GetPreviousState() == State_ForwardIntoSpace && m_ParkType == Park_Parallel)
		{
			SetState(State_BackIntoSpace);
		}
		else if (GetPreviousState() == State_ForwardIntoSpace)
		{
			SetState(State_BackAwayFromSpace);
		}
		else if (GetPreviousState() == State_BackAwayFromSpace)
		{
			SetState(State_ForwardIntoSpace);
		}
		else if (GetPreviousState() == State_BackIntoSpace  && m_ParkType == Park_Perpendicular_BackIn)
		{
			SetState(State_PullForward);
		}
		else if (GetPreviousState() == State_BackIntoSpace && m_ParkType == Park_Parallel)
		{
			SetState(State_ForwardIntoSpace);
		}
		else if (GetPreviousState() == State_PassengerExit && m_ParkType == Park_PassengerExit)
		{
			m_bMissionAchieved = true;
			return FSM_Quit;
		}
		else if(m_bDoTestsForBlockedSpace && GetPreviousState() == State_Navigate)
		{
			// Check if enough time has passed since the last blocked space test.
			// If so, go back to Navigate.
			if(GetTimeInState() >= sm_Tunables.m_ParkingSpaceBlockedWaitTimePerAttempt)
			{
				// Note: if the test takes multiple frames, it would probably be best
				// to set up the test here already and wait in this state until the results
				// are in, or add a new state for that.
				SetState(State_Navigate);
			}
		}
		else
		{
			taskAssertf(0, "Somehow we jumped to State_Stop from an unsupported state???");
		}
	}

	return FSM_Continue;
}


void CTaskVehicleParkNew::Failed_OnEnter()
{
}


CTask::FSM_Return CTaskVehicleParkNew::Failed_OnUpdate()
{
	// Note: we could potentially just FSM_Quit here, but I had some difficulty in getting
	// the ped tasks detect the failure, the vehicle task got removed too early for that.

	// As safety, we still quit after some time.
	if(GetTimeInState() >= 2.0f)
	{
		return FSM_Quit;
	}

	return FSM_Continue;
}


bool CTaskVehicleParkNew::IsCurrentHeadingWithinTolerance(CVehicle* pVehicle) const
{
	const float fVehicleHeading = pVehicle->GetTransform().GetHeading();

	const bool bIsPerpendicularBackIn = m_ParkType == Park_Perpendicular_BackIn;
	const Vector3 vSpaceDir = bIsPerpendicularBackIn ? -m_SpaceDir : m_SpaceDir;
	const float fSpaceHeading = fwAngle::LimitRadianAngleSafe(fwAngle::GetATanOfXY(vSpaceDir.x, vSpaceDir.y) - (DtoR * 90.0f));

	//grcDebugDraw::Arrow(pVehicle->GetVehiclePosition(), pVehicle->GetVehiclePosition() + (pVehicle->GetVehicleForwardDirection() * ScalarV(V_FIVE)), 1.0f, Color_yellow1);
	//grcDebugDraw::Arrow(VECTOR3_TO_VEC3V(m_vTargetPos), VECTOR3_TO_VEC3V(m_vTargetPos+(m_SpaceDir*3.0f)), 1.0f, Color_pink1);

	float fDiff = rage::Abs(fSpaceHeading-fVehicleHeading);
	fDiff = fwAngle::LimitRadianAngle(fDiff);

	const float fToleranceRadiansToUse = pVehicle->InheritsFromPlane() ? PI : m_HeadingToleranceRadians;

	return rage::Abs(fDiff) <= fToleranceRadiansToUse;
}

bool CTaskVehicleParkNew::IsCurrentPositionWithinTolerance(CVehicle* pVehicle) const
{
	return IsCurrentPositionWithinXTolerance(pVehicle) && IsCurrentPositionWithinYTolerance(pVehicle);
	//return DistSquared(pVehicle->GetVehiclePosition(), VECTOR3_TO_VEC3V(m_vTargetPos)).Getf() <= ms_fTargetAchieveDistance*ms_fTargetAchieveDistance;
}

bool CTaskVehicleParkNew::IsReadyToQuit(CVehicle* pVehicle) const
{
	taskAssert(GetState() == State_Stop);
	if (m_ParkType == Park_PullOver
		|| m_ParkType == Park_PullOverImmediate)
	{
		return true;
	}

	if ((m_ParkType == Park_LeaveParallelSpace || m_ParkType == Park_BackOutPerpendicularSpace)
		&& GetPreviousState() == State_BackAwayFromSpace)
	{
		return true;
	}

	//early out if we weren't trying to actually enter the space on this move
	if (GetPreviousState() == State_BackAwayFromSpace)
	{
		return false;
	}
	
	if (!IsCurrentHeadingWithinTolerance(pVehicle))
	{
		return false;
	}

	if (!IsCurrentPositionWithinTolerance(pVehicle))
	{
		return false;
	}

	return true;
}

void CTaskVehicleParkNew::HelperGetParkingSpaceFrontPosition(Vector3& vPositionOut) const
{
	vPositionOut = m_Params.GetTargetPosition() + (m_SpaceDir * ms_fParkingSpaceHalfLength);
}

void CTaskVehicleParkNew::HelperGetParkingSpaceRearPosition(Vector3& vPositionOut) const
{
	vPositionOut = m_Params.GetTargetPosition() + (m_SpaceDir * -ms_fParkingSpaceHalfLength);
}

bool CTaskVehicleParkNew::HelperUseReverseSpace() const
{
	return m_ParkType == Park_Parallel && GetState() == State_BackIntoSpace;
}

bool CTaskVehicleParkNew::HelperUseRearBonnetPos() const
{
	return GetState() == State_BackIntoSpace;
}

float CTaskVehicleParkNew::GetDistanceInFrontOfSpace(CVehicle* pVehicle) const
{
	Vector3 vPosition;

	const bool bReverseSpace = HelperUseReverseSpace();

	if (bReverseSpace)
	{
		HelperGetParkingSpaceRearPosition(vPosition);
	}
	else
	{
		HelperGetParkingSpaceFrontPosition(vPosition);
	}
	
	const Vector3 vVehicleNavPos = VEC3V_TO_VECTOR3(CVehicleFollowRouteHelper::GetVehicleBonnetPosition(pVehicle, HelperUseRearBonnetPos()));
	Vector3 vDeltaFromSpace = vVehicleNavPos - vPosition;

	float dotProduct = vDeltaFromSpace.Dot((bReverseSpace ? -1.0f : 1.0f ) * m_SpaceDir);

	if (bReverseSpace)
	{
		static dev_float s_fExtraParallelReverseDistance = 3.0f;
		dotProduct -= s_fExtraParallelReverseDistance;
	}
	return dotProduct;
}

bool CTaskVehicleParkNew::IsCurrentPositionWithinXTolerance(CVehicle* pVehicle) const
{
	const bool bUseRearPosForVehicle = HelperUseRearBonnetPos();
	const Vector3 vVehicleNavPos = VEC3V_TO_VECTOR3(CVehicleFollowRouteHelper::GetVehicleBonnetPosition(pVehicle, bUseRearPosForVehicle));

	Vector3 vTargetPos;
	if (HelperUseReverseSpace())
	{
		HelperGetParkingSpaceRearPosition(vTargetPos);
	}
	else
	{
		HelperGetParkingSpaceFrontPosition(vTargetPos);
	}

	const Vector3 vDeltaFromSpace = vVehicleNavPos - vTargetPos;

	//rotate the parking space heading 90 degrees
	Vector3 vSpaceNormal(m_SpaceDir.y, -m_SpaceDir.x, 0.0f);

	const float debugDotProduct = vDeltaFromSpace.Dot(vSpaceNormal);

	//grcDebugDraw::Sphere(m_vTargetPos, 0.3f, Color_green, false);
	//grcDebugDraw::Sphere(m_vTargetPos + (vSpaceNormal * ms_fPositionXTolerance), 0.3f, Color_green2, false);
	//grcDebugDraw::Sphere(m_vTargetPos + (vSpaceNormal * -ms_fPositionXTolerance), 0.3f, Color_green2, false);

	float fXTolerance = ms_fPositionXTolerance;
	if (pVehicle->InheritsFromBike() || pVehicle->InheritsFromQuadBike() || pVehicle->InheritsFromAmphibiousQuadBike())
	{
		fXTolerance = ms_fPositionXToleranceBike;
	}
	else if (pVehicle->InheritsFromPlane())
	{
		fXTolerance = ms_fPositionXTolerancePlane;
	}

	return fabsf(debugDotProduct) <= fXTolerance;
}

bool CTaskVehicleParkNew::IsCurrentPositionWithinYTolerance(CVehicle* pVehicle) const
{
	const bool bUseRearPosForVehicle = HelperUseRearBonnetPos();
	const Vector3 vVehicleNavPos = VEC3V_TO_VECTOR3(CVehicleFollowRouteHelper::GetVehicleBonnetPosition(pVehicle, bUseRearPosForVehicle));

	Vector3 vTargetPos;
	if (HelperUseReverseSpace())
	{
		HelperGetParkingSpaceRearPosition(vTargetPos);
	}
	else
	{
		HelperGetParkingSpaceFrontPosition(vTargetPos);
	}

	const Vector3 vDeltaFromSpace = vVehicleNavPos - vTargetPos;

	const float debugDotProduct = vDeltaFromSpace.Dot(m_SpaceDir);

	float fYTolerance = ms_fPositionYTolerance;
	if (pVehicle->InheritsFromBike() || pVehicle->InheritsFromQuadBike() || pVehicle->InheritsFromAmphibiousQuadBike())
	{
		fYTolerance = ms_fPositionYToleranceBike;
	}
	else if (pVehicle->InheritsFromPlane())
	{
		fYTolerance = ms_fPositionYTolerancePlane;
	}

	return fabsf(debugDotProduct) <= fYTolerance;
}

void CTaskVehicleParkNew::HelperGetPositionToSteerTo(CVehicle* pVehicle, Vector3& vPositionOut) const
{
	//project N meters out in front of the car, and find the closest position
	//on the line segment defining the parking space
	static dev_float s_fDistInFrontOfCar = 7.0f;
	static dev_float s_fDistInFrontOfCarParallel = 3.0f;
	Vector3 vFrontPos, vRearPos, vParkingSpaceSegment, vSegmentStartPos;

	HelperGetParkingSpaceFrontPosition(vFrontPos);
	HelperGetParkingSpaceRearPosition(vRearPos);

	Vector3 vSpaceNormal(m_SpaceDir.y, -m_SpaceDir.x, 0.0f);
	
	//figure out which segment to use
	const float fVehicleHeading = pVehicle->GetTransform().GetHeading();
	const float fSpaceHeading = fwAngle::LimitRadianAngleSafe(fwAngle::GetATanOfXY(m_SpaceDir.x, m_SpaceDir.y) - (DtoR * 90.0f));

	const bool bBackupParallel = m_ParkType == Park_Parallel && GetState() == State_BackIntoSpace;
	const bool bBackupPerpendicular = m_ParkType == Park_Perpendicular_BackIn;
	//const bool bForwardPerpendicular = m_ParkType == Park_Perpendicular_NoseIn;

if (/*bForwardPerpendicular ||*/ IsCurrentPositionWithinXTolerance(pVehicle) && IsCurrentHeadingWithinTolerance(pVehicle))
	{
		//more or less there, use the straight line
		if (bBackupParallel)
		{
			vParkingSpaceSegment = vRearPos - vFrontPos;
			vSegmentStartPos = vFrontPos;
		}
		else
		{
			vParkingSpaceSegment = vFrontPos - vRearPos;
			vSegmentStartPos = vRearPos;
		}

	}
	//else if (bBackupPerpendicular && fVehicleHeading < fSpaceHeading || !bBackupPerpendicular && fVehicleHeading > fSpaceHeading)
	else if ((((bBackupPerpendicular || bBackupParallel) && (VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition()) - m_Params.GetTargetPosition()).Dot(vSpaceNormal) > 0.0f))
		|| ((!bBackupPerpendicular && !bBackupParallel) && fVehicleHeading > fSpaceHeading))
	{
		//we're to the right of the space
		vSegmentStartPos = vRearPos + -vSpaceNormal * ms_fParkingSpaceHalfWidth;

		if (bBackupParallel)
		{
			vParkingSpaceSegment = (vFrontPos + -vSpaceNormal * ms_fParkingSpaceHalfWidth) - vRearPos;
			vParkingSpaceSegment.Negate();
		}
		else 
		{
			vParkingSpaceSegment = vFrontPos - vSegmentStartPos;
		}
	}
	else
	{
		//to the left of the space
		vSegmentStartPos = vRearPos + vSpaceNormal * ms_fParkingSpaceHalfWidth;

		if (bBackupParallel)
		{
			vParkingSpaceSegment = (vFrontPos + vSpaceNormal * ms_fParkingSpaceHalfWidth) - vRearPos;
			vParkingSpaceSegment.Negate();
		}
		else
		{
			vParkingSpaceSegment = vFrontPos - vSegmentStartPos;
		}
	}

	//we need to continue steering toward the line even past the extents of the parking space
	vParkingSpaceSegment *= 2.0f;

	Vector3 vVehicleForward = VEC3V_TO_VECTOR3(pVehicle->GetVehicleForwardDirection());
	if (m_ParkType == Park_Perpendicular_BackIn || bBackupParallel)
	{
		vVehicleForward.Negate();
	}

	Vector3 vSearchPosition = VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition()) + vVehicleForward * (bBackupParallel ? s_fDistInFrontOfCarParallel : s_fDistInFrontOfCar);

	vPositionOut = VEC3V_TO_VECTOR3( geomPoints::FindClosestPointSegToPoint(VECTOR3_TO_VEC3V(vSegmentStartPos), VECTOR3_TO_VEC3V(vParkingSpaceSegment), VECTOR3_TO_VEC3V(vSearchPosition)) );

//#if __DEV
	//grcDebugDraw::Sphere(vPositionOut, 0.5f, Color_red);
	//grcDebugDraw::Sphere(vSearchPosition, 0.5f, Color_blue3);
	//grcDebugDraw::Arrow(VECTOR3_TO_VEC3V(vSegmentStartPos + Vector3(0.0f, 0.0f, 0.1f))
	//	, VECTOR3_TO_VEC3V(vSegmentStartPos + vParkingSpaceSegment + Vector3(0.0f, 0.0f, 0.1f)), 0.5f, Color_red);
//#endif //__DEV
}

float CTaskVehicleParkNew::GetCruiseSpeedForDistanceToTarget(CVehicle* pVehicle) const
{
	//if we're already there, and trying to enter  the space, go ahead and stop
	if ( !pVehicle->InheritsFromBoat() &&
		( GetState() == State_ForwardIntoSpace || GetState() == State_BackIntoSpace ) )
	{
		//if we're done, just stop
// 		if (IsCurrentPositionWithinTolerance(pVehicle)
// 			&& IsCurrentHeadingWithinTolerance(pVehicle))
// 		{
// 			return 0.0f;
// 		}

		float fDistInFrontOfSpace = GetDistanceInFrontOfSpace(pVehicle);
		static dev_float s_fSpeedMultiplier = 1.1f;
		const float fExtraPaddingForNonSuccesfulParallelParks = m_ParkType == Park_Parallel && !IsCurrentPositionWithinXTolerance(pVehicle)
			? pVehicle->GetBoundingBoxMax().y
			: 0.0f;

		fDistInFrontOfSpace -= fExtraPaddingForNonSuccesfulParallelParks;

		if (fDistInFrontOfSpace > (2.0f * -ms_fParkingSpaceHalfLength))
		{
			static dev_float s_fSpeedFloor = 1.0f;
			const float retVal = -(s_fSpeedMultiplier * fDistInFrontOfSpace) + (s_fSpeedFloor * 0.5f);
			return retVal >= s_fSpeedFloor ? retVal : 0.0f;
		}
	}

	//if the user passed in a slower speed than what we computed, don't speed up
	return rage::Min(pVehicle->InheritsFromBoat() ? ms_fCruiseSpeedToParkBoat : ms_fCruiseSpeedToPark, GetCruiseSpeed());
}

bool CTaskVehicleParkNew::HelperIsVehicleStopped(CVehicle* pVehicle) const
{
	return pVehicle->GetVelocity().XYMag2() <= 0.05f*0.05f;
}

void CTaskVehicleParkNew::HelperGetBackAwayFromSpacePosition(CVehicle* pVehicle, Vector3& vPositionOut) const
{
	//depending on what type of action we're doing, we'll back up different amounts
	//just behind the space for parallel parking
	//a few meters back for perpendicular
	//way back (maybe all the way to the road network?) for leaving a perpendicular space
	
	if (m_ParkType == Park_Parallel || m_ParkType == Park_LeaveParallelSpace)
	{
		vPositionOut = m_Params.GetTargetPosition() + (-m_SpaceDir * 1.0f);
	}
	else if (m_ParkType == Park_Perpendicular_NoseIn)
	{
		//TODO: base this on the amount our heading is off from the target
		//back up less if we're only a few degrees off
		Vector3 vSpaceNormal(m_SpaceDir.y, -m_SpaceDir.x, 0.0f);
		const float fBackupLength = rage::Max(2.0f, pVehicle->GetBoundingBoxMax().y - pVehicle->GetBoundingBoxMin().y);
		Vector3 vCenter = VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition()) + (-m_SpaceDir * fBackupLength);
		const float fXTolerance = (!pVehicle->InheritsFromBike() && !pVehicle->InheritsFromQuadBike() && !pVehicle->InheritsFromAmphibiousQuadBike()) ? ms_fPositionXTolerance : ms_fPositionXToleranceBike;
		Vector3 vLeftPos = vCenter + -vSpaceNormal * fXTolerance * 2.0f;
		Vector3 vToRightPos = vSpaceNormal * fXTolerance * 4.0f;
		Vector3 vSearchPos = VEC3V_TO_VECTOR3(pVehicle->GetVehiclePosition()) + (VEC3V_TO_VECTOR3(-pVehicle->GetVehicleForwardDirection()) * 5.0f);

		vPositionOut = VEC3V_TO_VECTOR3( geomPoints::FindClosestPointSegToPoint(VECTOR3_TO_VEC3V(vLeftPos), VECTOR3_TO_VEC3V(vToRightPos), VECTOR3_TO_VEC3V(vSearchPos)) );
	}
	else if (m_ParkType == Park_BackOutPerpendicularSpace || m_ParkType == Park_Perpendicular_BackIn)
	{
		//TODO: base this on the size of the car and/or distance to road network
		vPositionOut = m_Params.GetTargetPosition() + (-m_SpaceDir * 8.0f);
	}

}

//test the area immediately behind the parking space for vehicles and other objects

bool CTaskVehicleParkNew::HelperIsAnythingBlockingEntryToSpace(CVehicle* pVehicle) const
{
	const Vector3 vSearchPos = m_Params.GetTargetPosition() + (-m_SpaceDir * 2.0f * ms_fParkingSpaceHalfLength);
	const float fSearchRadiusSqr = ms_fParkingSpaceHalfLength * ms_fParkingSpaceHalfLength;

	CEntityScannerIterator entityList = pVehicle->GetIntelligence()->GetVehicleScanner().GetIterator();
	for( CEntity* pEntity = entityList.GetFirst(); pEntity; pEntity = entityList.GetNext() )
	{
		Assert(pEntity->GetIsTypeVehicle());
		CVehicle *pVehObstacle =(CVehicle *)pEntity;

		// Ignore trailers we're towing
		if(pVehObstacle->InheritsFromTrailer())
		{
			// check whether the trailer is attached to the parent vehicle
			CTrailer* pTrailer = static_cast<CTrailer*>(pVehObstacle);
			if(pTrailer->GetAttachParent() == pVehicle)
			{
				continue;
			}
		}

		if ((VEC3V_TO_VECTOR3(pEntity->GetTransform().GetPosition()) - vSearchPos).XYMag2() < fSearchRadiusSqr)
		{
			return true;
		}
	}

	//const CEntityScannerIterator objectList = pVehicle->GetIntelligence()->GetObjectScanner().GetIterator();
	for( const CEntity* pEntity = entityList.GetFirst(); pEntity; pEntity = entityList.GetNext() )
	{
		if(!pEntity->IsCollisionEnabled())
		{
			continue;
		}

		//don't care about small stuff (this is the same threshold we use when determining whether to avoid a car)
		if (pEntity->GetBoundingBoxMax().z < 0.9f)
		{
			continue;
		}

		if ((VEC3V_TO_VECTOR3(pEntity->GetTransform().GetPosition()) - vSearchPos).XYMag2() < fSearchRadiusSqr)
		{
			return true;
		}
	}

	return false;
}

//////////////////////////////////////////////////////////////////////////////////////////

CTaskVehicleReactToCopSiren::CTaskVehicleReactToCopSiren(ReactToCopSirenType reactionType, u32 nWaitDuration)
: m_ReactionType(reactionType)
, m_nWaitDuration(nWaitDuration)
{
	SetInternalTaskType(CTaskTypes::TASK_VEHICLE_REACT_TO_COP_SIREN);
}

void CTaskVehicleReactToCopSiren::CleanUp()
{
	if (aiVerify(GetVehicle()))
	{
		GetVehicle()->m_nVehicleFlags.bDisablePretendOccupants = m_bPreviousDisablePretendOccupantsCached;
	}
}

CTask::FSM_Return CTaskVehicleReactToCopSiren::UpdateFSM( const s32 iState, const FSM_Event iEvent )
{
	CVehicle *pVehicle = GetVehicle(); //Get the vehicle ptr.

	FSM_Begin
		// Start
		FSM_State(State_Start)
			FSM_OnUpdate
				return Start_OnUpdate(pVehicle);

		// Pullover
		FSM_State(State_Pullover)
			FSM_OnEnter
				Pullover_OnEnter(pVehicle);
			FSM_OnUpdate
				return Pullover_OnUpdate(pVehicle);

		// Wait
		FSM_State(State_Wait)
			FSM_OnEnter
				Wait_OnEnter(pVehicle);
			FSM_OnUpdate
				return Wait_OnUpdate(pVehicle);
	FSM_End
}

CTask::FSM_Return CTaskVehicleReactToCopSiren::Start_OnUpdate(CVehicle* pVehicle)
{
	m_bPreviousDisablePretendOccupantsCached = pVehicle->m_nVehicleFlags.bDisablePretendOccupants;

	if (m_ReactionType == React_JustWait)
	{
		SetState(State_Wait);
	}
	else
	{
		SetState(State_Pullover);
	}
	return FSM_Continue;
}

void CTaskVehicleReactToCopSiren::Pullover_OnEnter(CVehicle* /*pVehicle*/)
{
	aiAssert(m_ReactionType == React_PullOverLeft || m_ReactionType == React_PullOverRight);

	CTaskVehiclePullOver::ForcePulloverDirection pulloverDir = m_ReactionType == React_PullOverLeft 
		? CTaskVehiclePullOver::Force_Left 
		: CTaskVehiclePullOver::Force_Right;
	SetNewTask(rage_new CTaskVehiclePullOver(true, pulloverDir, true));
}

CTask::FSM_Return CTaskVehicleReactToCopSiren::Pullover_OnUpdate(CVehicle* /*pVehicle*/)
{
	if (GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_PULL_OVER))
	{	
		SetState(State_Wait);
	}

	return FSM_Continue;
}

//////////////////////////////////////////////////////////////////////
//State_Wait
//////////////////////////////////////////////////////////////////////
void		CTaskVehicleReactToCopSiren::Wait_OnEnter(CVehicle* UNUSED_PARAM(pVehicle))
{
	SetNewTask(rage_new CTaskVehicleWait(NetworkInterface::GetSyncedTimeInMilliseconds() + m_nWaitDuration));
}

/////////////////////////////////////////////////////////////////////////////////

CTask::FSM_Return	CTaskVehicleReactToCopSiren::Wait_OnUpdate(CVehicle* UNUSED_PARAM(pVehicle))
{
	if (GetIsSubtaskFinished(CTaskTypes::TASK_VEHICLE_WAIT))
	{
		return FSM_Quit;
	}
	return FSM_Continue;
}

#if !__FINAL
const char * CTaskVehicleReactToCopSiren::GetStaticStateName( s32 iState  )
{
	Assert(iState>=State_Start && iState<=State_Wait);
	static const char* aStateNames[] = 
	{
		"State_Start",
		"State_Pullover",
		"State_Wait",
	};

	return aStateNames[iState];
}
#endif