import numpy as np
from LinearAlgebra.SwingTwist import SwingTwist
from LinearAlgebra.Spherical import Spherical
from LinearAlgebra.Quaternion import Quaternion
from LinearAlgebra.Angle import Angle

def SendAngle8(buffer, ref_ix, angle):
    """! Send an 8-bit angle value """
    # angle = Angle.Normalize(angle)

    ix = ref_ix[0]
    buffer[ix] = int((angle.InDegrees() / 360) * 256).to_bytes(1, 'big', signed=True)[0]
    ref_ix[0] += 1

def ReceiveAngle8(buffer: bytes, ref_ix: list[int]) -> Angle:
    angle: Angle = Angle()
    angle.value = buffer[ref_ix[0]]
    ref_ix[0] += 1
    return angle

def SendFloat16(buffer, ix_ref, value):
    """! Send a 16-bit floating point 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:
    """! Receive a 16-bit floating point value """
    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):
    """! Send a spherical vector 
    """
    SendFloat16(buffer, ix_ref, vector.distance)
    SendAngle8(buffer, ix_ref, vector.direction.horizontal)
    SendAngle8(buffer, ix_ref, vector.direction.vertical)

def ReceiveSpherical(buffer: bytes, ref_ix: list[int]) -> Spherical:
    """! Receive a spherical vector
    """
    distance = ReceiveFloat16(buffer, ref_ix)
    horizontal = ReceiveAngle8(buffer, ref_ix)
    vertical = ReceiveAngle8(buffer, ref_ix)
    v: Spherical = Spherical.Degrees(distance, horizontal, vertical)
    return v

def SendQuat32(buffer, ref_ix, q):
    """! Send a 32-bit quaternion value """
    if isinstance(q, SwingTwist):
        q = q.ToQuaternion()

    ix = ref_ix[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
    ]
    ref_ix[0] += 4

def ReceiveQuaternion(buffer: bytes, ref_ix: list[int]):
    qx: float = (buffer[ref_ix[0]] - 128) / 127
    ref_ix[0] += 1
    qy: float = (buffer[ref_ix[0]] - 128) / 127
    ref_ix[0] += 1
    qz: float = (buffer[ref_ix[0]] - 128) / 127
    ref_ix[0] += 1
    qw: float = buffer[ref_ix[0]] / 255
    ref_ix[0] += 1
    q: Quaternion = Quaternion(qx, qy, qz, qw)
    return q