// MockRobotiq2fSocketAdapter.hpp

#ifndef MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
#define MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP

#include <string>
#include <map>
#include <atomic>
#include <tuple>

#include "robotiq_2f_interface/SocketAdapterBase.hpp"

namespace robotiq{
class MockRobotiq2fSocketAdapter : public SocketAdapterBase {
public:
        MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {}

    ~MockRobotiq2fSocketAdapter() override {
        if (movementThread && movementThread->joinable()) {
            movementThread->join();
        }
    }

    bool connect(const std::string& hostname, int port) override {
        isConnected.store(true);
        return true; // Simulate successful connection
    }

    void disconnect() override {
        isConnected.store(false);
    }

    int getGripperPosition() override {
        return gripperPosition.load();
    }

    int getGripperForce() override {
        // Assuming this is supposed to mimic `force()`
        return gripperForce.load();
    }

    int force() override {
        return gripperForce.load();
    }

    int speed() override {
        return gripperSpeed.load();
    }

    int position() override {
        return gripperPosition.load();
    }

    bool is_active() override {
        return isActive.load();
    }

    void activate(bool autoCalibrate = true) override {
        isActive.store(true);
    }

    void deactivate() override {
        isActive.store(false);
    }

    std::tuple<bool, int> move(int targetPosition, int speed, int force) override{
        // Ensure only one movement operation is active at a time
        if (movementThread && movementThread->joinable()) {
            movementThread->join(); // Wait for any ongoing movement to complete
        }

        // Start a new movement operation in a background thread
        movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);

        return std::make_tuple(true, targetPosition); // Immediately return success and the target position
    }

private:
    std::atomic<bool> isConnected;
    std::atomic<int> gripperPosition;
    std::atomic<int> gripperForce;
    std::atomic<int> gripperSpeed;
    std::atomic<bool> isActive;

    std::unique_ptr<std::thread> movementThread;

    void simulateMovement(int targetPosition, int speed, int force) {
        int step = (targetPosition > gripperPosition) ? 1 : -1;

        // Simulate moving towards the target position
        while (gripperPosition != targetPosition) {
            gripperPosition += step;
            std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step
        }

        // Here, you simulate setting the speed and force, though for simplicity, we're just logging it
        // In a real mock, you might update some internal state to reflect these changes
        std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
    }
};
} // end namespace

#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP