// hardware_interface.hpp

#ifndef ROBOTIQ2F_HARDWARE_INTERFACE_HPP
#define ROBOTIQ2F_HARDWARE_INTERFACE_HPP

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

#include "robotiq_2f_interface/SocketAdapterBase.hpp"
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp"

#include <hardware_interface/actuator_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>

#include <rclcpp/rclcpp.hpp>

#include <stdexcept>

namespace robotiq{
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
{
public:
    // Constructors
    ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface();
    ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface();
    // ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<SocketFactory> socket_factory);

    // Lifecycle Management
    ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
    ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
    ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
    ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;

    // State and Command Interfaces
    ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
    ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

    // Data Access (Read/Write)
    ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
    ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;

protected: // Likely changes the access to protected from private
    // Members for driver interaction
    bool use_mock_hardware_;
    std::unique_ptr<SocketAdapterBase> socket_adapter_;
    void RobotiqGripperHardwareInterface::configureAdapter(bool useMock);

    // Background communication thread
    std::thread communication_thread_;
    std::atomic<bool> communication_thread_is_running_;
    void background_task();

    static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();

    // Conversion Methods
    double convertRawToPosition(int raw_position);
    int convertPositionToRaw(double position);
    int convertSpeedToRaw(double speed);
    int convertForceToRaw(double force);

    // Gripper Commands
    std::atomic<uint8_t> write_command_;
    std::atomic<uint8_t> write_force_;
    std::atomic<uint8_t> write_speed_;
    std::atomic<uint8_t> gripper_current_state_;


    // Gripper State Tracking
    double gripper_position_ = 0.0;
    double gripper_velocity_ = 0.0;
    double gripper_position_command_ = 0.0;

    rclcpp::Time last_update_time_;

    double reactivate_gripper_cmd_ = 0.0;
    std::atomic<bool> reactivate_gripper_async_cmd_;
    double reactivate_gripper_response_ = 0.0;
    double gripper_force_ = 0.0;
    double gripper_speed_ = 0.0;
    std::atomic<std::optional<bool>> reactivate_gripper_async_response_;

    // Connection Variables
    std::string robot_ip_;
    int robot_port_;

    // Parameters for physical limits and configuration
    double min_position_;
    double max_position_;
    double min_speed_;
    double max_speed_;
    double min_force_;
    double max_force_;

    // loop time
    constexpr auto gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };

    // Logger
    const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
};
} // end namespace

#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP