#include <gtest/gtest.h>
#include "RobotiqGripperHardwareInterface.hpp"

class TestableRobotiqGripperHardwareInterface : public robotiq_driver::RobotiqGripperHardwareInterface {
public:
    using robotiq_driver::RobotiqGripperHardwareInterface::convertPositionToRaw;
    using robotiq_driver::RobotiqGripperHardwareInterface::convertRawToPosition;
    using robotiq_driver::RobotiqGripperHardwareInterface::convertSpeedToRaw;
    using robotiq_driver::RobotiqGripperHardwareInterface::convertForceToRaw;
};

class RobotiqGripperConversionTests : public ::testing::Test {
protected:
    TestableRobotiqGripperHardwareInterface gripper_interface;

    void SetUp() override {
        gripper_interface.min_position_ = 0.0;
        gripper_interface.max_position_ = 0.14;
        gripper_interface.min_speed_ = 0.02;
        gripper_interface.max_speed_ = 0.15;
        gripper_interface.min_force_ = 20.0;
        gripper_interface.max_force_ = 235.0;
    }
};

// Test for convertPositionToRaw
TEST_F(RobotiqGripperConversionTests, PositionToRawValidInput) {
    EXPECT_EQ(gripper_interface.convertPositionToRaw(0.07), 128);
}

TEST_F(RobotiqGripperConversionTests, PositionToRawBoundaryInput) {
    EXPECT_EQ(gripper_interface.convertPositionToRaw(0.0), 0);
    EXPECT_EQ(gripper_interface.convertPositionToRaw(0.14), 255);
}

// Test for convertRawToPosition
TEST_F(RobotiqGripperConversionTests, RawToPositionValidInput) {
    ASSERT_NEAR(gripper_interface.convertRawToPosition(128), 0.07, 0.01);
}

TEST_F(RobotiqGripperConversionTests, RawToPositionBoundaryInput) {
    ASSERT_NEAR(gripper_interface.convertRawToPosition(0), 0.0, 0.01);
    ASSERT_NEAR(gripper_interface.convertRawToPosition(255), 0.14, 0.01);
}

// Test for convertSpeedToRaw
TEST_F(RobotiqGripperConversionTests, SpeedToRawValidInput) {
    EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.085), 128);
}

TEST_F(RobotiqGripperConversionTests, SpeedToRawBoundaryInput) {
    EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.02), 0);
    EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.15), 255);
}

// Test for convertForceToRaw
TEST_F(RobotiqGripperConversionTests, ForceToRawValidInput) {
    EXPECT_EQ(gripper_interface.convertForceToRaw(127.5), 128);
}

TEST_F(RobotiqGripperConversionTests, ForceToRawBoundaryInput) {
    EXPECT_EQ(gripper_interface.convertForceToRaw(20.0), 0);
    EXPECT_EQ(gripper_interface.convertForceToRaw(235.0), 255);
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}