using UnityEngine;

namespace Fusion.Addons.SimpleKCC
{
    internal static class GroundSnapProcessor
    {
        private static KCCOverlapInfo _overlapInfo = new KCCOverlapInfo();

        public static void AfterMoveStep(KCC kcc, KCCData data, KCCOverlapInfo baseOverlapInfo)
        {
            if (kcc.Settings.SnapDistance <= 0f || data.IsGrounded || !data.WasGrounded || data.JumpFrames > 0 || data.IsSteppingUp || data.WasSteppingUp || data.DynamicVelocity.y > 0f)
            {
                return;
            }

            float snapDistance = kcc.Settings.SnapDistance;
            float num = kcc.Settings.Radius * 0.25f;
            int num2 = Mathf.CeilToInt(snapDistance / num);
            float num3 = snapDistance / (float)num2;
            kcc.CapsuleOverlap(_overlapInfo, data.TargetPosition - new Vector3(0f, kcc.Settings.SnapDistance, 0f), kcc.Settings.Radius, kcc.Settings.Height + kcc.Settings.SnapDistance, QueryTriggerInteraction.Ignore);
            if (_overlapInfo.ColliderHitCount == 0)
            {
                return;
            }

            Vector3 vector = data.TargetPosition;
            Vector3 vector2 = new Vector3(0f, 0f - num3, 0f);
            for (int i = 0; i < num2; i++)
            {
                vector = kcc.ResolvePenetration(_overlapInfo, data, vector, vector + vector2, probeGrounding: false, 0, 0, resolveTriggers: false);
                if (data.IsGrounded)
                {
                    float num4 = kcc.Settings.SnapSpeed * data.UpdateDeltaTime;
                    Vector3 vector3 = vector - data.TargetPosition;
                    if (!data.WasSnappingToGround)
                    {
                        num4 *= 0.5f;
                    }

                    Vector3 vector4 = (data.TargetPosition = ((!(vector3.sqrMagnitude <= num4 * num4)) ? (data.TargetPosition + vector3.normalized * num4) : vector));
                    data.GroundDistance = Mathf.Max(0f, vector4.y - vector.y);
                    data.IsSnappingToGround = true;
                    kcc.UpdateHits(baseOverlapInfo, EKCCHitsOverlapQuery.Default);
                    break;
                }
            }
        }

        public static void AfterMoveStep(KCCOffline kcc, KCCData data, KCCOverlapInfo baseOverlapInfo)
        {
            if (kcc.Settings.SnapDistance <= 0f || data.IsGrounded || !data.WasGrounded || data.JumpFrames > 0 || data.IsSteppingUp || data.WasSteppingUp || data.DynamicVelocity.y > 0f)
            {
                return;
            }

            float snapDistance = kcc.Settings.SnapDistance;
            float num = kcc.Settings.Radius * 0.25f;
            int num2 = Mathf.CeilToInt(snapDistance / num);
            float num3 = snapDistance / (float)num2;
            kcc.CapsuleOverlap(_overlapInfo, data.TargetPosition - new Vector3(0f, kcc.Settings.SnapDistance, 0f), kcc.Settings.Radius, kcc.Settings.Height + kcc.Settings.SnapDistance, QueryTriggerInteraction.Ignore);
            if (_overlapInfo.ColliderHitCount == 0)
            {
                return;
            }

            Vector3 vector = data.TargetPosition;
            Vector3 vector2 = new Vector3(0f, 0f - num3, 0f);
            for (int i = 0; i < num2; i++)
            {
                vector = kcc.ResolvePenetration(_overlapInfo, data, vector, vector + vector2, probeGrounding: false, 0, 0, resolveTriggers: false);
                if (data.IsGrounded)
                {
                    float num4 = kcc.Settings.SnapSpeed * data.UpdateDeltaTime;
                    Vector3 vector3 = vector - data.TargetPosition;
                    if (!data.WasSnappingToGround)
                    {
                        num4 *= 0.5f;
                    }

                    Vector3 vector4 = (data.TargetPosition = ((!(vector3.sqrMagnitude <= num4 * num4)) ? (data.TargetPosition + vector3.normalized * num4) : vector));
                    data.GroundDistance = Mathf.Max(0f, vector4.y - vector.y);
                    data.IsSnappingToGround = true;
                    kcc.UpdateHits(baseOverlapInfo, EKCCHitsOverlapQuery.Default);
                    break;
                }
            }
        }
    }
}