#include "robotiq_2f_interface/hardware_interface.hpp"

namespace robotiq
{
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
  : communication_thread_is_running_(false)
{
}

RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
{
  if (communication_thread_is_running_.load())
  {
    communication_thread_is_running_.store(false);
    if (communication_thread_.joinable())
    {
      communication_thread_.join();
    }
  }
}

hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
{
    RCLCPP_DEBUG(rclcpp::get_logger("RobotiqGripperHardwareInterface"), "on_init");

    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
    {
        return CallbackReturn::ERROR;
    }

    // Loading paramters from Hardware info and .yaml file
       auto node = this->get_node();
    node->declare_parameter("min_position", 0.0);
    node->declare_parameter("max_position", 0.14);
    node->declare_parameter("min_speed", 0.02);
    node->declare_parameter("max_speed", 0.15);
    node->declare_parameter("min_force", 20.0);
    node->declare_parameter("max_force", 235.0);
    try
    {
        robot_ip_ =  info_.hardware_parameters.at("robot_ip");
        robot_port_ =  std::stod(info_.hardware_parameters.at("robot_port"));
        use_mock_hardware_ = std::stod(info_.hardware_parameters.at("use_fake_hardware"));
        node->get_parameter("min_position", min_position_);
        node->get_parameter("max_position", max_position_);
        node->get_parameter("min_speed", min_speed_);
        node->get_parameter("max_speed", max_speed_);
        node->get_parameter("min_force", min_force_);
        node->get_parameter("max_force", max_force_);
    }
    catch (const std::exception& e)
    {
        RCLCPP_FATAL(rclcpp::get_logger("RobotiqGripperHardwareInterface"),
                     "Failed to load parameters: %s", e.what());
        return CallbackReturn::ERROR;
    }

    gripper_position_ = std::numeric_limits<double>::quiet_NaN();
    gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
    gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
    reactivate_gripper_cmd_ = NO_NEW_CMD_;
    reactivate_gripper_async_cmd_.store(false);

    const hardware_interface::ComponentInfo& joint = info_.joints[0];

    // There is one command interface: position.
    if (joint.command_interfaces.size() != 1)
    {
        RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
                     joint.command_interfaces.size());
        return CallbackReturn::ERROR;
    }

    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
    {
        RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
                     joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
        return CallbackReturn::ERROR;
    }

    // There are two state interfaces: position and velocity.
    if (joint.state_interfaces.size() != 2)
    {
        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
                     joint.state_interfaces.size());
        return CallbackReturn::ERROR;
    }

    for (int i = 0; i < 2; ++i)
    {
        if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
              joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
        {
            RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(),
                       joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
                       hardware_interface::HW_IF_VELOCITY);
            return CallbackReturn::ERROR;
        }
    }

    return CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
{
    RCLCPP_DEBUG(logger_, "on_configure");
    try
    {
        if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
        {
            return CallbackReturn::ERROR;
        }

        socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
        if (!socket_adapter_->connect(robot_ip_, robot_port_))
        {
            RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
            return CallbackReturn::ERROR;
        }
    }
    catch (const std::exception& e)
    {
        RCLCPP_ERROR(logger_, "Cannot configure the Robotiq gripper: %s", e.what());
        return CallbackReturn::ERROR;
    }

    return CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
{
    RCLCPP_DEBUG(logger_, "on_activate");

    // set some default values for joints
    if (std::isnan(gripper_position_))
    {
        gripper_position_ = 0;
        gripper_velocity_ = 0;
        gripper_position_command_ = 0;
    }

    // Activate the gripper.
    try
    {
        socket_adapter_->deactivate();
        socket_adapter_->activate();

        communication_thread_is_running_.store(true);
        communication_thread_ = std::thread([this] { this->background_task(); });
    }
    catch (const std::exception& e)
    {
        RCLCPP_FATAL(logger_, "Failed to communicate with the Robotiq gripper: %s", e.what());
        return CallbackReturn::ERROR;
    }

    RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!");
    return CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
{
    RCLCPP_DEBUG(logger_, "on_deactivate");

    communication_thread_is_running_.store(false);
    communication_thread_.join();
    if (communication_thread_.joinable())
    {
        communication_thread_.join();
    }

    try
    {
        socket_adapter_->deactivate();
    }
    catch (const std::exception& e)
    {
        RCLCPP_ERROR(logger_, "Failed to deactivate the Robotiq gripper: %s", e.what());
        return CallbackReturn::ERROR;
    }
    RCLCPP_INFO(logger_, "Robotiq Gripper successfully deactivated!");
    return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
{
    RCLCPP_DEBUG(logger_, "export_state_interfaces");

    std::vector<hardware_interface::StateInterface> state_interfaces;

    state_interfaces.emplace_back(
        hardware_interface::StateInterface(
            info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_
        )
    );

    state_interfaces.emplace_back(
        hardware_interface::StateInterface(
            info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_
        )
    );

    return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterface::export_command_interfaces()
{
    RCLCPP_DEBUG(logger_, "export_command_interfaces");

    std::vector<hardware_interface::CommandInterface> command_interfaces;

    command_interfaces.emplace_back(
        hardware_interface::CommandInterface(
        info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_
        )
    );

    // Handling potential uninitialized parameter 'gripper_speed_multiplier'
    double gripper_speed_multiplier = 1.0;
    if (info_.hardware_parameters.find("gripper_speed_multiplier") != info_.hardware_parameters.end()) {
        gripper_speed_multiplier = std::stod(info_.hardware_parameters["gripper_speed_multiplier"]);
    }
    gripper_speed_ = gripper_speed_multiplier;

    // Handling potential uninitialized parameter 'gripper_force_multiplier'
    double gripper_force_multiplier = 1.0;
    if (info_.hardware_parameters.find("gripper_force_multiplier") != info_.hardware_parameters.end()) {
        gripper_force_multiplier = std::stod(info_.hardware_parameters["gripper_force_multiplier"]);
    }
    gripper_force_ = gripper_force_multiplier;

    command_interfaces.emplace_back(
        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
    command_interfaces.emplace_back(
        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
    command_interfaces.emplace_back(
        hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
    command_interfaces.emplace_back(
        hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_));

    return command_interfaces;
}

hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& /*period*/)
{
    RCLCPP_DEBUG(logger_, "Reading state from the gripper");

    // Fetch current position and other states from the hardware
    try {
        int raw_position = socket_adapter_->position();  // This should be an existing method in your adapter
        double new_position = convertRawToPosition(raw_position);
        rclcpp::Time current_time = time;

        if (!std::isnan(gripper_position_)) {
            double time_difference = (current_time - last_update_time_).seconds();  // Calculate time difference in seconds
            if (time_difference > 0) {
                gripper_velocity_ = (new_position - gripper_position_) / time_difference;  // Calculate velocity
            }
        }

        gripper_position_ = new_position;  // Update current position
        last_update_time_ = current_time;  // Update last update timestamp

    } catch (const std::exception& e) {
        RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what());
        return hardware_interface::return_type::ERROR;
    }

    // Handle reactivation command if set
    if (!std::isnan(reactivate_gripper_cmd_) && reactivate_gripper_cmd_ != NO_NEW_CMD_) {
        RCLCPP_INFO(logger_, "Sending gripper reactivation request.");
        reactivate_gripper_async_cmd_.store(true);
        reactivate_gripper_cmd_ = NO_NEW_CMD_;  // Reset command
    }

    // Process asynchronous reactivation response if available
    if (reactivate_gripper_async_response_.load().has_value()) {
        reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
        reactivate_gripper_async_response_.store(std::nullopt);  // Reset response
    }

    return hardware_interface::return_type::OK;
}

hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
{
    try {
        // Use the conversion methods to scale the command values to the range expected by the hardware
        int scaled_position = convertPositionToRaw(gripper_position_command_);
        int scaled_speed = convertSpeedToRaw(gripper_speed_);
        int scaled_force = convertForceToRaw(gripper_force_);

        // Store the scaled values into atomic variables
        write_command_.store(scaled_position);
        write_speed_.store(scaled_speed);
        write_force_.store(scaled_force);

    } catch (const std::exception& e) {
        RCLCPP_ERROR(logger_, "Failed to write to Robotiq gripper: %s", e.what());
        return hardware_interface::return_type::ERROR;
    }

    return hardware_interface::return_type::OK;
}

void RobotiqGripperHardwareInterface::background_task()
{
  while (communication_thread_is_running_.load())
  {
    std::lock_guard<std::mutex> guard(resource_mutex);
    try
    {
      // Re-activate the gripper
      // This can be used, for example, to re-run the auto-calibration.
      if (reactivate_gripper_async_cmd_.load())
      {
        socket_adapter_->deactivate();
        socket_adapter_->activate();
        reactivate_gripper_async_cmd_.store(false);
        reactivate_gripper_async_response_.store(true);
      }

      // Write the latest command to the gripper.
      int scaled_position = write_command_.load();
      int scaled_speed = write_speed_.load();
      int scaled_force = write_force_.load();

      if (!socket_adapter_->move(scaled_position, scaled_speed, scaled_force)) {
        throw std::runtime_error("Failed to send move command to Robotiq gripper.");
      }

      // Read the state of the gripper.
      int raw_position = socket_adapter_->position();
      gripper_current_state_.store(convertRawToPosition(raw_position));
    }
    catch (const std::exception& e)
    {
      RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
    }

    std::this_thread::sleep_for(gripperCommsLoopPeriod);  // Sleep for a predefined period
  }
}


// Mock Hardware
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
    if (useMock) {
        RCLCPP_INFO(logger_, "Using mock Robotiq adapter");
        socket_adapter_ = std::make_unique<MockRobotiq2fSocketAdapter>();
    } else {
        RCLCPP_INFO(logger_, "Using real Robotiq adapter");
        socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
    }
}


// Conversion methods
double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
        throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
    }
    double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
    return std::max(min_position_, std::min(scaled_position, max_position_));
}

uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
        throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
    }
    double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
}

uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
    if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) {
        throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly.");
    }
    double scaled = (speed - min_speed_) / (max_speed_ - min_speed_) * 255.0;
    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
}

uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
    if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) {
        throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly.");
    }
    double scaled = (force - min_force_) / (max_force_ - min_force_) * 255.0;
    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
}

}  // namespace robotiq_driver

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(robotiq_2f_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)
