import numpy as np
from LinearAlgebra.SwingTwist import SwingTwist

def SendAngle8(buffer, ix_ref, angle):
    # Normalize angle
    while angle >= 180:
        angle -= 360
    while angle < -180:
        angle += 360

    ix = ix_ref[0]
    buffer[ix] = round((angle / 360) * 256).to_bytes(1, 'big')[0]
    ix_ref[0] += 1

def SendFloat16(buffer, ix_ref, value):
    ix = ix_ref[0]
    value16 = np.float16(value)
    binary = value16.view(np.uint16)
    buffer[ix:ix+2] = [
        (binary & 0xFF00) >> 8,
        (binary & 0x00FF)
    ]
    ix_ref[0] += 2

def ReceiveFloat16(buffer, ix_ref) -> float:
    ix = ix_ref[0]
    # if ix < len(buffer) - 1:
    binary = (buffer[ix] << 8) + buffer[ix+1]
    # else:
    #     binary = 0
    value16 = np.uint16(binary).view(np.float16)
    ix_ref[0] += 2
    return float(value16)

def SendSpherical(buffer, ix_ref, vector):
    SendFloat16(buffer, ix_ref, vector.distance)
    SendAngle8(buffer, ix_ref, vector.direction.horizontal)
    SendAngle8(buffer, ix_ref, vector.direction.vertical)

def SendQuat32(buffer, ix_ref, q):
    if isinstance(q, SwingTwist):
        q = q.ToQuaternion()

    ix = ix_ref[0]
    qx = (int)(q.x * 127 + 128)
    qy = (int)(q.y * 127 + 128)
    qz = (int)(q.z * 127 + 128)
    qw = (int)(q.w * 255)
    if q.w < 0:        
        qx = -qx
        qy = -qy
        qz = -qz
        qw = -qw        
    buffer[ix:ix+4] = [
        qx,
        qy,
        qz,
        qw
    ]
    ix_ref[0] += 4