// Copyright 2020 PAL Robotics S.L.
//
// 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.

#ifndef TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
#define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_

#include <fmt/compile.h>

#include <cassert>
#include <string>
#include <vector>

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "transmission_interface/accessor.hpp"
#include "transmission_interface/exception.hpp"
#include "transmission_interface/transmission.hpp"

namespace transmission_interface
{
/// Implementation of a differential transmission.
/**
 *
 * This transmission relates <b>two actuators</b> and <b>two joints</b> through a differential
 * mechanism, as illustrated below.
 *
 * \image html differential_transmission.png
 *
 * <CENTER>
 * <table>
 * <tr><th></th><th><CENTER>Effort</CENTER></th><th><CENTER>Velocity</CENTER></th><th><CENTER>Position</CENTER></th></tr>
 * <tr><td>
 * <b> Actuator to joint </b>
 * </td>
 * <td>
 * \f{eqnarray*}{
 * \tau_{j_1} & = & n_{j_1} ( n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2} ) \\[2.5em]
 * \tau_{j_2} & = & n_{j_2} ( n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2} )
 * \f}
 * </td>
 * <td>
 * \f{eqnarray*}{
 * \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} / n_{a_1} + \dot{x}_{a_2} / n_{a_2} }{2 n_{j_1}} \\[1em]
 * \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_1} / n_{a_1} - \dot{x}_{a_2} / n_{a_2} }{2 n_{j_2}}
 * \f}
 * </td>
 * <td>
 * \f{eqnarray*}{
 * x_{j_1} & = & \frac{ x_{a_1} / n_{a_1} + x_{a_2} / n_{a_2} }{2 n_{j_1}}  + x_{off_1} \\[1em]
 * x_{j_2} & = & \frac{ x_{a_1} / n_{a_1} - x_{a_2} / n_{a_2} }{2 n_{j_1}}  + x_{off_2}
 * \f}
 * </td>
 * </tr>
 * <tr><td>
 * <b> Joint to actuator </b>
 * </td>
 * <td>
 * \f{eqnarray*}{
 * \tau_{a_1} & = & \frac{ \tau_{j_1} / n_{j_1} + \tau_{j_2} / n_{j_2} }{2 n_{a_1}} \\[1em]
 * \tau_{a_2} & = & \frac{ \tau_{j_1} / n_{j_1} - \tau_{j_2} / n_{j_2} }{2 n_{a_1}}
 * \f}
 * </td>
 * <td>
 * \f{eqnarray*}{
 * \dot{x}_{a_1} & = & n_{a_1} ( n_{j_1} \dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2} ) \\[2.5em]
 * \dot{x}_{a_2} & = & n_{a_2} ( n_{j_1} \dot{x}_{j_1} - n_{j_2} \dot{x}_{j_2} )
 * \f}
 * </td>
 * <td>
 * \f{eqnarray*}{
 * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})
 * \right] \\[2.5em] x_{a_2} & = & n_{a_2} \left[ n_{j_1} (x_{j_1} - x_{off_1}) - n_{j_2} (x_{j_2} -
 * x_{off_2}) \right] \f}
 * </td></tr></table>
 * </CENTER>
 *
 * where:
 * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables,
 * respectively.
 * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space
 * variables, respectively.
 * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position
 * coordinates (cf. SimpleTransmission class documentation for a more detailed description of this
 * variable).
 * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator
 * and joint sides (depicted as timing belts in the figure). A transmission ratio can take any real
 * value \e except zero. In particular:
 *     - If its absolute value is greater than one, it's a velocity reducer / effort amplifier,
 * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer.
 *     - Negative values represent a direction flip, ie. input and output move in opposite
 * directions.
 *     - <b>Important:</b> Use transmission ratio signs to match this class' convention of positive
 * actuator/joint directions with a given mechanical design, as they will in general not match.
 *
 * \note This implementation currently assumes a specific layout for location of the actuators and
 * joint axes which is common in robotic mechanisms. Please file an enhancement ticket if your use
 * case does not adhere to this layout.
 *
 * \ingroup transmission_types
 */

constexpr auto HW_IF_ABSOLUTE_POSITION = "absolute_position";

class DifferentialTransmission : public Transmission
{
public:
  /**
   * \param[in] actuator_reduction Reduction ratio of actuators.
   * \param[in] joint_reduction    Reduction ratio of joints.
   * \param[in] joint_offset       Joint position offset used in the position mappings.
   * \pre Nonzero actuator and joint reduction values.
   */
  DifferentialTransmission(
    const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
    const std::vector<double> & joint_offset = {0.0, 0.0});

  /**
   * \param[in] joint_handles     Handles of joint values.
   * \param[in] actuator_handles  Handles of actuator values.
   * \pre Handles are valid and matching in size
   */
  void configure(
    const std::vector<JointHandle> & joint_handles,
    const std::vector<ActuatorHandle> & actuator_handles) override;

  /// Transform variables from actuator to joint space.
  /**
   * \pre Actuator and joint vectors must have size 2 and point to valid data.
   *  To call this method it is not required that all other data vectors contain valid data, and can
   * even remain empty.
   */
  void actuator_to_joint() override;

  /// Transform variables from joint to actuator space.
  /**
   * \pre Actuator and joint vectors must have size 2 and point to valid data.
   *  To call this method it is not required that all other data vectors contain valid data, and can
   * even remain empty.
   */
  void joint_to_actuator() override;

  std::size_t num_actuators() const override { return 2; }
  std::size_t num_joints() const override { return 2; }

  const std::vector<double> & get_actuator_reduction() const { return actuator_reduction_; }
  const std::vector<double> & get_joint_reduction() const { return joint_reduction_; }
  const std::vector<double> & get_joint_offset() const { return joint_offset_; }

  /// Get human-friendly report of handles
  std::string get_handles_info() const;

  std::vector<std::string> get_supported_actuator_interfaces() const override
  {
    return {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
            hardware_interface::HW_IF_EFFORT,   hardware_interface::HW_IF_TORQUE,
            hardware_interface::HW_IF_FORCE,    HW_IF_ABSOLUTE_POSITION};
  }

  std::vector<std::string> get_supported_joint_interfaces() const override
  {
    return {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
            hardware_interface::HW_IF_EFFORT,   hardware_interface::HW_IF_TORQUE,
            hardware_interface::HW_IF_FORCE,    HW_IF_ABSOLUTE_POSITION};
  }

protected:
  std::vector<double> actuator_reduction_;
  std::vector<double> joint_reduction_;
  std::vector<double> joint_offset_;

  std::vector<JointHandle> joint_position_;
  std::vector<JointHandle> joint_velocity_;
  std::vector<JointHandle> joint_effort_;
  std::vector<JointHandle> joint_torque_;
  std::vector<JointHandle> joint_force_;
  std::vector<JointHandle> joint_absolute_position_;

  std::vector<ActuatorHandle> actuator_position_;
  std::vector<ActuatorHandle> actuator_velocity_;
  std::vector<ActuatorHandle> actuator_effort_;
  std::vector<ActuatorHandle> actuator_torque_;
  std::vector<ActuatorHandle> actuator_force_;
  std::vector<ActuatorHandle> actuator_absolute_position_;
};

inline DifferentialTransmission::DifferentialTransmission(
  const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
  const std::vector<double> & joint_offset)
: actuator_reduction_(actuator_reduction),
  joint_reduction_(joint_reduction),
  joint_offset_(joint_offset)
{
  if (
    num_actuators() != actuator_reduction_.size() || num_joints() != joint_reduction_.size() ||
    num_joints() != joint_offset_.size())
  {
    throw Exception("Reduction and offset vectors must have size 2.");
  }

  if (
    0.0 == actuator_reduction_[0] || 0.0 == actuator_reduction_[1] || 0.0 == joint_reduction_[0] ||
    0.0 == joint_reduction_[1])
  {
    throw Exception("Transmission reduction ratios cannot be zero.");
  }
}

void DifferentialTransmission::configure(
  const std::vector<JointHandle> & joint_handles,
  const std::vector<ActuatorHandle> & actuator_handles)
{
  if (joint_handles.empty())
  {
    throw Exception("No joint handles were passed in");
  }

  if (actuator_handles.empty())
  {
    throw Exception("No actuator handles were passed in");
  }

  const auto joint_names = get_names(joint_handles);
  if (joint_names.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("There should be exactly two unique joint names but was given '{}'."),
        to_string(joint_names)));
  }
  const auto actuator_names = get_names(actuator_handles);
  if (actuator_names.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("There should be exactly two unique actuator names but was given '{}'."),
        to_string(actuator_names)));
  }

  joint_position_ =
    get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_POSITION);
  joint_velocity_ =
    get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY);
  joint_effort_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_EFFORT);
  joint_torque_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_TORQUE);
  joint_force_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_FORCE);
  joint_absolute_position_ =
    get_ordered_handles(joint_handles, joint_names, HW_IF_ABSOLUTE_POSITION);

  if (!joint_position_.empty() && joint_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint position handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_velocity_.empty() && joint_velocity_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint velocity handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_effort_.empty() && joint_effort_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint effort handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_torque_.empty() && joint_torque_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint torque handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_force_.empty() && joint_force_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint force handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_absolute_position_.empty() && joint_absolute_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE(
          "Not enough valid or required joint absolute position handles were present. \n{}"),
        get_handles_info()));
  }

  actuator_position_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_POSITION);
  actuator_velocity_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_VELOCITY);
  actuator_effort_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT);
  actuator_torque_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_TORQUE);
  actuator_force_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_FORCE);
  actuator_absolute_position_ =
    get_ordered_handles(actuator_handles, actuator_names, HW_IF_ABSOLUTE_POSITION);

  if (!actuator_position_.empty() && actuator_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator position handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_velocity_.empty() && actuator_velocity_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator velocity handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_effort_.empty() && actuator_effort_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator effort handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_torque_.empty() && actuator_torque_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator torque handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_force_.empty() && actuator_force_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator force handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_absolute_position_.empty() && actuator_absolute_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE(
          "Not enough valid or required actuator absolute position handles were "
          "present. \n{}"),
        get_handles_info()));
  }

  if (
    !joint_position_.empty() && !actuator_position_.empty() &&
    joint_position_.size() != actuator_position_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on position interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_velocity_.empty() && !actuator_velocity_.empty() &&
    joint_velocity_.size() != actuator_velocity_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on velocity interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_effort_.empty() && !actuator_effort_.empty() &&
    joint_effort_.size() != actuator_effort_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on effort interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_torque_.empty() && !actuator_torque_.empty() &&
    joint_torque_.size() != actuator_torque_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on torque interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_force_.empty() && !actuator_force_.empty() &&
    joint_force_.size() != actuator_force_.size())
  {
    throw Exception(
      fmt::format(FMT_COMPILE("Pair-wise mismatch on force interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_absolute_position_.empty() && !actuator_absolute_position_.empty() &&
    joint_absolute_position_.size() != actuator_absolute_position_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on absolute position interfaces. \n{}"),
        get_handles_info()));
  }

  // Check at least one pair-wise interface is available
  if (!((!joint_position_.empty() && !actuator_position_.empty() &&
         joint_position_.size() == actuator_position_.size()) ||
        (!joint_velocity_.empty() && !actuator_velocity_.empty() &&
         joint_velocity_.size() == actuator_velocity_.size()) ||
        (!joint_effort_.empty() && !actuator_effort_.empty() &&
         joint_effort_.size() == actuator_effort_.size()) ||
        (!joint_torque_.empty() && !actuator_torque_.empty() &&
         joint_torque_.size() == actuator_torque_.size()) ||
        (!joint_force_.empty() && !actuator_force_.empty() &&
         joint_force_.size() == actuator_force_.size()) ||
        (!joint_absolute_position_.empty() && !actuator_absolute_position_.empty() &&
         joint_absolute_position_.size() == actuator_absolute_position_.size())))
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("No pair-wise interface available between joints and actuators. \n{}"),
        get_handles_info()));
  }
}

inline void DifferentialTransmission::actuator_to_joint()
{
  const auto & ar = actuator_reduction_;
  const auto & jr = joint_reduction_;

  auto & act_pos = actuator_position_;
  auto & joint_pos = joint_position_;
  if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints())
  {
    assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]);

    joint_pos[0].set_value(
      (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) +
      joint_offset_[0]);
    joint_pos[1].set_value(
      (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) +
      joint_offset_[1]);
  }

  auto & act_vel = actuator_velocity_;
  auto & joint_vel = joint_velocity_;
  if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints())
  {
    assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]);

    joint_vel[0].set_value(
      (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0]));
    joint_vel[1].set_value(
      (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / (2.0 * jr[1]));
  }

  auto & act_eff = actuator_effort_;
  auto & joint_eff = joint_effort_;
  if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints())
  {
    assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]);

    joint_eff[0].set_value(
      jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1]));
    joint_eff[1].set_value(
      jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1]));
  }

  auto & act_tor = actuator_torque_;
  auto & joint_tor = joint_torque_;
  if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
  {
    assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);

    joint_tor[0].set_value(
      jr[0] * (act_tor[0].get_value() * ar[0] + act_tor[1].get_value() * ar[1]));
    joint_tor[1].set_value(
      jr[1] * (act_tor[0].get_value() * ar[0] - act_tor[1].get_value() * ar[1]));
  }

  auto & act_for = actuator_force_;
  auto & joint_for = joint_force_;
  if (act_for.size() == num_actuators() && joint_for.size() == num_joints())
  {
    assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]);

    joint_for[0].set_value(
      jr[0] * (act_for[0].get_value() * ar[0] + act_for[1].get_value() * ar[1]));
    joint_for[1].set_value(
      jr[1] * (act_for[0].get_value() * ar[0] - act_for[1].get_value() * ar[1]));
  }

  auto & act_abs_pos = actuator_absolute_position_;
  auto & joint_abs_pos = joint_absolute_position_;
  if (act_abs_pos.size() == num_actuators() && joint_abs_pos.size() == num_joints())
  {
    assert(act_abs_pos[0] && act_abs_pos[1] && joint_abs_pos[0] && joint_abs_pos[1]);

    joint_abs_pos[0].set_value(
      (act_abs_pos[0].get_value() / ar[0] + act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) +
      joint_offset_[0]);
    joint_abs_pos[1].set_value(
      (act_abs_pos[0].get_value() / ar[0] - act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) +
      joint_offset_[1]);
  }
}

inline void DifferentialTransmission::joint_to_actuator()
{
  const auto & ar = actuator_reduction_;
  const auto & jr = joint_reduction_;

  auto & act_pos = actuator_position_;
  auto & joint_pos = joint_position_;
  if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints())
  {
    assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]);

    double joints_offset_applied[2] = {
      joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]};
    act_pos[0].set_value(
      (joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1]) * ar[0]);
    act_pos[1].set_value(
      (joints_offset_applied[0] * jr[0] - joints_offset_applied[1] * jr[1]) * ar[1]);
  }

  auto & act_vel = actuator_velocity_;
  auto & joint_vel = joint_velocity_;
  if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints())
  {
    assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]);

    act_vel[0].set_value(
      (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]);
    act_vel[1].set_value(
      (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * ar[1]);
  }

  auto & act_eff = actuator_effort_;
  auto & joint_eff = joint_effort_;
  if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints())
  {
    assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]);

    act_eff[0].set_value(
      (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0]));
    act_eff[1].set_value(
      (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1]));
  }

  auto & act_tor = actuator_torque_;
  auto & joint_tor = joint_torque_;
  if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
  {
    assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);

    act_tor[0].set_value(
      (joint_tor[0].get_value() / jr[0] + joint_tor[1].get_value() / jr[1]) / (2.0 * ar[0]));
    act_tor[1].set_value(
      (joint_tor[0].get_value() / jr[0] - joint_tor[1].get_value() / jr[1]) / (2.0 * ar[1]));
  }

  auto & act_for = actuator_force_;
  auto & joint_for = joint_force_;
  if (act_for.size() == num_actuators() && joint_for.size() == num_joints())
  {
    assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]);

    act_for[0].set_value(
      (joint_for[0].get_value() / jr[0] + joint_for[1].get_value() / jr[1]) / (2.0 * ar[0]));
    act_for[1].set_value(
      (joint_for[0].get_value() / jr[0] - joint_for[1].get_value() / jr[1]) / (2.0 * ar[1]));
  }
}

std::string DifferentialTransmission::get_handles_info() const
{
  return fmt::format(
    FMT_COMPILE(
      "Got the following handles:\n"
      "Joint position: {}, Actuator position: {}\n"
      "Joint velocity: {}, Actuator velocity: {}\n"
      "Joint effort: {}, Actuator effort: {}\n"
      "Joint torque: {}, Actuator torque: {}\n"
      "Joint force: {}, Actuator force: {}\n"
      "Joint absolute position: {}, Actuator absolute position: {}"),
    to_string(get_names(joint_position_)), to_string(get_names(actuator_position_)),
    to_string(get_names(joint_velocity_)), to_string(get_names(actuator_velocity_)),
    to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_)),
    to_string(get_names(joint_torque_)), to_string(get_names(actuator_torque_)),
    to_string(get_names(joint_force_)), to_string(get_names(actuator_force_)),
    to_string(get_names(joint_absolute_position_)),
    to_string(get_names(actuator_absolute_position_)));
}

}  // namespace transmission_interface

#endif  // TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
