using System.Runtime.InteropServices;
using UnityEngine;
using Object = UnityEngine.Object;

public static class PhysicsUtils
{
	private static readonly Collider[] _colliders = new Collider[512];

	private static SphereCollider _probeSphere;

	private static SphereCollider ProbeSphere
	{
		get
		{
			if (_probeSphere == null)
			{
				var go = new GameObject(nameof(ProbeSphere));
				go.hideFlags = HideFlags.HideInInspector | HideFlags.HideInHierarchy;
				Object.DontDestroyOnLoad(go);
				_probeSphere = go.AddComponent<SphereCollider>();
				_probeSphere.enabled = false;
			}

			return _probeSphere;
		}
	}

	public static bool ClosestPoint(Vector3 position, float checkRadius, out RaycastHit hit, int layerMask = ~0, float epsilon = 1e-5f)
	{
		hit = default;

		var overlaps = Physics.OverlapSphereNonAlloc(position, checkRadius + 0.001f, _colliders, layerMask, QueryTriggerInteraction.Ignore);
		if (overlaps == 0)
		{
			return false;
		}

		var minDistance = float.MaxValue;
		var closestNormal = Vector3.zero;
		Collider closestCollider = null;

		ProbeSphere.enabled = true;

		for (var i = 0; i < overlaps; i++)
		{
			var collider = _colliders[i];

			if (ClosestPointOnCollider(checkRadius, position, collider,
				collider.transform.position, collider.transform.rotation,
				out var normal, out var distance, epsilon))
			{
				if (distance < minDistance)
				{
					minDistance = distance;
					closestNormal = normal;
					closestCollider = collider;
				}
			}
		}

		ProbeSphere.enabled = false;

		if (minDistance.Equals(float.MaxValue))
		{
			return false;
		}

		hit.distance = minDistance;
		hit.point = position - closestNormal * minDistance;
		hit.normal = closestNormal;
		SetHitCollider(ref hit, closestCollider);

		return true;
	}

	private static bool ClosestPointOnCollider(
		float overestimate,
		Vector3 position,
		Collider collider,
		Vector3 colliderPosition,
		Quaternion colliderRotation,
		out Vector3 normal,
		out float distance,
		float epsilon = 1e-5f)
	{
		if (collider is not MeshCollider meshCollider || meshCollider.convex)
		{
			var direction = position - collider.ClosestPoint(position);
			distance = direction.magnitude;
			normal = direction / distance;
			return distance <= radius;
		}

		var probeSphere = ProbeSphere;

		probeSphere.radius = radius;

		if (!Physics.ComputePenetration(
			probeSphere, position, Quaternion.identity,
			collider, colliderPosition, colliderRotation,
			out normal, out var depth))
		{
			distance = default;
			return false;
		}

		distance = radius - depth;

		radius *= 0.5f;

		var step = radius * 0.5f;

		while (step > epsilon)
		{
			probeSphere.radius = radius;

			if (Physics.ComputePenetration(
				probeSphere, position, Quaternion.identity,
				collider, colliderPosition, colliderRotation,
				out var currentNormal, out depth))
			{
				distance = radius - depth;
				normal = currentNormal;

				radius -= step;
			}
			else
			{
				radius += step;
			}

			step *= 0.5f;
		}

		return true;
	}

	private static readonly int _sizeOfHit = Marshal.SizeOf<RaycastHit>();
	private static readonly int _colliderFieldOffset = (int)Marshal.OffsetOf<RaycastHit>("m_Collider");

	private static void SetHitCollider(ref RaycastHit hit, Collider collider)
	{
		var colliderId = collider.GetInstanceID();
		var bytes = MemoryMarshal.AsBytes(MemoryMarshal.CreateSpan(ref hit, _sizeOfHit));
		MemoryMarshal.Write(bytes.Slice(_colliderFieldOffset, sizeof(int)), ref colliderId);
	}
}