#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"


namespace robotiq{
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
}


Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
    disconnect();  // Ensure the socket is closed properly.
}

// Connection and disconnection
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
    std::lock_guard<std::mutex> lock(socketMutex_);
    if (*sockfd_ >= 0) {
        std::cerr << "Already connected. Disconnecting to reconnect.\n";
        disconnect();
    }

    int sock = socket(AF_INET, SOCK_STREAM, 0);
    if (sock < 0) {
        std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
        return false;
    }
    *sockfd_ = sock;

    sockaddr_in serv_addr{};
    memset(&serv_addr, 0, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(port);

    if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
        std::cerr << "Invalid address: " << hostname << "\n";
        close(*sockfd_);
        *sockfd_ = -1;
        return false;
    }

    if (::connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
        std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
        close(*sockfd_);
        *sockfd_ = -1;
        return false;
    }

    return true;
}

void Robotiq2fSocketAdapter::disconnect() {
    std::lock_guard<std::mutex> lock(socketMutex_);
    if (*sockfd_ >= 0) {
        if (close(*sockfd_) == -1) {
            std::cerr << "Error closing socket: " << strerror(errno) << "\n";
        } else {
            std::cout << "Disconnected successfully.\n";
        }
        *sockfd_ = -1;
    }
}

// Utility methods
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
    std::lock_guard<std::mutex> lock(socketMutex_);
    if (*sockfd_ < 0) {
        std::cerr << "Attempted to send command on a disconnected socket.\n";
        return false;
    }

    ssize_t result = send(*sockfd_, cmd.c_str(), cmd.length(), 0);
    if (result < 0) {
        std::cerr << "Failed to send command: " << strerror(errno) << "\n";
        return false;
    }

    return true;
}

std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
    std::lock_guard<std::mutex> lock(socketMutex_);
    if (*sockfd_ < 0) {
        std::cerr << "Attempted to receive response on a disconnected socket.\n";
        return "";
    }

    std::string result;
    char buffer[BUFFER_SIZE];
    bool complete_response = false;

    timeval tv;
    tv.tv_sec = timeout_ms / 1000;
    tv.tv_usec = (timeout_ms % 1000) * 1000;

    // Loop to receive data until a complete response is obtained or a timeout occurs
    while (!complete_response) {
        fd_set readfds;
        FD_ZERO(&readfds);
        FD_SET(*sockfd_, &readfds);

        int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
        if (activity == -1) {
            std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
            return "";
        } else if (activity == 0) {
            std::cerr << "Timeout occurred while waiting for response.\n";
            return "";
        }

        ssize_t bytes_read = recv(*sockfd_, buffer, BUFFER_SIZE - 1, 0);
        if (bytes_read < 0) {
            if (errno != EAGAIN && errno != EWOULDBLOCK) {
                std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
            }
            return "";
        } else if (bytes_read == 0) {
            std::cerr << "Connection closed by peer.\n";
            return "";
        } else {
            buffer[bytes_read] = '\0';
            result += buffer;

            // Check if we have a complete response:
            if (result.find("\n") != std::string::npos) {
                complete_response = true;
            }
        }
    }

    return result;
}

bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
    return data == ACK_MESSAGE;
}

int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
    return std::max(min_value, std::min(value, max_value));
}

void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
    int retries = 0;
    int status = getGripperVariable(CMD_STA);
    while (static_cast<GripperStatus>(status) != expectedStatus && retries < MAX_RETRIES) {
        std::this_thread::sleep_for(std::chrono::milliseconds(RETRY_DELAY_MS));
        status = getGripperVariable(CMD_STA);
        retries++;
    }
    if (retries >= MAX_RETRIES) {
        throw std::runtime_error("Gripper failed to reach expected status.");
    }
}


// Gripper variable Getter and Setter
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
    return setGripperVariables(reinterpret_cast<const std::map<std::string, double>&>(variableMap));
}

bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, double>& variableMap) {
    // Build the command string
    std::string cmd = SET_COMMAND;
    for (const auto& [variable, value] : variableMap) {
        std::stringstream ss;
        ss << value;
        cmd += " " + commandVariables.at(variable) + " " + ss.str();
    }
    cmd += "\n";

    // Send the command and receive the response
    std::lock_guard<std::mutex> lock(socketMutex_);
    if (*sockfd_ < 0) {
        throw std::runtime_error("Cannot set variables on a disconnected socket.");
        return false;
    }

    if (!sendCommand(cmd)) {
        throw std::runtime_error("[setGripperVariables] Sending command failed.");
        return false;
    }

    std::string response = receiveResponse();
    return isAck(response);
}

bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
    return setGripperVariable(variable, static_cast<double>(value)); // Convert int to double
}

bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
    std::stringstream ss;
    ss << value;  // Convert the value to a string

    std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";

    std::lock_guard<std::mutex> lock(socketMutex_);
    if (*sockfd_ < 0) {
        throw std::runtime_error("Cannot set variables on a disconnected socket.");
        return false;
    }

    if (!sendCommand(cmd)) {
        throw std::runtime_error("[setGripperVariable] Sending command failed.");
        return false; // Sending command failed
    }

    std::string response = receiveResponse();
    return isAck(response);
}

int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
    std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";

    std::lock_guard<std::mutex> lock(socketMutex_);
    if (*sockfd_ < 0) {
        throw std::runtime_error("Cannot get variables on a disconnected socket.");
        return -1;
    }

    if (!sendCommand(cmd)) {
        throw std::runtime_error("[getGripperVariable] Sending command failed.");
        return -1; // Error sending command
    }

    std::string response = receiveResponse();

    // Parse the response (assuming format: "variable_name value")
    std::istringstream iss(response);
    std::string var_name, value_str;

    if (!(iss >> var_name >> value_str)) {
        throw std::runtime_error("Invalid gripper response format.");
        return -1;
    }

    if (var_name != variable) {
        throw std::runtime_error("Unexpected response - variable name mismatch.");
        return -1;
    }

    try {
        return std::stoi(value_str);
    } catch (const std::exception& ex) {
        std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
        return -1;
    }
}

// Activation, deactivation and reset of the Gripper
void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
    reset(); // Always start with a reset to ensure a known state
    setGripperVariable(CMD_ACT, 1); // Activate the gripper
    waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active

    if (autoCalibrate) {
        auto_calibrate();
    }
}

void Robotiq2fSocketAdapter::reset() {
    setGripperVariable(CMD_ACT, 0);
    setGripperVariable(CMD_ATR, 0);
    waitForGripperStatus(GripperStatus::RESET);
}


void Robotiq2fSocketAdapter::deactivate() {
    if (is_active()) {
        reset();
    }
}

// State subsscriber
bool Robotiq2fSocketAdapter::is_active(){
    GripperStatus status = static_cast<GripperStatus>(getGripperVariable(CMD_STA));
    return GripperStatus::ACTIVE == status;
}

int Robotiq2fSocketAdapter::force(){
    return getGripperVariable(CMD_FOR);
}

int Robotiq2fSocketAdapter::speed(){
    return getGripperVariable(CMD_SPE);
}

int Robotiq2fSocketAdapter::position(){
    return getGripperVariable(CMD_POS);
}

// Movement Methods
std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
    int clippedPosition = clip_val(min_position_, position, max_position_);
    int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
    int clippedForce = clip_val(min_force_, force, max_force_);

    // Create a map for gripper variables (similar to Python's OrderedDict)
    std::map<std::string, int> variableMap = {
        { CMD_POS, clippedPosition },
        { CMD_SPE, clippedSpeed },
        { CMD_FOR, clippedForce },
        { CMD_GTO, 1 }
    };

    bool setResult = setGripperVariables(variableMap);
    return std::make_tuple(setResult, clippedPosition);
}

std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
    int clippedPosition = clip_val(min_position_, position, max_position_);
    int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
    int clippedForce = clip_val(min_force_, force, max_force_);

    // Set gripper variables to initiate the move
    std::map<std::string, int> variableMap = {
        { CMD_POS, clippedPosition },
        { CMD_SPE, clippedSpeed },
        { CMD_FOR, clippedForce },
        { CMD_GTO, 1 }
    };
    bool setResult = setGripperVariables(variableMap);
    if (!setResult) {
        // Handle the error case, e.g., throw an exception
        throw std::runtime_error("Failed to set variables for move.");
    }

    // Wait until position is acknowledged:
    while(getGripperVariable(CMD_PRE) != clippedPosition) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
    }

    // Wait until not moving
    int currentObjectStatus = getGripperVariable(CMD_OBJ);
    while (ObjectStatus(currentObjectStatus) == ObjectStatus::MOVING) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
        currentObjectStatus = getGripperVariable(CMD_OBJ);
    }

    // Report final position and object status
    int finalPosition = getGripperVariable(CMD_POS);
    ObjectStatus finalStatus = static_cast<ObjectStatus>(currentObjectStatus);
    return std::make_tuple(finalPosition, finalStatus);
}


void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
    // Open in case we are holding an object
    std::tuple<int, ObjectStatus> result = move_and_wait_for_pos(open_position_, 64, 1);
    if (std::get<1>(result) != ObjectStatus::AT_DEST) {
        throw std::runtime_error("Calibration failed opening to start.");
    }

    // Close as far as possible
    result = move_and_wait_for_pos(closed_position_, 64, 1);
    if (std::get<1>(result) != ObjectStatus::AT_DEST) {
        throw std::runtime_error("Calibration failed because of an object.");
    }
    max_position_ = std::get<0>(result); // Update max position

    // Open as far as possible
    result = move_and_wait_for_pos(open_position_, 64, 1);
    if (std::get<1>(result) != ObjectStatus::AT_DEST) {
        throw std::runtime_error("Calibration failed because of an object.");
    }
    min_position_ = std::get<0>(result); // Update min position

    if (log) {
        std::cout << "Gripper auto-calibrated to ["
                  << min_position_ << ", " << max_position_ << "]\n";
    }
}
} // end namespace