# Copyright 2025-2026 Dimensional Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

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

from io import BytesIO
import struct


class RobotState:
    msg_name = "sensor_msgs.RobotState"

    __slots__ = [
        "cmdnum",
        "error_code",
        "joints",
        "mode",
        "mt_able",
        "mt_brake",
        "state",
        "tcp_offset",
        "tcp_pose",
        "warn_code",
    ]

    __typenames__ = [
        "int32_t",
        "int32_t",
        "int32_t",
        "int32_t",
        "int32_t",
        "int32_t",
        "int32_t",
        "float",
        "float",
        "float",
    ]

    __dimensions__ = [None, None, None, None, None, None, None, None, None, None]

    def __init__(  # type: ignore[no-untyped-def]
        self,
        state: int = 0,
        mode: int = 0,
        error_code: int = 0,
        warn_code: int = 0,
        cmdnum: int = 0,
        mt_brake: int = 0,
        mt_able: int = 0,
        tcp_pose=None,
        tcp_offset=None,
        joints=None,
    ) -> None:
        # LCM Type: int32_t
        self.state = state
        # LCM Type: int32_t
        self.mode = mode
        # LCM Type: int32_t
        self.error_code = error_code
        # LCM Type: int32_t
        self.warn_code = warn_code
        # LCM Type: int32_t
        self.cmdnum = cmdnum
        # LCM Type: int32_t
        self.mt_brake = mt_brake
        # LCM Type: int32_t
        self.mt_able = mt_able
        # LCM Type: float[] - TCP pose [x, y, z, roll, pitch, yaw]
        self.tcp_pose = tcp_pose if tcp_pose is not None else []
        # LCM Type: float[] - TCP offset [x, y, z, roll, pitch, yaw]
        self.tcp_offset = tcp_offset if tcp_offset is not None else []
        # LCM Type: float[] - Joint positions (variable length based on robot DOF)
        self.joints = joints if joints is not None else []

    def lcm_encode(self):  # type: ignore[no-untyped-def]
        """Encode for LCM transport (dimos uses lcm_encode method name)."""
        return self.encode()  # type: ignore[no-untyped-call]

    def encode(self):  # type: ignore[no-untyped-def]
        buf = BytesIO()
        buf.write(RobotState._get_packed_fingerprint())  # type: ignore[no-untyped-call]
        self._encode_one(buf)
        return buf.getvalue()

    def _encode_one(self, buf) -> None:  # type: ignore[no-untyped-def]
        buf.write(
            struct.pack(
                ">iiiiiii",
                self.state,
                self.mode,
                self.error_code,
                self.warn_code,
                self.cmdnum,
                self.mt_brake,
                self.mt_able,
            )
        )
        # Encode tcp_pose array
        buf.write(struct.pack(">i", len(self.tcp_pose)))
        for val in self.tcp_pose:
            buf.write(struct.pack(">f", val))
        # Encode tcp_offset array
        buf.write(struct.pack(">i", len(self.tcp_offset)))
        for val in self.tcp_offset:
            buf.write(struct.pack(">f", val))
        # Encode joints array
        buf.write(struct.pack(">i", len(self.joints)))
        for val in self.joints:
            buf.write(struct.pack(">f", val))

    @classmethod
    def lcm_decode(cls, data: bytes):  # type: ignore[no-untyped-def]
        """Decode from LCM transport (dimos uses lcm_decode method name)."""
        return cls.decode(data)

    @classmethod
    def decode(cls, data: bytes):  # type: ignore[no-untyped-def]
        if hasattr(data, "read"):
            buf = data
        else:
            buf = BytesIO(data)  # type: ignore[assignment]
        if buf.read(8) != cls._get_packed_fingerprint():  # type: ignore[no-untyped-call]
            raise ValueError("Decode error")
        return cls._decode_one(buf)  # type: ignore[no-untyped-call]

    @classmethod
    def _decode_one(cls, buf):  # type: ignore[no-untyped-def]
        self = RobotState()
        (
            self.state,
            self.mode,
            self.error_code,
            self.warn_code,
            self.cmdnum,
            self.mt_brake,
            self.mt_able,
        ) = struct.unpack(">iiiiiii", buf.read(28))
        # Decode tcp_pose array
        tcp_pose_len = struct.unpack(">i", buf.read(4))[0]
        self.tcp_pose = []
        for _ in range(tcp_pose_len):
            self.tcp_pose.append(struct.unpack(">f", buf.read(4))[0])
        # Decode tcp_offset array
        tcp_offset_len = struct.unpack(">i", buf.read(4))[0]
        self.tcp_offset = []
        for _ in range(tcp_offset_len):
            self.tcp_offset.append(struct.unpack(">f", buf.read(4))[0])
        # Decode joints array
        joints_len = struct.unpack(">i", buf.read(4))[0]
        self.joints = []
        for _ in range(joints_len):
            self.joints.append(struct.unpack(">f", buf.read(4))[0])
        return self

    @classmethod
    def _get_hash_recursive(cls, parents):  # type: ignore[no-untyped-def]
        if cls in parents:
            return 0
        # Updated hash to reflect new fields: tcp_pose, tcp_offset, joints
        tmphash = (0x8C3B9A1FE7D24E6A) & 0xFFFFFFFFFFFFFFFF
        tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF
        return tmphash

    _packed_fingerprint = None

    @classmethod
    def _get_packed_fingerprint(cls):  # type: ignore[no-untyped-def]
        if cls._packed_fingerprint is None:
            cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([]))  # type: ignore[no-untyped-call]
        return cls._packed_fingerprint

    def get_hash(self):  # type: ignore[no-untyped-def]
        """Get the LCM hash of the struct"""
        return struct.unpack(">Q", RobotState._get_packed_fingerprint())[0]  # type: ignore[no-untyped-call]
