using RoR2.ConVar;
using UnityEngine;

namespace RoR2;

public class LocalNavigator
{
	private readonly struct BodyComponents
	{
		public readonly CharacterBody body;

		public readonly Transform transform;

		public readonly CharacterMotor motor;

		public readonly Collider bodyCollider;

		public BodyComponents(CharacterBody body)
		{
			this = default(BodyComponents);
			this.body = body;
			if ((bool)body)
			{
				transform = body.transform;
				motor = body.characterMotor;
				if ((bool)motor)
				{
					bodyCollider = body.characterMotor.Motor.Capsule;
				}
			}
		}
	}

	private readonly struct BodySnapshot
	{
		public readonly Vector3 position;

		public readonly Vector3 chestPosition;

		public readonly Vector3 footPosition;

		public readonly Vector3 motorMoveDirection;

		public readonly Vector3 motorVelocity;

		public readonly float maxMoveSpeed;

		public readonly float acceleration;

		public readonly float maxJumpHeight;

		public readonly float maxJumpSpeed;

		public readonly bool isGrounded;

		public readonly float time;

		public readonly float bodyRadius;

		public readonly bool isFlying;

		public readonly bool isJumping;

		private readonly bool hasBody;

		private readonly bool hasBodyCollider;

		private readonly bool hasMotor;

		public bool isValid => hasBody;

		public bool canJump
		{
			get
			{
				if (isGrounded)
				{
					return maxJumpSpeed > 0f;
				}
				return false;
			}
		}

		public BodySnapshot(in BodyComponents bodyComponents, float time)
		{
			this = default(BodySnapshot);
			this.time = time;
			hasBody = bodyComponents.body;
			hasBodyCollider = bodyComponents.bodyCollider;
			hasMotor = bodyComponents.motor;
			if ((bool)bodyComponents.body)
			{
				position = bodyComponents.transform.position;
				chestPosition = position;
				footPosition = position;
				maxMoveSpeed = bodyComponents.body.moveSpeed;
				acceleration = bodyComponents.body.acceleration;
				maxJumpHeight = bodyComponents.body.maxJumpHeight;
				maxJumpSpeed = bodyComponents.body.jumpPower;
				bodyRadius = bodyComponents.body.radius;
				isFlying = bodyComponents.body.isFlying;
			}
			if ((bool)bodyComponents.bodyCollider)
			{
				Bounds bounds = bodyComponents.bodyCollider.bounds;
				bounds.center = Vector3.zero;
				chestPosition.y += bounds.max.y * 0.5f;
				footPosition.y += bounds.min.y;
			}
			if ((bool)bodyComponents.motor)
			{
				isGrounded = bodyComponents.motor.isGrounded;
				motorVelocity = bodyComponents.motor.velocity;
				isJumping = !isGrounded;
			}
		}
	}

	private readonly struct SnapshotDelta
	{
		public readonly float deltaTime;

		public readonly Vector3 estimatedVelocity;

		public readonly bool isValid;

		public SnapshotDelta(in BodySnapshot oldSnapshot, in BodySnapshot newSnapshot)
		{
			this = default(SnapshotDelta);
			if (oldSnapshot.isValid && newSnapshot.isValid)
			{
				deltaTime = newSnapshot.time - oldSnapshot.time;
				isValid = deltaTime > 0f;
				if (isValid)
				{
					estimatedVelocity = (newSnapshot.position - oldSnapshot.position) / deltaTime;
				}
			}
		}
	}

	private struct RaycastResults
	{
		public bool forwardObstructed;

		public bool leftObstructed;

		public bool rightObstructed;
	}

	private Vector3 previousMoveVector;

	public Vector3 targetPosition;

	public float avoidanceDuration = 0.5f;

	private float avoidanceTimer;

	public bool allowWalkOffCliff;

	public bool wasObstructedLastUpdate;

	private float localTime;

	private float raycastTimer;

	private static readonly float raycastUpdateInterval = 0.2f;

	private static readonly float avoidanceAngle = 45f;

	private const bool enableFrustration = true;

	private const bool enableWhiskers = true;

	private const bool enableCliffAvoidance = true;

	private BodyComponents bodyComponents;

	private float walkFrustration;

	private const float frustrationLimit = 1f;

	private const float frustrationDecayRate = 0.25f;

	private const float frustrationMinimumSpeed = 0.1f;

	private float aiStopwatch;

	private Vector3 velocityDelta;

	private Vector3 estimatedAcceleration;

	private float accelerationAccuracy;

	private float moveDirectionAccuracy;

	private bool hasMadeSharpTurn;

	private float speedAsFractionOfTopSpeed;

	private bool isAlreadyMovingAtSufficientSpeed;

	private BodySnapshot currentSnapshot;

	private BodySnapshot previousSnapshot;

	private SnapshotDelta currentSnapshotDelta;

	private SnapshotDelta previousSnapshotDelta;

	private RaycastResults raycastResults;

	private static readonly BoolConVar cvLocalNavigatorDebugDraw = new BoolConVar("local_navigator_debug_draw", ConVarFlags.None, "0", "Enables debug drawing of LocalNavigator (drawing visible in editor only).\n  Orange Line: Current position to target position\n  Yellow Line: Raycasts\n  Red Point: Raycast hit position\n  Green Line: Final chosen move vector");

	public Vector3 moveVector { get; private set; }

	public float jumpSpeed { get; private set; }

	public void SetBody(CharacterBody newBody)
	{
		bodyComponents = new BodyComponents(newBody);
		PushSnapshot();
		PushSnapshot();
	}

	private void PushSnapshot()
	{
		previousSnapshot = currentSnapshot;
		currentSnapshot = new BodySnapshot(in bodyComponents, localTime);
		previousSnapshotDelta = currentSnapshotDelta;
		currentSnapshotDelta = new SnapshotDelta(in previousSnapshot, in currentSnapshot);
	}

	public void Update(float deltaTime)
	{
		localTime += deltaTime;
		PushSnapshot();
		if (cvLocalNavigatorDebugDraw.value)
		{
			Debug.DrawLine(currentSnapshot.position, targetPosition, new Color32(byte.MaxValue, 127, 39, 127), deltaTime);
		}
		wasObstructedLastUpdate = false;
		jumpSpeed = 0f;
		previousMoveVector = moveVector;
		Vector3 vector = targetPosition - currentSnapshot.position;
		if (!currentSnapshot.isFlying)
		{
			vector.y = 0f;
		}
		float magnitude = vector.magnitude;
		Vector3 positionToTargetNormalized = vector / magnitude;
		Vector3 vector2 = moveVector;
		CompensateForVelocityByRefinement(deltaTime, ref vector2, in positionToTargetNormalized);
		moveVector = vector2;
		if (magnitude == 0f)
		{
			moveVector = Vector3.zero;
		}
		else
		{
			raycastResults = GetRaycasts(in currentSnapshot, moveVector, deltaTime);
			if (raycastResults.forwardObstructed)
			{
				int num = ((!raycastResults.leftObstructed) ? (-1) : 0) + ((!raycastResults.rightObstructed) ? 1 : 0);
				if (num == 0)
				{
					num = ((Random.Range(0, 1) != 1) ? 1 : (-1));
				}
				moveVector = Quaternion.Euler(0f, (0f - avoidanceAngle) * (float)num, 0f) * moveVector;
				wasObstructedLastUpdate = raycastResults.leftObstructed || raycastResults.rightObstructed;
			}
			float deltaTime2 = deltaTime + 0.4f;
			ref BodySnapshot reference = ref currentSnapshot;
			ref SnapshotDelta reference2 = ref currentSnapshotDelta;
			Vector3 currentMoveVector = moveVector;
			if (CheckCliffAhead(in reference, in reference2, in currentMoveVector, deltaTime2))
			{
				wasObstructedLastUpdate = true;
				moveVector = -moveVector;
				ref BodySnapshot reference3 = ref currentSnapshot;
				ref SnapshotDelta reference4 = ref currentSnapshotDelta;
				currentMoveVector = moveVector;
				if (CheckCliffAhead(in reference3, in reference4, in currentMoveVector, deltaTime2))
				{
					moveVector *= 0.25f;
				}
			}
			CalculateFrustration(deltaTime, ref walkFrustration);
			if (walkFrustration >= 1f)
			{
				jumpSpeed = currentSnapshot.maxJumpSpeed;
			}
		}
		avoidanceTimer -= deltaTime;
		walkFrustration = Mathf.Clamp(walkFrustration - deltaTime * 0.25f, 0f, 1f);
		if (cvLocalNavigatorDebugDraw.value)
		{
			Debug.DrawRay(currentSnapshot.position, moveVector * 5f, Color.green, deltaTime, depthTest: false);
		}
	}

	private void CompensateForVelocityByRefinement(float nextExpectedDeltaTime, ref Vector3 moveVector, in Vector3 positionToTargetNormalized)
	{
		Vector3 estimatedVelocity = currentSnapshotDelta.estimatedVelocity;
		_ = currentSnapshot.acceleration;
		float num = Vector3.Dot(estimatedVelocity, positionToTargetNormalized);
		Vector3 vector = positionToTargetNormalized;
		Vector3 vector2 = vector;
		int num2 = 8;
		if (!currentSnapshot.isJumping)
		{
			for (int i = 0; i < num2; i++)
			{
				EstimateNextMovement(in currentSnapshot, in currentSnapshotDelta, in vector2, nextExpectedDeltaTime, out var nextPosition, out var nextVelocity);
				Vector3 vector3 = targetPosition - nextPosition;
				if (!currentSnapshot.isFlying)
				{
					vector3.y = 0f;
				}
				Vector3 normalized = vector3.normalized;
				vector2 = normalized;
				if (Vector3.Dot(nextVelocity, normalized) > num)
				{
					vector = normalized;
				}
			}
		}
		moveVector = vector;
	}

	private void CompensateForVelocityByBruteForce(float nextExpectedDeltaTime, ref Vector3 moveVector, in Vector3 positionToTargetNormalized)
	{
		Vector3 estimatedVelocity = currentSnapshotDelta.estimatedVelocity;
		_ = currentSnapshot;
		float num = Vector3.Dot(estimatedVelocity.normalized, positionToTargetNormalized);
		Vector3 vector = moveVector;
		int num2 = 16;
		float num3 = 360f / (float)num2;
		for (int i = 0; i < num2; i++)
		{
			Vector3 vector2 = Quaternion.AngleAxis((float)i * num3, Vector3.up) * Vector3.forward;
			EstimateNextMovement(in currentSnapshot, in currentSnapshotDelta, in vector2, nextExpectedDeltaTime, out var nextPosition, out var nextVelocity);
			float num4 = Vector3.Dot((targetPosition - nextPosition).normalized, nextVelocity.normalized);
			if (num4 > num)
			{
				num = num4;
				vector = vector2;
			}
		}
		moveVector = vector;
	}

	private static void EstimateNextMovement(in BodySnapshot currentSnapshot, in SnapshotDelta currentSnapshotDelta, in Vector3 moveVector, float nextDeltaTime, out Vector3 nextPosition, out Vector3 nextVelocity)
	{
		Vector3 estimatedVelocity = currentSnapshotDelta.estimatedVelocity;
		float acceleration = currentSnapshot.acceleration;
		nextVelocity = Vector3.MoveTowards(estimatedVelocity, moveVector * currentSnapshot.maxMoveSpeed, acceleration * nextDeltaTime);
		float num = Mathf.Min((nextVelocity - estimatedVelocity).magnitude / acceleration, nextDeltaTime);
		float num2 = nextDeltaTime - num;
		Vector3 vector = (nextVelocity - estimatedVelocity) / num;
		Vector3 vector2 = nextVelocity * num + vector * (0.5f * num * num);
		Vector3 vector3 = nextVelocity * num2;
		nextPosition = currentSnapshot.position + vector2 + vector3;
	}

	private RaycastResults GetRaycasts(in BodySnapshot currentSnapshot, Vector3 positionToTargetNormalized, float lookaheadTime)
	{
		RaycastResults result = default(RaycastResults);
		Vector3 origin = currentSnapshot.chestPosition;
		LayerMask layerMask = (int)LayerIndex.world.mask | (int)LayerIndex.CommonMasks.characterBodiesOrDefault;
		float maxDistance = currentSnapshot.bodyRadius + currentSnapshot.maxMoveSpeed * lookaheadTime;
		result.forwardObstructed = Raycast(in origin, in positionToTargetNormalized, out var hitInfo, maxDistance, layerMask);
		if (result.forwardObstructed)
		{
			Vector3 direction = Quaternion.Euler(0f, avoidanceAngle, 0f) * positionToTargetNormalized;
			result.leftObstructed = Raycast(in origin, in direction, out hitInfo, maxDistance, layerMask);
			direction = Quaternion.Euler(0f, 0f - avoidanceAngle, 0f) * positionToTargetNormalized;
			result.rightObstructed = Raycast(in origin, in direction, out hitInfo, maxDistance, layerMask);
		}
		return result;
	}

	private bool Raycast(in Vector3 origin, in Vector3 direction, out RaycastHit hitInfo, float maxDistance, LayerMask layerMask)
	{
		bool flag = Physics.Raycast(origin, direction, out hitInfo, maxDistance, layerMask);
		if (cvLocalNavigatorDebugDraw.value)
		{
			Debug.DrawRay(origin, direction.normalized * maxDistance, Color.yellow, raycastUpdateInterval);
			if (flag)
			{
				Util.DebugCross(hitInfo.point, 1f, Color.red, raycastUpdateInterval);
			}
		}
		return flag;
	}

	private bool Linecast(in Vector3 start, in Vector3 end, out RaycastHit hitInfo, LayerMask layerMask)
	{
		bool flag = Physics.Linecast(start, end, out hitInfo, layerMask);
		if (cvLocalNavigatorDebugDraw.value)
		{
			Debug.DrawLine(start, end, Color.yellow, raycastUpdateInterval);
			if (flag)
			{
				Util.DebugCross(hitInfo.point, 1f, Color.red, raycastUpdateInterval);
			}
		}
		return flag;
	}

	private bool CheckCliffAhead(in BodySnapshot currentSnapshot, in SnapshotDelta currentSnapshotDelta, in Vector3 currentMoveVector, float deltaTime)
	{
		if (!allowWalkOffCliff && currentSnapshot.isGrounded)
		{
			EstimateNextMovement(in currentSnapshot, in currentSnapshotDelta, in currentMoveVector, deltaTime, out var nextPosition, out var _);
			float num = currentSnapshot.chestPosition.y - currentSnapshot.position.y;
			float num2 = currentSnapshot.footPosition.y - currentSnapshot.position.y;
			Vector3 start = nextPosition;
			start.y += num;
			Vector3 end = start;
			end.y += num2;
			end.y -= 4f;
			RaycastHit hitInfo;
			return !Linecast(in start, in end, out hitInfo, LayerIndex.world.mask);
		}
		return false;
	}

	private void CalculateFrustration(float deltaTime, ref float frustration)
	{
		if (!currentSnapshot.canJump)
		{
			frustration = 0f;
		}
		else
		{
			if (!currentSnapshot.isValid || !currentSnapshotDelta.isValid || !currentSnapshotDelta.isValid || !previousSnapshotDelta.isValid)
			{
				return;
			}
			if (true)
			{
				Vector3 rhs = FlattenDirection(moveVector);
				Vector3 vector2 = FlattenDirection(currentSnapshotDelta.estimatedVelocity);
				Vector3 vector3 = FlattenDirection(previousSnapshotDelta.estimatedVelocity);
				float num = Vector3.Dot(vector2, rhs);
				velocityDelta = vector2 - vector3;
				estimatedAcceleration = velocityDelta / deltaTime;
				float num2 = Vector3.Dot(estimatedAcceleration, rhs);
				float num3 = estimatedAcceleration.magnitude - num2;
				speedAsFractionOfTopSpeed = num / currentSnapshot.maxMoveSpeed;
				isAlreadyMovingAtSufficientSpeed = speedAsFractionOfTopSpeed >= 0.45f;
				if (isAlreadyMovingAtSufficientSpeed)
				{
					frustration = 0f;
				}
				else if (num3 > num2 * 2f)
				{
					frustration += 1.25f * deltaTime;
				}
				return;
			}
			float num4 = currentSnapshot.motorVelocity.magnitude + bodyComponents.motor.rootMotion.magnitude / deltaTime;
			float magnitude = (targetPosition - currentSnapshot.position).magnitude;
			if (currentSnapshot.maxMoveSpeed != 0f && num4 != 0f && magnitude > Mathf.Epsilon)
			{
				float magnitude2 = currentSnapshotDelta.estimatedVelocity.magnitude;
				if (magnitude2 <= num4 || magnitude2 <= 0.1f)
				{
					frustration += 1.25f * deltaTime;
				}
			}
		}
		static Vector3 FlattenDirection(Vector3 vector)
		{
			if (Mathf.Abs(vector.y) > 0f)
			{
				vector.y = 0f;
			}
			return vector;
		}
	}
}