#include "ar3_hardware/ar3_system.hpp"

#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <vector>

#include "hardware_interface/types/hardware_interface_type_values.hpp"

namespace ar3_hardware
{

hardware_interface::return_type Ar3SystemHardware::configure(
  const hardware_interface::HardwareInfo & info)
{
  // Initialize default configuration
  if (configure_default(info) != hardware_interface::return_type::OK)
  {
    return hardware_interface::return_type::ERROR;
  }

  //hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
  //hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);

  // Resize position and command vectors to match actual configuration
  // This makes sure a default value is initialized for each joint and saves
  hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  for (const hardware_interface::ComponentInfo & joint : info_.joints)
  {
    // We only use one command interface
    if (joint.command_interfaces.size() != 1)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("Ar3SystemHardware"),
        "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(),
        joint.command_interfaces.size());
      return hardware_interface::return_type::ERROR;
    }

    // Make sure the command interface is in position mode
    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("Ar3SystemHardware"),
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::return_type::ERROR;
    }

    // We only publish to one state interface
    if (joint.state_interfaces.size() != 1)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("Ar3SystemHardware"),
        "Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(),
        joint.state_interfaces.size());
      return hardware_interface::return_type::ERROR;
    }

    // Make sure the state interface is in position mode
    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("Ar3SystemHardware"),
        "Joint '%s' have '%s' as state interface. '%s' expected.",
        joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
        hardware_interface::HW_IF_POSITION);
      return hardware_interface::return_type::ERROR;
    }
  }

  status_ = hardware_interface::status::CONFIGURED;
  return hardware_interface::return_type::OK;
}

std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_interfaces()
{
  // Tell the controller about all joints that we publish state info for
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (auto i = 0u; i < info_.joints.size(); i++)
  {
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> Ar3SystemHardware::export_command_interfaces()
{
  // Tell the controller about all joints that we take commands from
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (auto i = 0u; i < info_.joints.size(); i++)
  {
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
  }

  return command_interfaces;
}

hardware_interface::return_type Ar3SystemHardware::start()
{
  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Starting ...please wait...");

  // set some default values
  for (auto i = 0u; i < hw_positions_.size(); i++)
  {
    if (std::isnan(hw_positions_[i]))
    {
      hw_positions_[i] = 0;
      hw_commands_[i] = 0;
    }
  }

  // Hack to make sure the node stays running by putting it in its own thread.
  // this is because we wrote the hardware interface in python earlier and
  // the driver just listens on some subscribers. This allows a node in this
  // controller to publish values to that driver
  // The correct thing to do would be to move all of the driver code into this
  // controller and remove the node, publishers, and subscribers entierly
  executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
  nh_ = std::make_shared<rclcpp::Node>("ar3_hardware");
  executor_->add_node(nh_);

  auto spin = [this]() {
    while(rclcpp::ok()) {
      executor_->spin_once();
    }
  };
  thread_executor_spin_ = std::thread(spin);

  // Create joint state publishers
  // TODO: Don't use Float32, use Pose
  j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j1", 10);
  j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j2", 10);
  j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j3", 10);
  j4_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j4", 10);
  j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
  j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);

  // Create joint command subscribers
  // TODO: Don't use Float32, use Pose
  s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
  s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
  s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
  s_j4 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j4", 10, std::bind(&Ar3SystemHardware::update_j4_cb, this, std::placeholders::_1));
  s_j5 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j5", 10, std::bind(&Ar3SystemHardware::update_j5_cb, this, std::placeholders::_1));
  s_j6 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j6", 10, std::bind(&Ar3SystemHardware::update_j6_cb, this, std::placeholders::_1));

  status_ = hardware_interface::status::STARTED;

  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System Successfully started!");

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type Ar3SystemHardware::stop()
{
  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Stopping ...please wait...");

  status_ = hardware_interface::status::STOPPED;

  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System successfully stopped!");

  return hardware_interface::return_type::OK;
}

// These callbacks are called from the command subscribers, they update
// the hw_positions_ array with the current joint state that will be
// sent to the controller.
// Again, this is wrong and these values should be filled in by the read() function
void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
    hw_positions_[0] = msg->data;
}

void Ar3SystemHardware::update_j2_cb(std_msgs::msg::Float32::SharedPtr msg) {
    hw_positions_[1] = msg->data;
}

void Ar3SystemHardware::update_j3_cb(std_msgs::msg::Float32::SharedPtr msg) {
    hw_positions_[2] = msg->data;
}

void Ar3SystemHardware::update_j4_cb(std_msgs::msg::Float32::SharedPtr msg) {
    hw_positions_[3] = msg->data;
}

void Ar3SystemHardware::update_j5_cb(std_msgs::msg::Float32::SharedPtr msg) {
    hw_positions_[4] = msg->data;
}

void Ar3SystemHardware::update_j6_cb(std_msgs::msg::Float32::SharedPtr msg) {
    hw_positions_[5] = msg->data;
}

hardware_interface::return_type Ar3SystemHardware::read()
{
  // RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");

  /*
  for (uint i = 0; i < hw_commands_.size(); i++)
  {
    hw_positions_[i] = 6.9;

  //  RCLCPP_INFO(
  //    rclcpp::get_logger("Ar3SystemHardware"),
  //    "Got position state %.5f for '%s'!", hw_positions_[i],
  //    info_.joints[i].name.c_str());
  }
  */

  //RCLCPP_INFO(
  //  rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",
  //  hw_commands_[0], hw_commands_[1], hw_commands_[2]);

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type ar3_hardware::Ar3SystemHardware::write()
{
  //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Writing...");

  // Publish joint commands to the arm driver
  auto message = std_msgs::msg::Float32();
  message.data = hw_commands_[0];
  j1_pub_->publish(message);
  message.data = hw_commands_[1];
  j2_pub_->publish(message);
  message.data = hw_commands_[2];
  j3_pub_->publish(message);
  message.data = hw_commands_[3];
  j4_pub_->publish(message);
  message.data = hw_commands_[4];
  j5_pub_->publish(message);
  message.data = hw_commands_[5];
  j6_pub_->publish(message);

  //for (auto i = 0u; i < hw_commands_.size(); i++)
  //{
    // Simulate sending commands to the hardware
  //  RCLCPP_INFO(
  //    rclcpp::get_logger("Ar3SystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i],
  //    info_.joints[i].name.c_str());
  //}
  //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully written!");

  return hardware_interface::return_type::OK;
}

}  // namespace ar3_hardware

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
  ar3_hardware::Ar3SystemHardware, hardware_interface::SystemInterface)
