using Passer.LinearAlgebra;

namespace Passer.Control.Core
{
    public class LowLevelMessages
    {

        public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
        {
            SendFloat16(buffer, ref ix, new float16(v.distance));
            SendAngle8(buffer, ref ix, v.direction.horizontal);
            SendAngle8(buffer, ref ix, v.direction.horizontal);
        }

        public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
        {
            float distance = ReceiveFloat16(data, ref ix);
            float horizontal = ReceiveAngle8(data, ref ix);
            float vertical = ReceiveAngle8(data, ref ix);
            Spherical v = new Spherical(distance, horizontal, vertical);
            return v;
        }

        public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s)
        {
            Quat32 q32 = Quat32.FromSwingTwist(s);
            SendQuat32(buffer, ref ix, q32);

        }
        public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
        {
            int qx = (int)(q.x * 127 + 128);
            int qy = (int)(q.y * 127 + 128);
            int qz = (int)(q.z * 127 + 128);
            int qw = (int)(q.w * 255);
            if (q.w < 0)
            {
                qx = -qx;
                qy = -qy;
                qz = -qz;
                qw = -qw;
            }

            buffer[ix++] = (byte)qx;
            buffer[ix++] = (byte)qy;
            buffer[ix++] = (byte)qz;
            buffer[ix++] = (byte)qw;
        }
        public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
        {
            Quat32 q = new Quat32(
                (data[ix++] - 128.0F) / 127.0F,
                (data[ix++] - 128.0F) / 127.0F,
                (data[ix++] - 128.0F) / 127.0F,
                data[ix++] / 255.0F);
            return q;
        }

        public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix)
        {
            Quat32 q32 = ReceiveQuat32(data, ref ix);
            // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
            // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);

            SwingTwist r = SwingTwist.FromQuat32(q32);
            return r;
        }

        public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
        {
            // Normalize angle
            while (angle >= 180)
                angle -= 360;
            while (angle < -180)
                angle += 360;
            buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
        }

        public static float ReceiveAngle8(byte[] data, ref byte ix)
        {
            float value = (data[ix++] * 180) / 128.0F;
            return value;
        }

        public static void SendFloat16(byte[] data, ref byte ix, float f)
        {
            float16 f16 = new float16(f);
            ushort binary = f16.GetBinary();
            data[ix++] = (byte)(binary >> 8);
            data[ix++] = (byte)(binary & 255);
        }
        public static void SendFloat16(byte[] data, ref byte ix, float16 f)
        {
            ushort binary = f.GetBinary();
            data[ix++] = (byte)(binary >> 8);
            data[ix++] = (byte)(binary & 255);
        }

        public static float ReceiveFloat16(byte[] data, ref byte ix)
        {
            ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
            float16 f16 = new float16();
            f16.SetBinary(value);
            float f = f16.toFloat();
            return f;
        }
    }

}