#define NUNAVUT_ASSERT assert
#include "cyphal_hardware_interface/cyphal_system.hpp"
#include "include/cyphal_hardware_interface/cyphal_system.hpp"

#include <chrono>
#include <cmath>
#include <cstddef>
#include <hardware_interface/hardware_info.hpp>
#include <limits>
#include <memory>
#include <set>
#include <sstream>
#include <string>
#include <vector>

#include <thread>

#include "socketcan.h"

// #include "serial.hpp"

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

// #include "cyphal.hpp"
#include <chrono>
#include <cstdint>
#include <iomanip> // For std::setw, std::setfill, std::hex, etc.
#include <iostream>
#include <sstream> // For std::ostringstream
#include <thread>

#define NUNAVUT_ASSERT assert

extern "C" {
#include "canard.h"
#include <reg/udral/physics/dynamics/rotation/PlanarTs_0_1.h>
#include <reg/udral/service/common/Readiness_0_1.h>
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h>
#include <uavcan/node/Heartbeat_1_0.h>
#include <uavcan/node/port/List_0_1.h>
}

namespace cyphal_hardware_interface {

uint64_t getMonotonicMicroseconds() {
  auto now = std::chrono::steady_clock::now();
  auto micros = std::chrono::duration_cast<std::chrono::microseconds>(
                    now.time_since_epoch())
                    .count();
  return static_cast<uint64_t>(micros);
}

std::string cyphalUdralServo::get_non_read_register_name() const {
  for (const auto &[name, reg] : registers) {
    if (uavcan_register_Value_1_0_is_empty_(&reg)) {
      return name;
    }
  }
  return "";
}

bool cyphalUdralServo::all_registers_read() const {
  return state.register_read && get_non_read_register_name() == "";
}

cyphalSystemHardware::~cyphalSystemHardware() {
  if (can_socket >= 0) {
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Closing socket of interface '%s'", can_if_.c_str());
    // close(serial_port_handle);
  }
}

void cyphalSystemHardware::send(const CanardMicrosecond tx_deadline_usec,
                                const CanardTransferMetadata *const metadata,
                                const size_t payload_size,
                                const void *const payload_data,
                                const CanardMicrosecond now_usec) {
  const struct CanardPayload payload = {.size = payload_size,
                                        .data = payload_data};
  (void)canardTxPush(&canard_tx_queues[0], &canard, tx_deadline_usec, metadata,
                     payload, now_usec, NULL);
}

void cyphalSystemHardware::read_register_list_idx(unsigned char node_id,
                                                  int register_idx) {
  uavcan_register_List_Request_1_0 msg = {0};
  msg.index = register_idx;
  const CanardMicrosecond now_usec = getMonotonicMicroseconds();
  uint8_t serialized
      [uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
  size_t serialized_size = sizeof(serialized);
  const int8_t err = uavcan_register_List_Request_1_0_serialize_(
      &msg, &serialized[0], &serialized_size);
  assert(err >= 0);
  if (err >= 0) {
    const CanardTransferMetadata transfer = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindRequest,
        .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_,
        .remote_node_id = node_id,
        .transfer_id = (CanardTransferID)(next_transfer_id++),
    };
    send(now_usec + 1e9, &transfer, serialized_size, &serialized[0], now_usec);
  }
}

void cyphalSystemHardware::send_readiness(
    enum arming_state arming_state_target) {
  reg_udral_service_common_Readiness_0_1 msg;
  msg.value = arming_state_target == ARMED
                  ? reg_udral_service_common_Readiness_0_1_ENGAGED
                  : reg_udral_service_common_Readiness_0_1_STANDBY;
  const CanardMicrosecond now_usec = getMonotonicMicroseconds();
  uint8_t serialized
      [reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
  size_t serialized_size = sizeof(serialized);
  const int8_t err = reg_udral_service_common_Readiness_0_1_serialize_(
      &msg, &serialized[0], &serialized_size);
  assert(err >= 0);
  if (err >= 0) {
    const CanardTransferMetadata transfer = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindMessage,
        .port_id = readiness_port,
        .remote_node_id = CANARD_NODE_ID_UNSET,
        .transfer_id = (CanardTransferID)(next_transfer_id++),
    };
    send(now_usec + 1e9, &transfer, serialized_size, &serialized[0], now_usec);
  }
}

void cyphalSystemHardware::write_register(cyphalUdralServo &servo,
                                          std::string name,
                                          uavcan_register_Value_1_0 value) {
  if (servo.state.requested_register_name != "" || name == "") {
    return;
  }
  servo.state.requested_register_name = name;
  unsigned char node_id = servo.node_id;
  uavcan_register_Access_Request_1_0 msg;
  memset(&msg, 0, sizeof msg);
  if (name.length() > sizeof msg.name.name.elements - 1) {
    name.resize(sizeof msg.name.name.elements);
  }
  strcpy((char *)msg.name.name.elements, name.c_str());
  msg.name.name.count = name.length();
  msg.value = value;
  const CanardMicrosecond now_usec = getMonotonicMicroseconds();
  uint8_t serialized
      [uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
  size_t serialized_size = sizeof(serialized);
  const int8_t err = uavcan_register_Access_Request_1_0_serialize_(
      &msg, &serialized[0], &serialized_size);
  assert(err >= 0);
  if (err >= 0) {
    const CanardTransferMetadata transfer = {
        .priority = CanardPriorityNominal,
        .transfer_kind = CanardTransferKindRequest,
        .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_,
        .remote_node_id = node_id,
        .transfer_id = (CanardTransferID)(next_transfer_id++),
    };
    send(now_usec + 1e9, &transfer, serialized_size, &serialized[0], now_usec);
  }
}

void cyphalSystemHardware::read_register(cyphalUdralServo &servo,
                                         std::string name) {
  uavcan_register_Value_1_0 value;
  uavcan_register_Value_1_0_select_empty_(&value);
  write_register(servo, name, value);
}

/**
 * @brief Static method to call read_thread via std::thread
 * @param context pointer to this
 */
void cyphalSystemHardware::read_thread_static(void *context) {
  static_cast<cyphalSystemHardware *>(context)->read_thread();
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
    const hardware_interface::HardwareComponentInterfaceParams &params) {
  if (hardware_interface::SystemInterface::on_init(params) !=
      hardware_interface::CallbackReturn::SUCCESS) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  can_if_ = default_if_;

  if (info_.hardware_parameters.count("can_if") > 0) {
    can_if_ = info_.hardware_parameters.at("can_if");
  } else {
    RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"),
                "No can interface specified in urdf, using default value");
  }
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "If set as %s",
              can_if_.c_str());

  std::vector<hardware_interface::ComponentInfo> components;
  components.insert(components.begin(), info_.joints.begin(),
                    info_.joints.end());
  components.insert(components.begin(), info_.sensors.begin(),
                    info_.sensors.end());
  components.insert(components.begin(), info_.gpios.begin(), info_.gpios.end());

  for (const hardware_interface::ComponentInfo &joint : components) {
    if (joint.parameters.contains("node_id")) {
      int node_id = -1;
      try {
        node_id = std::stoi(joint.parameters.at("node_id"));
      } catch (std::exception &e) {
      }
      if (node_id < 0 || node_id >= 0xFF) {
        RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"),
                     "No node_id '%s' invalid!",
                     joint.parameters.at("node_id").c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      servos[node_id].node_id = node_id;
      servos[node_id].name = joint.name;
    } else {
      RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"),
                   "No node_id configured for servo!");
      return hardware_interface::CallbackReturn::ERROR;
    }
    // for (const auto &i : joint.command_interfaces) {
    //   RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "%s: %s: %d",
    //               joint.name.c_str(), i.name.c_str(),
    //               (int)i.parameters.size());
    // }
    // for (const auto &i : joint.state_interfaces) {
    //   RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "%s: %s: %d",
    //               joint.name.c_str(), i.name.c_str(),
    //               (int)i.parameters.size());
    // }
  }

  canard_memory.deallocate = canardDeallocate;
  canard_memory.allocate = canardAllocate;

  // The libcanard instance requires the allocator for managing protocol states.
  canard = canardInit(canard_memory);

  canard_tx_queues[0] =
      canardTxInit(100, CANARD_MTU_CAN_CLASSIC, canard_memory);
  canard.node_id = 100;

  static struct CanardRxSubscription rx;
  int8_t res = //
      canardRxSubscribe(&canard, CanardTransferKindMessage,
                        uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
                        uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
                        CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
  if (res < 0) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  static struct CanardRxSubscription rx_register_list_response_;
  res = //
      canardRxSubscribe(&canard, CanardTransferKindResponse,
                        uavcan_register_List_1_0_FIXED_PORT_ID_,
                        uavcan_register_List_Response_1_0_EXTENT_BYTES_,
                        CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
                        &rx_register_list_response_);
  if (res < 0) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  static struct CanardRxSubscription rx_register_access_Response_;
  res = //
      canardRxSubscribe(&canard, CanardTransferKindResponse,
                        uavcan_register_Access_1_0_FIXED_PORT_ID_,
                        uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
                        CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
                        &rx_register_access_Response_);
  if (res < 0) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  static struct CanardRxSubscription rx_reg_udral_physics_dynamics_;
  res = //
      canardRxSubscribe(
          &canard, CanardTransferKindMessage, dynamics_state_port,
          reg_udral_physics_dynamics_rotation_PlanarTs_0_1_EXTENT_BYTES_,
          CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
          &rx_reg_udral_physics_dynamics_);
  if (res < 0) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  return hardware_interface::CallbackReturn::SUCCESS;
} // namespace cyphal_hardware_interface

hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
    const rclcpp_lifecycle::State & /*previous_state*/) {

  can_socket = socketcanOpen(can_if_.c_str(), 8); /* MTU == 8 -> classic CAN */
  if (can_socket < 0) {
    RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                 "Could not open socket for can interface '%s'",
                 can_if_.c_str());
    return hardware_interface::CallbackReturn::FAILURE;
  }

  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Opened socketcan socket for '%s'", can_if_.c_str());

  read_thread_handle = std::thread([this]() { read_thread_static(this); });

  /** wait_for_nodes **/
  using namespace std::chrono_literals;
  auto start_time = std::chrono::steady_clock::now();
  while (true) {
    bool ready = true;
    for (const auto &[id, s] : servos) {
      std::unique_lock lock{s.mtx};
      if (!s.state.alive || !s.all_registers_read()) {
        RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                    "Waiting for '%d'", id);
        ready = false;
        break;
      }
    }
    if (ready) {
      break;
    }
    if (std::chrono::steady_clock::now() > start_time + 10s) {
      RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"),
                   "Timed out while waiting for servos");
      return hardware_interface::CallbackReturn::ERROR;
    }
    std::this_thread::sleep_for(200ms);
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

void cyphalSystemHardware::processReceivedTransfer(
    const struct CanardRxTransfer *const transfer,
    const CanardMicrosecond now_usec) {
  (void)now_usec;
  const int node_id = transfer->metadata.remote_node_id;
  if (transfer->metadata.transfer_kind == CanardTransferKindResponse) {
    if (!servos.contains(node_id)) {
      return;
    }
    std::unique_lock lock{servos[node_id].mtx};
    if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {

      uavcan_register_List_Response_1_0 resp;
      memset(&resp, 0, sizeof resp);
      size_t size = transfer->payload.size;

      if (uavcan_register_List_Response_1_0_deserialize_(
              &resp, (uint8_t *)transfer->payload.data, &size) >= 0) {
        if (resp.name.name.count == 0) {

          RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                      "Register list read from '%d'", node_id);
          servos[node_id].state.register_read = true;
          read_register(servos[node_id],
                        servos[node_id].get_non_read_register_name());
        } else {
          char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0};
          memcpy(&name[0], resp.name.name.elements, resp.name.name.count);
          name[resp.name.name.count] = '\0';
          RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                      "reg_name: %s", name);
          uavcan_register_Value_1_0 value;
          uavcan_register_Value_1_0_select_empty_(&value);
          servos[node_id].registers[name] = value;
          read_register_list_idx(node_id, servos[node_id].registers.size());
        }
      }
    } else if (transfer->metadata.port_id ==
               uavcan_register_Access_1_0_FIXED_PORT_ID_) {
      uavcan_register_Access_Response_1_0 resp;
      memset(&resp, 0, sizeof resp);
      size_t size = transfer->payload.size;
      if (uavcan_register_Access_Response_1_0_deserialize_(
              &resp, (uint8_t *)transfer->payload.data, &size) >= 0) {
        if (servos[node_id].state.requested_register_name.length() == 0) {
          RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"),
                       "reg_value recieved but non requested!");
          return;
        }
        servos[node_id]
            .registers[servos[node_id].state.requested_register_name] =
            resp.value;
        RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                    "reg_value '%s' recieved",
                    servos[node_id].state.requested_register_name.c_str());
        servos[node_id].state.requested_register_name = "";
        if (servos[node_id].state.requested_register_name == "") {
          auto non_read_register = servos[node_id].get_non_read_register_name();
          read_register(servos[node_id], non_read_register);
        }
      }
    }
  } else if (transfer->metadata.transfer_kind == CanardTransferKindMessage) {

    if (transfer->metadata.port_id ==
        uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_) {

      RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                  "heartbeat of '%d' recieved", node_id);
      if (!servos.contains(node_id)) {
        return;
      }
      std::unique_lock lock{servos[node_id].mtx};
      uavcan_node_Heartbeat_1_0 msg;
      size_t size = transfer->payload.size;
      if (uavcan_node_Heartbeat_1_0_deserialize_(
              &msg, (uint8_t *)transfer->payload.data, &size) >= 0) {
        if (!servos[node_id].state.alive) {
          RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                      "Starting to read register list of '%d'", node_id);
          read_register_list_idx(node_id, 0);
        }
        servos[node_id].state.alive = true;
      }
    } else if (transfer->metadata.port_id == dynamics_state_port) {

      if (!servos.contains(node_id)) {
        return;
      }

      std::unique_lock lock{servos[node_id].mtx};
      reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg;
      size_t size = transfer->payload.size;
      if (reg_udral_physics_dynamics_rotation_PlanarTs_0_1_deserialize_(
              &msg, (uint8_t *)transfer->payload.data, &size) >= 0) {
        servos[node_id].state.kinematics = msg.value.kinematics;
      }
    }
  }
}

void cyphalSystemHardware::write_can() {
  const CanardMicrosecond now_usec = getMonotonicMicroseconds();
  struct CanardTxQueue *const que = &canard_tx_queues[0];
  struct CanardTxQueueItem *tqi =
      canardTxPeek(que); // Find the highest-priority frame.
  while (tqi != NULL) {
    // Attempt transmission only if the frame is not yet timed out while waiting
    // in the TX queue. Otherwise just drop it and move on to the next one.
    if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) {
      const struct CanardFrame canard_frame = {
          .extended_can_id = tqi->frame.extended_can_id,
          .payload = {.size = tqi->frame.payload.size,
                      .data = tqi->frame.payload.data}};
      // const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0); //
      // Non-blocking write attempt.
      const int16_t result = socketcanPush(can_socket, &canard_frame,
                                           0); // Non-blocking write attempt.
      if (result == 0) {
        break; // The queue is full, we will try again on the next iteration.
      }
      if (result < 0) {
        return; // SocketCAN interface failure (link down?)
      }
    }

    struct CanardTxQueueItem *const mut_tqi = canardTxPop(que, tqi);
    canardTxFree(que, &canard, mut_tqi);

    tqi = canardTxPeek(que);
  }
}

void cyphalSystemHardware::read_thread() {
  while (true) {

    struct CanardFrame frame;
    CanardMicrosecond out_timestamp_usec;
    CanardMicrosecond timeout_usec = 0;
    bool loopback = false;

    uint8_t payload_buffer[CANARD_MTU_CAN_CLASSIC];
    int ret = socketcanPop(can_socket, &frame, &out_timestamp_usec,
                           sizeof(payload_buffer), payload_buffer, timeout_usec,
                           &loopback);

    if (loopback || ret == 0) {
      continue;
    }

    if (ret < 0) {
      RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                   "socketcanPop error (ret=%d)", ret);
      return;
    }
    // RCLCPP_INFO(
    //     rclcpp::get_logger("cyphalSystemHardware"),
    //     "ok %d %dms %d: %02X %02X %02X %02X %02X %02X %02X %02X", ret,
    //     (int)out_timestamp_usec / 1000, (int)frame.payload.size,
    //     frame.payload.size <= 0 ? 0 : (int)((uint8_t
    //     *)frame.payload.data)[0], frame.payload.size <= 1 ? 0 :
    //     (int)((uint8_t *)frame.payload.data)[1], frame.payload.size <= 2 ? 0
    //     : (int)((uint8_t *)frame.payload.data)[2], frame.payload.size <= 3 ?
    //     0 : (int)((uint8_t *)frame.payload.data)[3], frame.payload.size <= 4
    //     ? 0 : (int)((uint8_t *)frame.payload.data)[4], frame.payload.size <=
    //     5 ? 0 : (int)((uint8_t *)frame.payload.data)[5], frame.payload.size
    //     <= 6 ? 0 : (int)((uint8_t *)frame.payload.data)[6],
    //     frame.payload.size <= 7 ? 0 : (int)((uint8_t
    //     *)frame.payload.data)[7]);

    // The SocketCAN adapter uses the wall clock for timestamping, but we need
    // monotonic. Wall clock can only be used for time synchronization.
    const CanardMicrosecond timestamp_usec = getMonotonicMicroseconds();
    struct CanardRxTransfer transfer;
    memset(&transfer, 0, sizeof transfer);
    const int ifidx = 0; /* interface id */
    const int8_t canard_result = canardRxAccept(&canard, out_timestamp_usec,
                                                &frame, ifidx, &transfer, NULL);
    if (canard_result > 0) {
      processReceivedTransfer(&transfer, timestamp_usec);
      canard.memory.deallocate(canard.memory.user_reference,
                               transfer.payload.allocated_size,
                               transfer.payload.data);
    } else if (canard_result == -CANARD_ERROR_OUT_OF_MEMORY) {
      (void)0; // The frame did not complete a transfer so there is nothing to
               // do.
      // OOM should never occur if the heap is sized correctly. You can track
      // OOM errors via heap API.
      RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), "OOM!");
      return;
    } else if (canard_result == 0) {
      (void)0; // The frame did not complete a transfer so there is nothing to
               // do.
      using namespace std::chrono_literals;
      std::this_thread::sleep_for(10us);
      continue;
    } else {
      assert(false); // No other error can possibly occur at runtime.
      // return hardware_interface::return_type::ERROR;
      return;
    }

    write_can();
  }
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_activate(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // your production code
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Activating");

  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Successfully activated!");

  int i = 0;
  for (auto &[id, s] : servos) {
    std::unique_lock lock{s.mtx};
    s.arm_target = ARMED;

    uavcan_register_Value_1_0 value;
    uavcan_register_Value_1_0_select_natural16_(&value);
    value.natural16.value.elements[0] = readiness_port;
    value.natural16.value.count = 1;
    write_register(s, "uavcan.sub.servo.readiness.id", value);

    const uint16_t dynamics_port = dynamics_state_port;
    uavcan_register_Value_1_0_select_natural16_(&value);
    value.natural16.value.elements[0] = dynamics_port;
    value.natural16.value.count = 1;
    write_register(s, "uavcan.pub.servo.dynamics.id", value);

    i += 1;
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Deactivating");

  for (auto &[id, s] : servos) {
    std::unique_lock lock{s.mtx};
    s.arm_target = DISARMED;
  }
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::return_type
cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
                           const rclcpp::Duration & /*period*/) {

  for (const auto &[name, descr] : joint_state_interfaces_) {
    if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) {
      for (auto &[id, s] : servos) {
        std::unique_lock lock{s.mtx};
        if (s.name == descr.get_prefix_name()) {
          set_state<double>(name, (double)s.state.kinematics.angular_position.radian);
          break;
        }
      }
    } else if (descr.get_interface_name() ==
               hardware_interface::HW_IF_VELOCITY) {
      for (auto &[id, s] : servos) {
        std::unique_lock lock{s.mtx};
        if (s.name == descr.get_prefix_name()) {
          set_state<double>(name,
                    (double)s.state.kinematics.angular_velocity.radian_per_second);
          break;
        }
      }
    } else if (descr.get_interface_name() ==
               hardware_interface::HW_IF_ACCELERATION) {
      for (auto &[id, s] : servos) {
        std::unique_lock lock{s.mtx};
        if (s.name == descr.get_prefix_name()) {
          set_state<double>(
              name,
              (double)s.state.kinematics.angular_acceleration.radian_per_second_per_second);
          break;
        }
      }
    }
  }

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type
cyphal_hardware_interface ::cyphalSystemHardware::write(
    const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {

  for (auto &[id, s] : servos) {
    std::unique_lock lock{s.mtx};
  }
  send_readiness(ARMED);
  return hardware_interface::return_type::OK;
}

} // namespace cyphal_hardware_interface

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware,
                       hardware_interface::SystemInterface)