# 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
import time


class JointCommand:
    """
    Joint command message for robotic manipulators.

    Supports variable number of joints (DOF) with float64 values.
    Can be used for position commands or velocity commands.
    Includes timestamp for synchronization.
    """

    msg_name = "sensor_msgs.JointCommand"

    __slots__ = ["num_joints", "positions", "timestamp"]

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

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

    def __init__(
        self, positions: list[float] | None = None, timestamp: float | None = None
    ) -> None:
        """
        Initialize JointCommand.

        Args:
            positions: List of joint values (positions or velocities)
            timestamp: Unix timestamp (seconds since epoch). If None, uses current time.
        """
        if positions is None:
            positions = []

        if timestamp is None:
            timestamp = time.time()

        # LCM Type: double (timestamp)
        self.timestamp = timestamp
        # LCM Type: int32_t
        self.num_joints = len(positions)
        # LCM Type: double[num_joints]
        self.positions = list(positions)

    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(JointCommand._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]
        # Encode timestamp
        buf.write(struct.pack(">d", self.timestamp))

        # Encode num_joints
        buf.write(struct.pack(">i", self.num_joints))

        # Encode positions array
        for i in range(self.num_joints):
            buf.write(struct.pack(">d", self.positions[i]))

    @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 = JointCommand.__new__(JointCommand)

        # Decode timestamp
        self.timestamp = struct.unpack(">d", buf.read(8))[0]

        # Decode num_joints
        self.num_joints = struct.unpack(">i", buf.read(4))[0]

        # Decode positions array
        self.positions = []
        for _i in range(self.num_joints):
            self.positions.append(struct.unpack(">d", buf.read(8))[0])

        return self

    @classmethod
    def _get_hash_recursive(cls, parents):  # type: ignore[no-untyped-def]
        if cls in parents:
            return 0
        # Hash for variable-length double array message
        tmphash = (0x8A3D2E1C5F4B6A9D) & 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", JointCommand._get_packed_fingerprint())[0]  # type: ignore[no-untyped-call]

    def __str__(self) -> str:
        return f"JointCommand(timestamp={self.timestamp:.6f}, num_joints={self.num_joints}, positions={self.positions})"

    def __repr__(self) -> str:
        return f"JointCommand(positions={self.positions}, timestamp={self.timestamp})"
