using System.Runtime.CompilerServices;
using UnityEngine;

namespace Fusion.Addons.SimpleKCC
{
    internal sealed class KCCNetworkProperties : KCCNetworkProperty<KCCNetworkContext>
    {
        private const int TRSP_POSITION_ACCURACY = 1024;

        private const int PROPERTIES_WORD_COUNT = 10;

        public KCCNetworkProperties(KCCNetworkContext context)
            : base(context, 24)
        {
        }

        public static void ReadPositions(NetworkBehaviourBuffer fromBuffer, NetworkBehaviourBuffer toBuffer, out Vector3 fromTargetPosition, out Vector3 toTargetPosition)
        {
            fromTargetPosition = fromBuffer.ReinterpretState<NetworkTRSPData>().Position;
            toTargetPosition = toBuffer.ReinterpretState<NetworkTRSPData>().Position;
            KCCInterpolationInfo interpolationInfo = default(KCCInterpolationInfo);
            interpolationInfo.FromBuffer = fromBuffer;
            interpolationInfo.ToBuffer = toBuffer;
            interpolationInfo.Offset = 14;
            ReadVector3s(ref interpolationInfo, out var fromValue, out var toValue);
            fromTargetPosition += fromValue;
            toTargetPosition += toValue;
        }

        public static void ReadTransforms(NetworkBehaviourBuffer fromBuffer, NetworkBehaviourBuffer toBuffer, out Vector3 fromTargetPosition, out Vector3 toTargetPosition, out float fromLookPitch, out float toLookPitch, out float fromLookYaw, out float toLookYaw)
        {
            fromTargetPosition = fromBuffer.ReinterpretState<NetworkTRSPData>().Position;
            toTargetPosition = toBuffer.ReinterpretState<NetworkTRSPData>().Position;
            int num = 17;
            fromLookPitch = fromBuffer.ReinterpretState<float>(num);
            toLookPitch = toBuffer.ReinterpretState<float>(num);
            num++;
            fromLookYaw = fromBuffer.ReinterpretState<float>(num);
            toLookYaw = toBuffer.ReinterpretState<float>(num);
        }

        public unsafe override void Read(int* ptr)
        {
            KCCData data = Context.Data;
            KCCSettings settings = Context.Settings;
            _ = Context.KCC.Runner;
            Vector3 position = ((NetworkTRSPData*)ptr)->Position;
            ptr += 14;
            data.TargetPosition = position + ReadVector3(ref ptr);
            data.LookPitch = ReadFloat(ref ptr);
            data.LookYaw = ReadFloat(ref ptr);
            int num = ReadInt(ref ptr);
            data.IsActive = (num & 1) == 1;
            data.IsGrounded = ((num >> 1) & 1) == 1;
            data.WasGrounded = ((num >> 2) & 1) == 1;
            data.IsSteppingUp = ((num >> 3) & 1) == 1;
            data.WasSteppingUp = ((num >> 4) & 1) == 1;
            data.IsSnappingToGround = ((num >> 5) & 1) == 1;
            data.WasSnappingToGround = ((num >> 6) & 1) == 1;
            data.HasTeleported = ((num >> 7) & 1) == 1;
            data.JumpFrames = (num >> 8) & 1;
            settings.IsTrigger = ((num >> 9) & 1) == 1;
            settings.Shape = (EKCCShape)((num >> 11) & 3);
            settings.ProxyInterpolationMode = (EKCCInterpolationMode)((num >> 17) & 3);
            settings.ColliderLayer = (num >> 19) & 0x1F;
            settings.CollisionLayerMask = ReadInt(ref ptr);
            settings.Radius = ReadFloat(ref ptr);
            settings.Height = ReadFloat(ref ptr);
            settings.Extent = ReadFloat(ref ptr);
        }

        public unsafe override void Write(int* ptr)
        {
            KCCData data = Context.Data;
            KCCSettings settings = Context.Settings;
            _ = Context.KCC.Runner;
            Vector3 targetPosition = data.TargetPosition;
            NetworkTRSPData* ptr2 = (NetworkTRSPData*)ptr;
            ptr2->Parent = NetworkBehaviourId.None;
            ptr2->Position = targetPosition;
            ptr += 14;
            Vector3 value = default(Vector3);
            if (!settings.CompressNetworkPosition)
            {
                Vector3 vector = default(Vector3);
                vector.x = FloatUtils.Decompress(FloatUtils.Compress(targetPosition.x));
                vector.y = FloatUtils.Decompress(FloatUtils.Compress(targetPosition.y));
                vector.z = FloatUtils.Decompress(FloatUtils.Compress(targetPosition.z));
                value = targetPosition - vector;
            }

            WriteVector3(value, ref ptr);
            WriteFloat(data.LookPitch, ref ptr);
            WriteFloat(data.LookYaw, ref ptr);
            int num = 0;
            if (data.IsActive)
            {
                num |= 1;
            }

            if (data.IsGrounded)
            {
                num |= 2;
            }

            if (data.WasGrounded)
            {
                num |= 4;
            }

            if (data.IsSteppingUp)
            {
                num |= 8;
            }

            if (data.WasSteppingUp)
            {
                num |= 0x10;
            }

            if (data.IsSnappingToGround)
            {
                num |= 0x20;
            }

            if (data.WasSnappingToGround)
            {
                num |= 0x40;
            }

            if (data.HasTeleported)
            {
                num |= 0x80;
            }

            if (data.JumpFrames != 0)
            {
                num |= 0x100;
            }

            if (settings.IsTrigger)
            {
                num |= 0x200;
            }

            num |= (int)(settings.Shape & (EKCCShape)3) << 11;
            num |= (int)(settings.ProxyInterpolationMode & (EKCCInterpolationMode)3) << 17;
            num |= (settings.ColliderLayer & 0x1F) << 19;
            WriteInt(num, ref ptr);
            WriteInt(settings.CollisionLayerMask, ref ptr);
            WriteFloat(settings.Radius, ref ptr);
            WriteFloat(settings.Height, ref ptr);
            WriteFloat(settings.Extent, ref ptr);
        }

        public override void Interpolate(KCCInterpolationInfo interpolationInfo)
        {
            KCCData data = Context.Data;
            KCCSettings settings = Context.Settings;
            _ = Context.KCC.Runner;
            Vector3 position = interpolationInfo.FromBuffer.ReinterpretState<NetworkTRSPData>().Position;
            Vector3 position2 = interpolationInfo.ToBuffer.ReinterpretState<NetworkTRSPData>().Position;
            interpolationInfo.Offset += 14;
            ReadVector3s(ref interpolationInfo, out var fromValue, out var toValue);
            position += fromValue;
            position2 += toValue;
            data.BasePosition = position;
            data.DesiredPosition = position2;
            data.TargetPosition = Vector3.Lerp(position, position2, interpolationInfo.Alpha);
            ReadFloats(ref interpolationInfo, out var fromValue2, out var toValue2);
            data.LookPitch = Mathf.Lerp(fromValue2, toValue2, interpolationInfo.Alpha);
            ReadFloats(ref interpolationInfo, out var fromValue3, out var toValue3);
            data.LookYaw = KCCUtility.InterpolateRange(fromValue3, toValue3, -180f, 180f, interpolationInfo.Alpha);
            interpolationInfo.Offset += 5;
            int num = (int)interpolationInfo.ToBuffer.Tick - (int)interpolationInfo.FromBuffer.Tick;
            if (num > 0)
            {
                Vector3 vector = position2 - position;
                if (vector.sqrMagnitude > settings.TeleportThreshold * settings.TeleportThreshold * (float)num * (float)num)
                {
                    data.HasTeleported = true;
                    data.TargetPosition = position2;
                    data.RealVelocity = Vector3.zero;
                    data.RealSpeed = 0f;
                }
                else
                {
                    data.RealVelocity = vector / (data.DeltaTime * (float)num);
                    data.RealSpeed = data.RealVelocity.magnitude;
                }
            }
            else
            {
                data.RealVelocity = Vector3.zero;
                data.RealSpeed = 0f;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static int ReadInt(ref int* ptr)
        {
            int result = *ptr;
            ptr++;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static int ReadInt(ref int* ptrFrom, ref int* ptrTo, float alpha)
        {
            int result = ((alpha < 0.5f) ? (*ptrFrom) : (*ptrTo));
            ptrFrom++;
            ptrTo++;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static void WriteInt(int value, ref int* ptr)
        {
            *ptr = value;
            ptr++;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static float ReadFloat(ref int* ptr)
        {
            float result = *(float*)ptr;
            ptr++;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static float ReadFloat(ref int* ptrFrom, ref int* ptrTo, float alpha)
        {
            float result = ((alpha < 0.5f) ? (*(float*)ptrFrom) : (*(float*)ptrTo));
            ptrFrom++;
            ptrTo++;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static void WriteFloat(float value, ref int* ptr)
        {
            *(float*)ptr = value;
            ptr++;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static bool CompareAndWriteFloat(float value, ref int* ptr)
        {
            bool result = true;
            if (*(float*)ptr != value)
            {
                *(float*)ptr = value;
                result = false;
            }

            ptr++;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static Vector3 ReadVector3(ref int* ptr)
        {
            Vector3 result = default(Vector3);
            result.x = *(float*)ptr;
            ptr++;
            result.y = *(float*)ptr;
            ptr++;
            result.z = *(float*)ptr;
            ptr++;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static void WriteVector3(Vector3 value, ref int* ptr)
        {
            *(float*)ptr = value.x;
            ptr++;
            *(float*)ptr = value.y;
            ptr++;
            *(float*)ptr = value.z;
            ptr++;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe static bool CompareAndWriteVector3(Vector3 value, ref int* ptr)
        {
            bool result = true;
            if (*(float*)ptr != value.x)
            {
                *(float*)ptr = value.x;
                result = false;
            }

            ptr++;
            if (*(float*)ptr != value.y)
            {
                *(float*)ptr = value.y;
                result = false;
            }

            ptr++;
            if (*(float*)ptr != value.z)
            {
                *(float*)ptr = value.z;
                result = false;
            }

            ptr++;
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int InterpolateInt(ref KCCInterpolationInfo interpolationInfo)
        {
            int result = interpolationInfo.FromBuffer.ReinterpretState<int>(interpolationInfo.Offset);
            int result2 = interpolationInfo.ToBuffer.ReinterpretState<int>(interpolationInfo.Offset);
            interpolationInfo.Offset++;
            if (!(interpolationInfo.Alpha < 0.5f))
            {
                return result2;
            }

            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static float InterpolateFloat(ref KCCInterpolationInfo interpolationInfo)
        {
            float result = interpolationInfo.FromBuffer.ReinterpretState<float>(interpolationInfo.Offset);
            float result2 = interpolationInfo.ToBuffer.ReinterpretState<float>(interpolationInfo.Offset);
            interpolationInfo.Offset++;
            if (!(interpolationInfo.Alpha < 0.5f))
            {
                return result2;
            }

            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ReadFloats(ref KCCInterpolationInfo interpolationInfo, out float fromValue, out float toValue)
        {
            fromValue = interpolationInfo.FromBuffer.ReinterpretState<float>(interpolationInfo.Offset);
            toValue = interpolationInfo.ToBuffer.ReinterpretState<float>(interpolationInfo.Offset);
            interpolationInfo.Offset++;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ReadVector3s(ref KCCInterpolationInfo interpolationInfo, out Vector3 fromValue, out Vector3 toValue)
        {
            int offset = interpolationInfo.Offset;
            fromValue.x = interpolationInfo.FromBuffer.ReinterpretState<float>(offset);
            fromValue.y = interpolationInfo.FromBuffer.ReinterpretState<float>(offset + 1);
            fromValue.z = interpolationInfo.FromBuffer.ReinterpretState<float>(offset + 2);
            toValue.x = interpolationInfo.ToBuffer.ReinterpretState<float>(offset);
            toValue.y = interpolationInfo.ToBuffer.ReinterpretState<float>(offset + 1);
            toValue.z = interpolationInfo.ToBuffer.ReinterpretState<float>(offset + 2);
            interpolationInfo.Offset += 3;
        }
    }
}