using UnityEngine;

namespace Fusion.Addons.SimpleKCC
{
    internal static class EnvironmentProcessor
    {
        public static void SetDynamicVelocity(KCC kcc, KCCData data)        => SetDynamicVelocity(kcc.FixedData, kcc.Rigidbody.mass, kcc.IsInFixedUpdate, data);
        public static void SetDynamicVelocity(KCCOffline kcc, KCCData data) => SetDynamicVelocity(kcc.FixedData, kcc.Rigidbody.mass, kcc.IsInFixedUpdate, data);

        private static void SetDynamicVelocity(KCCData kccFixedData, float mass, bool isInFixedUpdate, KCCData data)
        {
            // KCCData fixedData = kcc.FixedData;
            KCCData fixedData = kccFixedData;
            float deltaTime = fixedData.DeltaTime;
            Vector3 vector = fixedData.DynamicVelocity;
            if (!fixedData.IsGrounded || (!fixedData.IsSteppingUp && (fixedData.IsSnappingToGround || fixedData.GroundDistance > 0.001f)))
            {
                vector += data.Gravity * deltaTime;
            }

            if (!data.JumpImpulse.IsZero())
            {
                Vector3 normalized = data.JumpImpulse.normalized;
                vector -= Vector3.Scale(vector, normalized);
                // vector += data.JumpImpulse / kcc.Rigidbody.mass;
                vector += data.JumpImpulse / mass;
                data.JumpFrames++;
            }

            if (!vector.IsZero() && vector.IsAlmostZero(0.001f))
            {
                vector = default(Vector3);
            }

            data.DynamicVelocity.x = 0f;
            data.DynamicVelocity.z = 0f;
            data.DynamicVelocity = vector;
            // if (kcc.IsInFixedUpdate)
            if (isInFixedUpdate)
            {
                data.JumpImpulse = default(Vector3);
            }
        }

        public static void AfterMoveStep(KCC kcc, KCCData data)        => AfterMoveStep(kcc.FixedData, kcc.IsInFixedUpdate, data);    
        public static void AfterMoveStep(KCCOffline kcc, KCCData data) => AfterMoveStep(kcc.FixedData, kcc.IsInFixedUpdate, data); 

        private static void AfterMoveStep(KCCData kccFixedData, bool isInFixedUpdate, KCCData data)
        {
            // KCCData fixedData = kcc.FixedData;
            KCCData fixedData = kccFixedData;
            if (data.IsGrounded)
            {
                if (fixedData.WasGrounded && !data.IsSnappingToGround && data.DynamicVelocity.y < 0f && data.DynamicVelocity.OnlyXZ().IsAlmostZero())
                {
                    data.DynamicVelocity.y = 0f;
                }

                if (!fixedData.WasGrounded)
                {
                    Vector3 projectedVector;
                    if (data.KinematicVelocity.OnlyXZ().IsAlmostZero())
                    {
                        data.KinematicVelocity.y = 0f;
                    }
                    else if (KCCPhysicsUtility.ProjectOnGround(data.GroundNormal, data.KinematicVelocity, out projectedVector))
                    {
                        data.KinematicVelocity = projectedVector.normalized * data.KinematicVelocity.magnitude;
                    }
                }
            }
            // else if (!fixedData.WasGrounded && data.DynamicVelocity.y > 0f && data.DeltaTime > 0f && ((!kcc.IsInFixedUpdate) ? ((data.TargetPosition - fixedData.TargetPosition) / (fixedData.DeltaTime * data.Alpha)) : ((data.TargetPosition - data.BasePosition) / data.DeltaTime)).y.IsAlmostZero())
            else if (!fixedData.WasGrounded && data.DynamicVelocity.y > 0f && data.DeltaTime > 0f && ((!isInFixedUpdate) ? ((data.TargetPosition - fixedData.TargetPosition) / (fixedData.DeltaTime * data.Alpha)) : ((data.TargetPosition - data.BasePosition) / data.DeltaTime)).y.IsAlmostZero())
            {
                data.DynamicVelocity.y = 0f;
            }
        }
    }
}