#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"


using config_type = controller_interface::interface_configuration_type;

namespace robotiq_2f_controllers
{
RobotiqForwardController::RobotiqForwardController() : controller_interface::ControllerInterface() {}

controller_interface::CallbackReturn RobotiqForwardController::on_init()
{
    joint_name_ = auto_declare<std::string>("joint", joint_name_);
    command_interface_types_ =
    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
    state_interface_types_ =
    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);


    if (joint_name_.empty()) {
        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter.");
        return CallbackReturn::ERROR;
    }
    else{
        RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
    }

    return CallbackReturn::SUCCESS;
}

controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
{
    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");

    controller_interface::InterfaceConfiguration config;
    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

    for (const auto& interface_map_entry : command_interface_map_) {
        const std::string& interface_type = interface_map_entry.first;
        config.names.push_back(joint_name_ + "/" + interface_type);
    }
    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");

    return config;
}


controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const {
    controller_interface::InterfaceConfiguration config;
    config.type = controller_interface::interface_configuration_type::NONE;
    return config;
}

controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &)
{
    auto callback =
        [this](const std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> cmd_msg) -> void
        {
            msg_external_ptr_.writeFromNonRT(cmd_msg);
            new_msg_ = true;
        };

    command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
        "~/command", rclcpp::SystemDefaultsQoS(), callback);

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &)
{
    RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:");
    for (const auto& interface : command_interfaces_) {
        RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
    }

    // clear out vectors in case of restart
    joint_position_command_interface_.clear();
    max_velocity_command_interface_.clear();
    max_effort_command_interface_.clear();

    // assign command interfaces
    for (auto & interface : command_interfaces_)
    {
        command_interface_map_[interface.get_interface_name()]->push_back(interface);
    }

    return CallbackReturn::SUCCESS;
}

controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
{
    if (new_msg_)
    {
        command_msg_ = *msg_external_ptr_.readFromRT();
        desired_position_ = command_msg_->position;
        desired_max_velocity_ = command_msg_->max_velocity;
        desired_max_effort_ = command_msg_->max_effort;
    }
    // Set the hardware interface commands
    if (joint_position_command_interface_.empty()) {
        RCLCPP_FATAL(get_node()->get_logger(), "Command interface is not available.");
        return controller_interface::return_type::ERROR;
    }

    joint_position_command_interface_[0].get().set_value(desired_position_);
    max_velocity_command_interface_[0].get().set_value(desired_max_velocity_);
    max_effort_command_interface_[0].get().set_value(desired_max_effort_);

    return controller_interface::return_type::OK;
}


controller_interface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State &)
{
  release_interfaces();

  return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqForwardController::on_cleanup(const rclcpp_lifecycle::State &)
{
  return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqForwardController::on_error(const rclcpp_lifecycle::State &)
{
  return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const rclcpp_lifecycle::State &)
{
  return CallbackReturn::SUCCESS;
}
}

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)

