import Messages.LowLevelMessages
from Thing import Thing

class PoseMsg():
    id = 0x10
    length = 4

    Pose_Position = 0x01
    Pose_Orientation = 0x02    
    Pose_LinearVelocity = 0x04
    Pose_AngularVelocity = 0x08

    def __init__(self, network_id, thing, force: bool = False):
        self.network_id = network_id
        self.thing: Thing = thing
        self.pose_type = 0
        if thing.position_updated or force:
            self.pose_type |= Thing.Position
            thing.position_updated = False
        if thing.orientation_updated or force:
            self.pose_type |= Thing.Orientation
            thing.orientation_updated = False
        if thing.linear_velocity_updated:
            self.pose_type |= Thing.LinearVelocity
            thing.linear_velocity_updated = False
        if thing.angular_velocity_updated:
            self.pose_type |= Thing.AngularVelocity
            thing.angular_velocity_updated = False

    def Serialize(self, buffer_ref):
        if (self.network_id is None) or (self.thing is None) or (self.pose_type == 0):
            return 0
        
        buffer: bytearray = buffer_ref[0]
        buffer[0:PoseMsg.length] = [
            PoseMsg.id,
            self.network_id,
            self.thing.id,
            self.pose_type
        ]
        ix = [4]
        if self.pose_type & Thing.Position:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
        if self.pose_type & Thing.Orientation:
            Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
        if self.pose_type & Thing.LinearVelocity:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linear_velocity)
        if self.pose_type & Thing.AngularVelocity:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angular_velocity)
        return PoseMsg.length + ix[0]