class FindTeleportPositionKurt extends AIbehaviortaskScript
{
	editable inlined var m_target : AIArgumentMapping;
	editable inlined var m_extents : AIArgumentMapping;
	editable inlined var m_extentsOffset : AIArgumentMapping;
	editable inlined var m_workspotRotation : AIArgumentMapping;
	editable inlined var m_workspotOffset : AIArgumentMapping;
	editable inlined var m_outPositionArgument : AIArgumentMapping;
	editable inlined var m_outRotationArgument : AIArgumentMapping;
	editable inlined var m_outMaybeStairs : AIArgumentMapping;

	protected export override function Activate( context : ScriptExecutionContext )
	{
		var sandevistanPsmEvent : PSMPostponedParameterBool;
		var kerenzikovPsmEvent : PSMPostponedParameterBool;
		var player : PlayerPuppet;
		player = ( ( PlayerPuppet )( GameInstance.GetPlayerSystem( ScriptExecutionContext.GetOwner( context ).GetGame() ).GetLocalPlayerControlledGameObject() ) );
		if( player )
		{
			sandevistanPsmEvent = new PSMPostponedParameterBool;
			sandevistanPsmEvent.id = 'requestSandevistanDeactivation';
			sandevistanPsmEvent.value = true;
			player.QueueEvent( sandevistanPsmEvent );
			kerenzikovPsmEvent = new PSMPostponedParameterBool;
			kerenzikovPsmEvent.id = 'requestKerenzikovDeactivation';
			kerenzikovPsmEvent.value = true;
			player.QueueEvent( kerenzikovPsmEvent );
		}
	}

	protected export override function Update( context : ScriptExecutionContext ) : AIbehaviorUpdateOutcome
	{
		var followTarget : weak< GameObject >;
		var teleportPosition : Vector4;
		var teleportRotation : EulerAngles;
		var navigationSystem : AINavigationSystem;
		var queryExtents : Vector4;
		var extentsOffset : Vector4;
		var workspotOffset : Vector4;
		var targetDirection : Vector4;
		var targetOrientation : Quaternion;
		var workspotRotation : Float;
		var maybeStairs : Bool;
		followTarget = ( ( weak< weak< GameObject > > )( ScriptExecutionContext.GetMappingValue( context, m_target ) ) );
		queryExtents = ( ( Vector4 )( ScriptExecutionContext.GetMappingValue( context, m_extents ) ) );
		extentsOffset = ( ( Vector4 )( ScriptExecutionContext.GetMappingValue( context, m_extentsOffset ) ) );
		workspotOffset = ( ( Vector4 )( ScriptExecutionContext.GetMappingValue( context, m_workspotOffset ) ) );
		workspotRotation = ( ( Float )( ScriptExecutionContext.GetMappingValue( context, m_workspotRotation ) ) );
		navigationSystem = GameInstance.GetAINavigationSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		if( !( navigationSystem ) || !( followTarget ) )
		{
			return AIbehaviorUpdateOutcome.FAILURE;
		}
		if( Vector4.IsZero( queryExtents ) )
		{
			queryExtents = Vector4( 0.5, 0.5, 0.5, 0.0 );
		}
		targetDirection = ScriptExecutionContext.GetOwner( context ).GetWorldPosition() - followTarget.GetWorldPosition();
		targetDirection.Z = 0.0;
		targetDirection = Vector4.Normalize( targetDirection );
		if( !( Vector4.IsZero( targetDirection ) ) )
		{
			targetOrientation = Quaternion.BuildFromDirectionVector( targetDirection, Vector4( 0.0, 0.0, 1.0, 0.0 ) );
		}
		else
		{
			targetOrientation = followTarget.GetWorldOrientation();
		}
		if( !( GetNavmeshPointWithOffset( followTarget, teleportPosition, targetOrientation, Vector4() ) ) )
		{
			return AIbehaviorUpdateOutcome.FAILURE;
		}
		if( !( CheckForOverlapAdvanced( context, extentsOffset, queryExtents, teleportPosition, targetOrientation ) ) )
		{
			maybeStairs = CheckForStairs( context, teleportPosition );
			teleportPosition = teleportPosition + ( targetOrientation * workspotOffset );
			teleportRotation = Quaternion.ToEulerAngles( targetOrientation );
			ScriptExecutionContext.SetMappingValue( context, m_outPositionArgument, teleportPosition );
			ScriptExecutionContext.SetMappingValue( context, m_outRotationArgument, teleportRotation.Yaw + workspotRotation );
			ScriptExecutionContext.SetMappingValue( context, m_outMaybeStairs, maybeStairs );
			return AIbehaviorUpdateOutcome.SUCCESS;
		}
		return AIbehaviorUpdateOutcome.FAILURE;
	}

	private const function GetNavmeshPointWithOffset( const origin : weak< Entity >, out point : Vector4, const rotation : Quaternion, optional offset : Vector4 ) : Bool
	{
		var checkedResults : NavigationFindPointResult;
		var checkedPosition : Vector4;
		var checkedZOffset : Float;
		var navigationPath : NavigationPath;
		var navigationSystem : AINavigationSystem;
		if( !( origin ) )
		{
			return false;
		}
		navigationSystem = GameInstance.GetAINavigationSystem( ( ( GameObject )( origin ) ).GetGame() );
		checkedPosition = origin.GetWorldPosition() + ( rotation * offset );
		checkedResults = navigationSystem.FindPointBelowForCharacter( checkedPosition, 0.5, 5, origin, checkedZOffset );
		if( checkedResults.status != worldNavigationRequestStatus.OK )
		{
			return false;
		}
		checkedPosition.Z += checkedZOffset;
		navigationPath = navigationSystem.CalculatePathForCharacter( checkedResults.point, checkedPosition, 0.5, origin );
		if( navigationPath.path.Size() <= 0 )
		{
			return false;
		}
		point = checkedResults.point;
		return true;
	}

	private function DrawDebugBox( context : ScriptExecutionContext, position : Vector4, extents : Vector4, orientation : Quaternion )
	{
		var debugVisualizerSystem : DebugVisualizerSystem;
		var t : WorldTransform;
		var v0 : Vector4;
		var v1 : Vector4;
		var v2 : Vector4;
		var v3 : Vector4;
		debugVisualizerSystem = GameInstance.GetDebugVisualizerSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		WorldTransform.SetPosition( t, position );
		WorldTransform.SetOrientation( t, orientation );
		v0 = WorldPosition.ToVector4( WorldTransform.TransformPoint( t, Vector4( extents.X, -( extents.Y ), 0.0, 0.0 ) ) );
		v1 = WorldPosition.ToVector4( WorldTransform.TransformPoint( t, Vector4( extents.X, extents.Y, 0.0, 0.0 ) ) );
		v2 = WorldPosition.ToVector4( WorldTransform.TransformPoint( t, Vector4( -( extents.X ), extents.Y, 0.0, 0.0 ) ) );
		v3 = WorldPosition.ToVector4( WorldTransform.TransformPoint( t, Vector4( -( extents.X ), -( extents.Y ), 0.0, 0.0 ) ) );
		debugVisualizerSystem.DrawLine3D( v0, v1, Color( 255, 0, 0, 255 ), 15.0 );
		debugVisualizerSystem.DrawLine3D( v1, v2, Color( 255, 0, 0, 255 ), 15.0 );
		debugVisualizerSystem.DrawLine3D( v2, v3, Color( 255, 0, 0, 255 ), 15.0 );
		debugVisualizerSystem.DrawLine3D( v3, v0, Color( 255, 0, 0, 255 ), 15.0 );
	}

	private function CheckForOverlapAdvanced( context : ScriptExecutionContext, queryOffset : Vector4, queryExtents : Vector4, queryPosition : Vector4, out queryOrientation : Quaternion ) : Bool
	{
		var spatialQueriesSystem : SpatialQueriesSystem;
		var queryFilter : QueryFilter;
		var queryEnd : Vector4;
		var queryResult : TraceResult;
		var isHit : Bool;
		var perpDirection : Vector4;
		var perpOrientation : Quaternion;
		var worldNormal : Vector4;
		QueryFilter.AddGroup( queryFilter, 'Static' );
		QueryFilter.AddGroup( queryFilter, 'Dynamic' );
		QueryFilter.AddGroup( queryFilter, 'Terrain' );
		QueryFilter.AddGroup( queryFilter, 'Collider' );
		QueryFilter.AddGroup( queryFilter, 'Destructible' );
		if( !( CheckForOverlap( context, queryPosition, queryOffset, queryExtents, queryOrientation, queryFilter ) ) )
		{
			return false;
		}
		spatialQueriesSystem = GameInstance.GetSpatialQueriesSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		Quaternion.SetIdentity( perpOrientation );
		Quaternion.SetZRot( perpOrientation, Deg2Rad( 35.0 ) );
		perpOrientation = queryOrientation * perpOrientation;
		if( !( CheckForOverlap( context, queryPosition, queryOffset, queryExtents, perpOrientation, queryFilter ) ) )
		{
			queryOrientation = perpOrientation;
			return false;
		}
		Quaternion.SetIdentity( perpOrientation );
		Quaternion.SetZRot( perpOrientation, Deg2Rad( -35.0 ) );
		perpOrientation = queryOrientation * perpOrientation;
		if( !( CheckForOverlap( context, queryPosition, queryOffset, queryExtents, perpOrientation, queryFilter ) ) )
		{
			queryOrientation = perpOrientation;
			return false;
		}
		queryEnd = queryPosition + ( queryOrientation * Vector4( 0.0, ( queryExtents.Y * 2.0 ), 0.0, 0.0 ) );
		isHit = spatialQueriesSystem.SyncRaycastByQueryFilter( queryPosition, queryEnd, queryFilter, queryResult );
		if( isHit )
		{
			worldNormal = ( ( Vector4 )( queryResult.normal ) );
			worldNormal.Z = 0.0;
			worldNormal = Vector4.Normalize( worldNormal );
			perpDirection = Vector4.Cross( worldNormal, Vector4( 0.0, 0.0, 1.0, 0.0 ) );
			perpOrientation = Quaternion.BuildFromDirectionVector( perpDirection );
			if( !( CheckForOverlap( context, queryPosition, queryOffset, queryExtents, perpOrientation, queryFilter ) ) )
			{
				queryOrientation = perpOrientation;
				return false;
			}
			perpDirection = -( perpDirection );
			perpOrientation = Quaternion.BuildFromDirectionVector( perpDirection );
			if( !( CheckForOverlap( context, queryPosition, queryOffset, queryExtents, perpOrientation, queryFilter ) ) )
			{
				queryOrientation = perpOrientation;
				return false;
			}
		}
		queryEnd = queryPosition + ( queryOrientation * Vector4( 0.0, ( -( queryExtents.Y ) * 2.0 ), 0.0, 0.0 ) );
		isHit = spatialQueriesSystem.SyncRaycastByQueryFilter( queryPosition, queryEnd, queryFilter, queryResult );
		if( isHit )
		{
			worldNormal = ( ( Vector4 )( queryResult.normal ) );
			worldNormal.Z = 0.0;
			worldNormal = Vector4.Normalize( worldNormal );
			perpDirection = Vector4.Cross( worldNormal, Vector4( 0.0, 0.0, 1.0, 0.0 ) );
			perpOrientation = Quaternion.BuildFromDirectionVector( perpDirection );
			if( !( CheckForOverlap( context, queryPosition, queryOffset, queryExtents, perpOrientation, queryFilter ) ) )
			{
				queryOrientation = perpOrientation;
				return false;
			}
			perpDirection = -( perpDirection );
			perpOrientation = Quaternion.BuildFromDirectionVector( perpDirection );
			if( !( CheckForOverlap( context, queryPosition, queryOffset, queryExtents, perpOrientation, queryFilter ) ) )
			{
				queryOrientation = perpOrientation;
				return false;
			}
		}
		return true;
	}

	private function CheckForOverlap( context : ScriptExecutionContext, targetPosition : Vector4, targetOffset : Vector4, queryDimensions : Vector4, queryOrientation : Quaternion, queryFilter : QueryFilter ) : Bool
	{
		var spatialQueriesSystem : SpatialQueriesSystem;
		var queryExtents : Vector4;
		var queryPosition : Vector4;
		var queryResult : TraceResult;
		var groundPosition : Vector4;
		spatialQueriesSystem = GameInstance.GetSpatialQueriesSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		queryExtents = Vector4( queryDimensions.X * 0.5, queryDimensions.Y * 0.5, queryDimensions.Z * 0.5, queryDimensions.W );
		queryPosition = ( targetPosition + ( queryOrientation * targetOffset ) ) + Vector4( 0.0, 0.0, ( 0.1 + queryExtents.Z ), 0.0 );
		if( spatialQueriesSystem.GetDebugPreviewDuration() > 0.0 )
		{
			DrawDebugBox( context, queryPosition, queryExtents, queryOrientation );
		}
		groundPosition = targetPosition + ( queryOrientation * targetOffset );
		if( !( CheckForGround( context, groundPosition ) ) )
		{
			return true;
		}
		return spatialQueriesSystem.OverlapByQueryFilter( queryExtents, queryPosition, queryOrientation, queryFilter, queryResult );
	}

	private function CheckForGround( context : ScriptExecutionContext, targetPosition : Vector4 ) : Bool
	{
		var spatialQueriesSystem : SpatialQueriesSystem;
		var debugVisualizerSystem : DebugVisualizerSystem;
		var queryFilter : QueryFilter;
		var queryResult : TraceResult;
		var debugPreviewDuration : Float;
		spatialQueriesSystem = GameInstance.GetSpatialQueriesSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		debugVisualizerSystem = GameInstance.GetDebugVisualizerSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		debugPreviewDuration = spatialQueriesSystem.GetDebugPreviewDuration();
		if( debugPreviewDuration > 0.0 )
		{
			debugVisualizerSystem.DrawWireSphere( targetPosition, 0.25, Color( 44, 128, 44, 255 ), debugPreviewDuration );
		}
		QueryFilter.AddGroup( queryFilter, 'Static' );
		QueryFilter.AddGroup( queryFilter, 'Dynamic' );
		QueryFilter.AddGroup( queryFilter, 'Terrain' );
		QueryFilter.AddGroup( queryFilter, 'Collider' );
		QueryFilter.AddGroup( queryFilter, 'Destructible' );
		return spatialQueriesSystem.SyncRaycastByQueryFilter( targetPosition, targetPosition - Vector4( 0.0, 0.0, 1.0, 0.0 ), queryFilter, queryResult );
	}

	private function CheckForStairs( context : ScriptExecutionContext, targetPosition : Vector4 ) : Bool
	{
		var spatialQueriesSystem : SpatialQueriesSystem;
		var debugVisualizerSystem : DebugVisualizerSystem;
		var queryFilter : QueryFilter;
		var queryResult : TraceResult;
		var dotFacingTop : Float;
		var debugPreviewDuration : Float;
		spatialQueriesSystem = GameInstance.GetSpatialQueriesSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		debugVisualizerSystem = GameInstance.GetDebugVisualizerSystem( ScriptExecutionContext.GetOwner( context ).GetGame() );
		QueryFilter.AddGroup( queryFilter, 'Static' );
		QueryFilter.AddGroup( queryFilter, 'Dynamic' );
		QueryFilter.AddGroup( queryFilter, 'Terrain' );
		QueryFilter.AddGroup( queryFilter, 'Collider' );
		QueryFilter.AddGroup( queryFilter, 'Destructible' );
		if( !( spatialQueriesSystem.SyncRaycastByQueryFilter( targetPosition, targetPosition - Vector4( 0.0, 0.0, 1.0, 0.0 ), queryFilter, queryResult ) ) )
		{
			return false;
		}
		dotFacingTop = Vector4.Dot( ( ( Vector4 )( queryResult.normal ) ), Vector4( 0.0, 0.0, 1.0, 0.0 ) );
		if( dotFacingTop < 0.0 )
		{
			return false;
		}
		debugPreviewDuration = spatialQueriesSystem.GetDebugPreviewDuration();
		if( debugPreviewDuration > 0.0 )
		{
			debugVisualizerSystem.DrawText3D( targetPosition, FloatToStringPrec( dotFacingTop, 3 ), Color( 128, 44, 44, 255 ), debugPreviewDuration );
		}
		return ( dotFacingTop < 0.86602497 ) && ( dotFacingTop > 0.819152 );
	}

}