#pragma once

#include "rm_control/interfaces/dexterous_hand.h"

namespace rm_control::tests {

class MockDexterousHand final : public interfaces::IDexterousHand {
public:
    common::Result<interfaces::DexterousHandReadiness> readiness_result =
        common::Result<interfaces::DexterousHandReadiness>::failure(
            "mock readiness was not configured");
    common::Result<void> command_result = common::Result<void>::success();
    int readiness_calls = 0;
    int command_calls = 0;
    interfaces::O6MotionCommand last_motion_command{};

    /** @copydoc interfaces::IDexterousHand::ReadReadiness */
    common::Result<interfaces::DexterousHandReadiness>
    ReadReadiness() override {
        ++readiness_calls;
        return readiness_result;
    }

    /** @copydoc interfaces::IDexterousHand::CommandMotion */
    common::Result<void> CommandMotion(
        const interfaces::O6MotionCommand &command) override {
        ++command_calls;
        last_motion_command = command;
        return command_result;
    }
};

} // namespace rm_control::tests
