#pragma once

#include "rm_control/interfaces/robot_arm.h"

#include <filesystem>
#include <fstream>
#include <string>
#include <vector>

namespace rm_control::tests {

class MockRobotArm final : public interfaces::IRobotArm {
public:
    bool connected = true;
    common::RobotInfo robot_info;
    common::RobotSoftwareInfo software_info;
    common::ArmState arm_state;
    common::ForceData force_data;
    common::RobotSafetyStatus safety_status;
    common::RobotJointLimits joint_limits;
    int vendor_save_return_code = 0;
    int vendor_point_count = 3;
    bool create_vendor_file = true;
    int fail_current_state_on_or_after_call = -1;
    int start_recording_failures_remaining = 0;
    int stop_recording_failures_remaining = 0;
    int current_state_calls = 0;
    int start_calls = 0;
    int stop_recording_calls = 0;
    int save_calls = 0;
    int origin_calls = 0;
    int replay_calls = 0;
    int pause_calls = 0;
    int continue_calls = 0;
    int stop_replay_calls = 0;
    int move_joint_calls = 0;
    int execute_vendor_file_calls = 0;
    int save_vendor_program_calls = 0;
    int run_controller_program_calls = 0;
    int program_state_calls = 0;
    int slow_stop_calls = 0;
    int program_slot_query_calls = 0;
    bool last_move_joint_blocking = true;
    bool apply_move_joint_target_immediately = true;
    bool controller_program_slot_available = true;
    bool controller_program_was_running = false;
    common::VendorDragTrajectoryExecutionRequest last_vendor_execution_request;
    common::VendorDragTrajectoryExecutionRequest last_vendor_program_save_request;
    common::ControllerProgramRunRequest last_controller_program_run_request;
    std::vector<common::VendorDragTrajectoryExecutionRequest>
        vendor_program_save_requests;
    std::vector<common::ControllerProgramRunRequest>
        controller_program_run_requests;
    common::Result<void> save_vendor_program_result = common::Result<void>::success();
    common::Result<void> run_controller_program_result = common::Result<void>::success();
    std::vector<common::Result<void>> save_vendor_program_results;
    std::vector<common::Result<void>> run_controller_program_results;
    std::vector<std::vector<float>> completed_program_joint_targets;
    std::vector<std::string> operation_log;
    std::vector<common::ControllerProgramRunState> program_run_states = {
        common::ControllerProgramRunState{1, 1, 0, 5},
        common::ControllerProgramRunState{0, 1, 0, 5},
    };

    MockRobotArm() {
        robot_info.arm_dof = static_cast<int>(common::kRm75JointCount);
        robot_info.controller_version = 3;
        robot_info.is_rm75 = true;
        robot_info.six_axis_force_sensor_available = true;
        software_info.sdk_version = "test-sdk";
        software_info.product_version = "RM75-MOCK";
        software_info.firmware_version = "test-firmware";
        arm_state.joints_deg.assign(common::kRm75JointCount, 0.0F);
        safety_status.real_mode_enabled = true;
        safety_status.arm_power_on = true;
        safety_status.all_joints_enabled = true;
        safety_status.arm_error_free = true;
        safety_status.controller_error_free = true;
        safety_status.virtual_wall_enabled = true;
        safety_status.self_collision_enabled = true;
        joint_limits.minimum_position_deg.fill(-360.0F);
        joint_limits.maximum_position_deg.fill(360.0F);
        joint_limits.maximum_speed_deg_per_second.fill(180.0F);
        joint_limits.maximum_acceleration_deg_per_second_squared.fill(180.0F);
    }

    common::Result<void> connect(const common::RobotConnectionConfig &) override {
        connected = true;
        return common::Result<void>::success();
    }

    void disconnect() override {
        connected = false;
    }

    bool isConnected() const noexcept override {
        return connected;
    }

    common::Result<common::RobotInfo> getRobotInfo() override {
        return common::Result<common::RobotInfo>::success(robot_info);
    }

    common::Result<common::RobotSoftwareInfo> getRobotSoftwareInfo() override {
        return common::Result<common::RobotSoftwareInfo>::success(software_info);
    }

    common::Result<common::ArmState> getCurrentArmState() override {
        ++current_state_calls;
        if (fail_current_state_on_or_after_call > 0 &&
            current_state_calls >= fail_current_state_on_or_after_call) {
            return common::Result<common::ArmState>::failure(
                "mock current arm state failure");
        }
        return common::Result<common::ArmState>::success(arm_state);
    }

    common::Result<common::ForceData> getForceData() override {
        return common::Result<common::ForceData>::success(force_data);
    }

    common::Result<common::RobotSafetyStatus> getRobotSafetyStatus() override {
        return common::Result<common::RobotSafetyStatus>::success(safety_status);
    }

    common::Result<common::RobotJointLimits> getJointLimits() override {
        return common::Result<common::RobotJointLimits>::success(joint_limits);
    }

    common::Result<void> moveJ(const std::vector<float> &joints_deg,
                               int,
                               int,
                               int,
                               bool blocking) override {
        ++move_joint_calls;
        last_move_joint_blocking = blocking;
        if (apply_move_joint_target_immediately) {
            arm_state.joints_deg = joints_deg;
        }
        return common::Result<void>::success();
    }

    common::Result<void> moveL(const common::Pose &,
                               int,
                               int,
                               int,
                               bool) override {
        return common::Result<void>::success();
    }

    common::Result<void> startDragTeach(
        const common::DragTeachStartOptions &) override {
        ++start_calls;
        if (start_recording_failures_remaining > 0) {
            --start_recording_failures_remaining;
            return common::Result<void>::failure("mock uncertain drag teach start");
        }
        return common::Result<void>::success();
    }

    common::Result<void> stopDragTeach() override {
        ++stop_recording_calls;
        if (stop_recording_failures_remaining > 0) {
            --stop_recording_failures_remaining;
            return common::Result<void>::failure("mock stop drag teach failure");
        }
        return common::Result<void>::success();
    }

    common::VendorTrajectorySaveAttempt saveDragTeachTrajectory(
        const std::filesystem::path &path) override {
        ++save_calls;
        common::VendorTrajectorySaveAttempt attempt;
        attempt.path = path;
        attempt.api_called = true;
        attempt.return_code = vendor_save_return_code;
        attempt.point_count = vendor_point_count;
        if (create_vendor_file && vendor_save_return_code == 0) {
            std::ofstream output(path, std::ios::binary);
            output << "mock rm75 raw trajectory";
        }
        return attempt;
    }

    common::Result<void> moveToDragTrajectoryOrigin(bool) override {
        ++origin_calls;
        return common::Result<void>::success();
    }

    common::Result<void> replayLastDragTrajectory(bool) override {
        ++replay_calls;
        return common::Result<void>::success();
    }

    common::Result<void> pauseDragTrajectoryReplay() override {
        ++pause_calls;
        return common::Result<void>::success();
    }

    common::Result<void> continueDragTrajectoryReplay() override {
        ++continue_calls;
        return common::Result<void>::success();
    }

    common::Result<void> stopDragTrajectoryReplay() override {
        ++stop_replay_calls;
        return common::Result<void>::success();
    }

    common::Result<void> executeVendorDragTrajectoryFile(
        const common::VendorDragTrajectoryExecutionRequest &request) override {
        ++execute_vendor_file_calls;
        last_vendor_execution_request = request;
        return common::Result<void>::success();
    }

    common::Result<void> saveVendorDragTrajectoryProgram(
        const common::VendorDragTrajectoryExecutionRequest &request) override {
        ++save_vendor_program_calls;
        last_vendor_program_save_request = request;
        vendor_program_save_requests.push_back(request);
        operation_log.push_back("saveVendorDragTrajectoryProgram");
        if (!save_vendor_program_results.empty()) {
            const auto result = save_vendor_program_results.front();
            save_vendor_program_results.erase(save_vendor_program_results.begin());
            return result;
        }
        return save_vendor_program_result;
    }

    common::Result<void> runControllerProgram(
        const common::ControllerProgramRunRequest &request) override {
        ++run_controller_program_calls;
        last_controller_program_run_request = request;
        controller_program_run_requests.push_back(request);
        operation_log.push_back("runControllerProgram");
        if (!run_controller_program_results.empty()) {
            const auto result = run_controller_program_results.front();
            run_controller_program_results.erase(run_controller_program_results.begin());
            return result;
        }
        return run_controller_program_result;
    }

    common::Result<common::ControllerProgramRunState>
    getControllerProgramRunState() override {
        ++program_state_calls;
        if (program_run_states.empty()) {
            return common::Result<common::ControllerProgramRunState>::failure(
                "mock program run state was not configured");
        }
        const auto state = program_run_states.front();
        if (state.raw_state == 1) {
            controller_program_was_running = true;
        } else if (state.raw_state == 0 && controller_program_was_running) {
            if (!completed_program_joint_targets.empty()) {
                arm_state.joints_deg = completed_program_joint_targets.front();
                completed_program_joint_targets.erase(
                    completed_program_joint_targets.begin());
            }
            controller_program_was_running = false;
        }
        if (program_run_states.size() > 1) {
            program_run_states.erase(program_run_states.begin());
        }
        return common::Result<common::ControllerProgramRunState>::success(state);
    }

    common::Result<void> slowStopMotion() override {
        ++slow_stop_calls;
        return common::Result<void>::success();
    }

    common::Result<bool> isControllerProgramSlotAvailable(int) override {
        ++program_slot_query_calls;
        return common::Result<bool>::success(controller_program_slot_available);
    }
};

} // namespace rm_control::tests
