"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""


from io import BytesIO
import struct

class lcmt_panda_status(object):
    """
    The current status of a Franka Panda arm.  All angular
    positions/velocities are expressed in radians and radians/second.
    
    The fields of this message are based on the franka::RobotState message
    found in libfranka, see
    https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html
    Where the names of some fields have been changed to be more "drake-like",
    the original field name is referenced here.
    """

    __slots__ = ["utime", "num_joints", "joint_position", "joint_position_desired", "joint_velocity", "joint_velocity_desired", "joint_acceleration_desired", "joint_torque", "joint_torque_desired", "joint_torque_external", "control_command_success_rate", "robot_mode", "robot_utime", "control_mode"]

    __typenames__ = ["int64_t", "int32_t", "double", "double", "double", "double", "double", "double", "double", "double", "double", "int8_t", "int64_t", "int8_t"]

    __dimensions__ = [None, None, ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], None, None, None, None]

    kOther = 0
    """ enum for robot mode """
    kIdle = 1
    kMove = 2
    kGuiding = 3
    kReflex = 4
    kUserStopped = 5
    kAutomaticErrorRecovery = 6
    CONTROL_MODE_POSITION = 1
    """
    Information about how the driver's control mode is configured, as
    described in
    https://frankaemika.github.io/docs/libfranka.html#realtime-commands and
    https://frankaemika.github.io/libfranka/classfranka_1_1Robot.html
    
    For a driver based on libfranka, at most one of position or velocity
    control would be enabled at any given time.  In other circumstances
    (e.g. simulation) both position and velocity commands may potentially be
    used.
    """
    CONTROL_MODE_VELOCITY = 2
    CONTROL_MODE_TORQUE = 4

    def __init__(self):
        self.utime = 0
        """
        The timestamp in microseconds.
        LCM Type: int64_t
        """

        self.num_joints = 0
        """ LCM Type: int32_t """
        self.joint_position = []
        """
        franka::RobotState.q
        LCM Type: double[num_joints]
        """

        self.joint_position_desired = []
        """
        franka::RobotState.q_d
        LCM Type: double[num_joints]
        """

        self.joint_velocity = []
        """
        franka::RobotState.dq
        LCM Type: double[num_joints]
        """

        self.joint_velocity_desired = []
        """
        franka::RobotState.dq_d
        LCM Type: double[num_joints]
        """

        self.joint_acceleration_desired = []
        """
        franka::RobotState.ddq_d
        LCM Type: double[num_joints]
        """

        self.joint_torque = []
        """
        franka::RobotState.tau_J
        This is correct w.r.t. the conventions outlined in manipulation/README.
        LCM Type: double[num_joints]
        """

        self.joint_torque_desired = []
        """
        franka::RobotState.tau_J_d
        LCM Type: double[num_joints]
        """

        self.joint_torque_external = []
        """
        franka::RobotState.tau_ext_hat_filtered
        This is *negative* w.r.t. the conventions outlined in manipulation/README.
        LCM Type: double[num_joints]
        """

        self.control_command_success_rate = 0.0
        """ LCM Type: double """
        self.robot_mode = 0
        """ LCM Type: int8_t """
        self.robot_utime = 0
        """
        franka::RobotState.time.toMSec() * 1000
        LCM Type: int64_t
        """

        self.control_mode = 0
        """
        Mask of enabled control modes
        LCM Type: int8_t
        """


    def encode(self):
        buf = BytesIO()
        buf.write(lcmt_panda_status._get_packed_fingerprint())
        self._encode_one(buf)
        return buf.getvalue()

    def _encode_one(self, buf):
        buf.write(struct.pack(">qi", self.utime, self.num_joints))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_position[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_position_desired[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_velocity[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_velocity_desired[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_acceleration_desired[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_torque[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_torque_desired[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_torque_external[:self.num_joints]))
        buf.write(struct.pack(">dbqb", self.control_command_success_rate, self.robot_mode, self.robot_utime, self.control_mode))

    @staticmethod
    def decode(data: bytes):
        if hasattr(data, 'read'):
            buf = data
        else:
            buf = BytesIO(data)
        if buf.read(8) != lcmt_panda_status._get_packed_fingerprint():
            raise ValueError("Decode error")
        return lcmt_panda_status._decode_one(buf)

    @staticmethod
    def _decode_one(buf):
        self = lcmt_panda_status()
        self.utime, self.num_joints = struct.unpack(">qi", buf.read(12))
        self.joint_position = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_position_desired = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_velocity = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_velocity_desired = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_acceleration_desired = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_torque = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_torque_desired = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_torque_external = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.control_command_success_rate, self.robot_mode, self.robot_utime, self.control_mode = struct.unpack(">dbqb", buf.read(18))
        return self

    @staticmethod
    def _get_hash_recursive(parents):
        if lcmt_panda_status in parents: return 0
        tmphash = (0xcab8e6fa6c17d73) & 0xffffffffffffffff
        tmphash  = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff
        return tmphash
    _packed_fingerprint = None

    @staticmethod
    def _get_packed_fingerprint():
        if lcmt_panda_status._packed_fingerprint is None:
            lcmt_panda_status._packed_fingerprint = struct.pack(">Q", lcmt_panda_status._get_hash_recursive([]))
        return lcmt_panda_status._packed_fingerprint

    def get_hash(self):
        """Get the LCM hash of the struct"""
        return struct.unpack(">Q", lcmt_panda_status._get_packed_fingerprint())[0]

