import Messages.LowLevelMessages
#from Thing import Thing

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

    Position = 0x01
    Orientation = 0x02    
    LinearVelocity = 0x04
    AngularVelocity = 0x08

    def __init__(self, arg1, thing, force: bool = False):
        if isinstance(arg1, bytes):
            self.thing_id = arg1[2]
            self.data = arg1[3:]
        else:
            self.network_id = arg1
            self.thing = thing
            self.network_id = 0

            self.pose_type = 0
            if thing.position_updated or force:
                self.pose_type |= PoseMsg.Position
                thing.position_updated = False
            if thing.orientation_updated or force:
                self.pose_type |= PoseMsg.Orientation
                thing.orientation_updated = False
            if thing.linear_velocity_updated:
                self.pose_type |= PoseMsg.LinearVelocity
                thing.linear_velocity_updated = False
            if thing.angular_velocity_updated:
                self.pose_type |= PoseMsg.AngularVelocity
                thing.angular_velocity_updated = False

    def Serialize(self, buffer_ref):
        if self.thing is None or self.pose_type == 0:
            return 0
        
        print(f'Send PoseMsg [{self.network_id}/{self.thing.id}] {self.pose_type}')

        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 & PoseMsg.Position:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.position)
        if self.pose_type & PoseMsg.Orientation:
            Messages.LowLevelMessages.SendQuat32(buffer, ix, self.thing.orientation)
        if self.pose_type & PoseMsg.LinearVelocity:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.linear_velocity)
        if self.pose_type & PoseMsg.AngularVelocity:
            Messages.LowLevelMessages.SendSpherical(buffer, ix, self.thing.angular_velocity)
        return PoseMsg.length + ix[0]