using Passer.LinearAlgebra;

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.horizontal);
        SendAngle8(buffer, ref ix, v.vertical);
    }

    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(distance, horizontal, vertical);
        return v;
    }

    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(
            (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 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, 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();
        f16.SetBinary(value);
        float f = f16.toFloat();
        return f;
    }
}