// Robotiq2fSocketAdapter.hpp

#ifndef ROBOTIQ2F_SOCKET_ADAPTER_HPP
#define ROBOTIQ2F_SOCKET_ADAPTER_HPP

#include <mutex>
#include <thread>

#include <string>
#include <map>
#include <vector>
#include <sys/time.h>
#include <sstream>
#include <tuple>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <unistd.h> // For close()
#include <cstring> // For memset()
#include <sys/select.h>  // For select() system call

#include <stdexcept> // For std::runtime_error
#include <cerrno> // For errno
#include <iostream>

#include "robotiq_2f_interface/SocketAdapterBase.hpp"

namespace robotiq{
// Enum for Gripper Status
enum class GripperStatus {
    RESET = 0,
    ACTIVATING = 1,
    // UNUSED = 2, // This value is currently not used by the gripper firmware
    ACTIVE = 3
};

// Enum for Object Status
enum class ObjectStatus {
    MOVING = 0,
    STOPPED_OUTER_OBJECT = 1,
    STOPPED_INNER_OBJECT = 2,
    AT_DEST = 3
};

class Robotiq2fSocketAdapter : public SocketAdapterBase {
public:

    Robotiq2fSocketAdapter() ;
    ~Robotiq2fSocketAdapter() override;

    // Connection management
    /**
     * Establishes a TCP connection to the Robotiq gripper.
     *
     * Attempts to open a socket and connect to the specified hostname and port. If the adapter
     * is already connected, it will disconnect and attempt to reconnect.
     *
     * @param hostname The IP address or hostname of the Robotiq gripper.
     * @param port The port number on which the gripper is listening for connections.
     * @return True if the connection was successfully established, False otherwise.
     *
     * @throws std::runtime_error if socket creation fails.
     */
    bool connect(const std::string& hostname, int port) override;
    /**
    * Closes the connection to the Robotiq gripper.
    *
    * Safely shuts down and closes the current socket connection to the gripper. If the socket
    * is already closed or was never connected, this method has no effect. This method is
    * thread-safe and can be called concurrently with other operations on the adapter.
    */
    void disconnect() override;


    /**
    * Sets multiple gripper variables using a map of variable names to integer/double values.
    *
    * This method allows setting multiple configuration parameters of the gripper simultaneously.
    * The variable names must match the expected variable identifiers used by the gripper.
    * Integer/double values are used directly in the command sent to the gripper.
    *
    * @param variableMap A map where each key is a string representing the variable name and
    *        each value is an integer representing the variable's desired value.
    * @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
    *
    * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
    */
    bool setGripperVariables(const std::map<std::string, int>& variableMap);
    bool setGripperVariables(const std::map<std::string, double>& variableMap);

    /**
    * Sets a single gripper variable.
    *
    * Allows setting a single configuration parameter of the gripper. The method overloads allow
    * specifying the variable value as either an integer or a double, depending on the expected
    * variable type.
    *
    * @param variable The variable name, matching the gripper's expected variable identifiers.
    * @param value The desired value for the variable, either as an int or a double.
    * @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
    *
    * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
    */
    bool setGripperVariable(const std::string& variable, int value);
    bool setGripperVariable(const std::string& variable, double value);

    /**
    * Retrieves the current value of a specified gripper variable.
    *
    * Sends a command to the gripper to report the current value of a specific configuration parameter.
    * The method parses the gripper's response to extract the variable value.
    *
    * @param variable The variable name, matching the gripper's expected variable identifiers.
    * @return The current value of the variable as an integer. Returns -1 if the command fails or
    *         if the variable is not recognized by the gripper.
    *
    * @throws std::runtime_error if the adapter is not connected to the gripper, if sending the command fails,
    *         or if the response from the gripper is malformed.
    */
    int getGripperVariable(const std::string& variable);

    // Gripper status
    int force() override;
    int speed() override;
    int position() override;

    /**
    * Checks if the gripper is currently active.
    *
    * Queries the current state of the gripper to determine if it is active. This method interprets
    * the gripper's status based on the last known communication and does not initiate a new request
    * to the gripper.
    *
    * @return True if the gripper is in the ACTIVE state, False otherwise.
    *
    * @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
    */
    bool is_active() override;


    // Activates the gripper and optionally performs auto-calibration
    /**
    * Activates the Robotiq gripper and optionally performs auto-calibration.
    *
    * This method sends an activation command to the gripper, bringing it from a RESET or INACTIVE
    * state to an ACTIVE state. If autoCalibrate is set to true, it also initiates an auto-calibration
    * procedure to determine the maximum and minimum positions of the gripper's motion range.
    *
    * @param autoCalibrate A boolean flag indicating whether to perform auto-calibration after
    *        activation. Default is true.
    *
    * @note Activation and auto-calibration may take several seconds to complete. The method
    *       waits for the gripper to acknowledge activation before returning.
    *
    * @throws std::runtime_error if the adapter is not connected to the gripper, if sending the
    *         activation command fails, or if the gripper does not reach the expected state within
    *         a reasonable timeframe.
    */
    void activate(bool autoCalibrate = true) override;
    /**
    * Performs auto-calibration of the gripper.
    *
    * This method initiates an auto-calibration sequence where the gripper fully opens and closes to determine
    * its operational range. This is useful for ensuring accurate position control, especially if the mechanical
    * setup of the gripper has been altered or if it is being used for the first time.
    *
    * @param log A boolean flag indicating whether to log the calibration results. Defaults to true. When set
    *            to true, the final calibration values will be logged to the standard output.
    *
    * @note This method assumes the gripper is already activated. If the gripper is not in an active state,
    *       the calibration sequence may fail. Ensure the gripper is activated before calling this method.
    *
    * @throws std::runtime_error if the adapter is not connected to the gripper, if the gripper is not in an
    *         active state, or if the calibration sequence fails.
    */
    void auto_calibrate(bool log=true);

    /**
    * Deactivates the gripper.
    *
    * Sends a command to transition the gripper from an active state back to a reset state. This method
    * is useful for safely shutting down the gripper or preparing it for a state change.
    *
    * @note This method should be called before disconnecting from the gripper or when the gripper is not
    *       needed for an extended period to ensure it is left in a safe state.
    *
    * @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
    */
    void deactivate() override;

    // Move Commands
    /**
    * Commands the gripper to move to a specified position.
    *
    * Instructs the gripper to move to a given position with specified speed and force. The position,
    * speed, and force parameters are automatically clipped to the valid range supported by the gripper.
    *
    * @param position The target position for the gripper, typically in the range [0, 255].
    * @param speed The speed at which the gripper should move, typically in the range [0, 255].
    * @param force The force to be applied by the gripper, typically in the range [0, 255].
    * @return A tuple containing a boolean indicating success of the command, and an integer representing the clipped target position.
    *
    * @throws std::runtime_error if there's an error in sending the move command or if the adapter is not connected to a gripper.
    */
    std::tuple<bool, int> move(int position, int speed, int force) override;
    /**
    * Commands the gripper to move to a specified position and waits for the move to complete.
    *
    * This method instructs the gripper to move to the given position with specified speed and force,
    * then blocks until the gripper acknowledges reaching the target position or stops moving due to
    * detecting an object.
    *
    * @param position The target position for the gripper, typically in the range [0, 255].
    * @param speed The speed at which the gripper should move, typically in the range [0, 255].
    * @param force The force to be applied by the gripper, typically in the range [0, 255].
    * @return A tuple containing the final position of the gripper as an integer and the object status
    *         as an `ObjectStatus` enumeration value.
    *
    * @throws std::runtime_error if there's an error in sending the move command, if the adapter is not
    *         connected to a gripper, or if the gripper does not reach the target position within a
    *         reasonable time frame.
    */
    std::tuple<int, ObjectStatus> move_and_wait_for_pos(int position, int speed, int force);

private:
    // Custom deleter for the smart pointer managing the socket descriptor
    struct SocketDeleter {
        void operator()(int* ptr) {
            if (ptr && *ptr >= 0) {
                close(*ptr);
            }
            delete ptr;
        }
    };

    // Smart pointer to manage the socket descriptor with the custom deleter
    std::unique_ptr<int, SocketDeleter> sockfd_;
    std::mutex socketMutex_; // Mutex for socket access synchronization

    // bounds of the encoded gripper states
    int min_position_ = 0;
    int max_position_ = 255;
    int min_speed_ = 0;
    int max_speed_ = 255;
    int min_force_ = 0;
    int max_force_ = 255;

    int open_position_ = min_position_;
    int closed_position_ = max_position_;


    /**
    * Sends a command to the connected Robotiq gripper.
    *
    * Constructs and sends a command string to the gripper over the established socket connection.
    * This method ensures thread-safe access to the socket and handles low-level socket write operations.
    *
    * @param cmd The command string to be sent to the gripper. Must conform to the gripper's protocol.
    * @return True if the command was successfully sent, False if there was an error sending the command.
    *
    * @throws std::runtime_error if attempted on a disconnected socket.
    */
    bool sendCommand(const std::string& cmd);

    /**
    * Receives a response from the Robotiq gripper.
    *
    * Waits for and reads a response from the gripper within a specified timeout period. This method
    * is blocking but allows specifying a timeout to prevent indefinite waiting. It ensures thread-safe
    * access to the socket and handles low-level socket read operations.
    *
    * @param timeout_ms The timeout in milliseconds to wait for a response. Defaults to 2000 milliseconds.
    * @return A string containing the response from the gripper. If the operation times out or fails, an empty string is returned.
    *
    * @throws std::runtime_error if attempted on a disconnected socket or if a select() call fails.
    */
    std::string receiveResponse(int timeout_ms=2000);


    /**
    * Checks if the received data is an acknowledgement message from the gripper.
    *
    * This method parses the received data string to determine if it matches the expected acknowledgement
    * pattern or message, indicating that a previously sent command was received and executed by the gripper.
    *
    * @param data The raw string data received from the gripper.
    * @return True if the data represents an acknowledgement message, False otherwise.
    */
    bool isAck(const std::string& data);

    // Helper Functions
    /**
    * Clips a variable to ensure it falls within a specified range.
    *
    * This helper function ensures that a given value does not exceed the minimum and maximum bounds
    * defined for it. It is particularly useful for sanitizing command parameters like position, speed,
    * and force values before sending them to the gripper.
    *
    * @param min_value The minimum allowable value for the variable.
    * @param variable The current value of the variable to be clipped.
    * @param max_value The maximum allowable value for the variable.
    * @return The clipped value, constrained within the specified min and max bounds.
    */
    int clip_val(int min_value, int variable, int max_value);
    /**
    * Blocks until the gripper reaches a specified status.
    *
    * This method polls the gripper's status at regular intervals until it matches the expected status
    * or until a timeout occurs. It is used internally to synchronize actions like activation, deactivation,
    * and calibration with the gripper's state transitions.
    *
    * @param expectedStatus The GripperStatus enumeration value the method waits for the gripper to achieve.
    *
    * @throws std::runtime_error if the gripper does not reach the expected status within a reasonable time frame,
    *         indicating a possible communication issue or a failure in executing the command.
    */
    void waitForGripperStatus(GripperStatus expectedStatus);

    // Resets the gripper
    /**
    * Resets the gripper to its initial state.
    *
    * Sends a command to the gripper to transition it to the RESET state. This is often used as a first step
    * in a sequence of commands that prepare the gripper for operation, ensuring it starts from a known state.
    *
    * @note This method can be used to recover the gripper from error states or to prepare it for reactivation.
    *
    * @throws std::runtime_error if the adapter is not connected to the gripper or if the reset command fails.
    */
    void reset();

    // Define the command strings and encoding used for communication
    const std::string SET_COMMAND = "SET";
    const std::string GET_COMMAND = "GET";
    const std::string ACK_MESSAGE = "ack";

    // Commands
    const std::string CMD_ACT = "ACT";  // Activation
    const std::string CMD_GTO = "GTO";  // GoTo
    const std::string CMD_ATR = "ATR";  // Automatic Release
    const std::string CMD_ADR = "ADR";  // Advanced Control
    const std::string CMD_FOR = "FOR";  // Force
    const std::string CMD_SPE = "SPE";  // Speed
    const std::string CMD_POS = "POS";  // Position
    const std::string CMD_STA = "STA";  // Status
    const std::string CMD_PRE = "PRE";  // Position Request
    const std::string CMD_OBJ = "OBJ";  // Object Detection
    const std::string CMD_FLT = "FLT";  // Fault

    // Constants buffer sizes
    const size_t BUFFER_SIZE = 1024; // Adjust as necessary
};
} // end namespace

#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP
