#include "rm_control/core/arm_o6_trajectory_task.h"
#include "rm_control/modules/trajectory/reversed_vendor_trajectory_store.h"

#include "mocks/mock_dexterous_hand.h"
#include "mocks/mock_robot_arm.h"

#include <chrono>
#include <cmath>
#include <cstddef>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <string>
#include <system_error>
#include <vector>

namespace {

struct TestContext {
    int failures = 0;

    void Expect(bool condition, const std::string &message) {
        if (!condition) {
            ++failures;
            std::cerr << "[FAIL] " << message << '\n';
        }
    }
};

const std::string &ValidTrajectoryJsonLines() {
    static const std::string content =
        "{\"point\":[0,0,0,0,0,0,0]}\n"
        "{\"point\":[100,0,0,0,0,0,0]}\n"
        "{\"point\":[200,0,0,0,0,0,0]}\n";
    return content;
}

std::filesystem::path TestOutputRoot() {
    static const std::filesystem::path output_root = [] {
        const auto unique_suffix =
            std::chrono::steady_clock::now().time_since_epoch().count();
        const std::filesystem::path path = std::filesystem::temp_directory_path() /
            ("rm_arm_o6_trajectory_task_tests_" + std::to_string(unique_suffix));
        std::error_code filesystem_error;
        std::filesystem::create_directories(path, filesystem_error);
        if (filesystem_error) {
            std::cerr << "[FAIL] failed to create task test output: "
                      << filesystem_error.message() << '\n';
        }
        return path;
    }();
    return output_root;
}

rm_control::modules::trajectory::VendorDragTrajectoryParseResult CreateTrajectoryFixture(
    const std::string &filename,
    bool make_read_only,
    const std::string &replacement_after_parse = {}) {
    const std::filesystem::path path = TestOutputRoot() / filename;
    {
        std::ofstream output(path, std::ios::binary | std::ios::trunc);
        output << ValidTrajectoryJsonLines();
    }
    const auto parsed =
        rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(path, 100);
    if (!parsed.ok) {
        std::cerr << "[FAIL] failed to parse task trajectory fixture: "
                  << parsed.error_message << '\n';
        return {};
    }
    if (!replacement_after_parse.empty()) {
        std::ofstream replacement(path, std::ios::binary | std::ios::trunc);
        replacement << replacement_after_parse;
    }
    if (make_read_only) {
        std::error_code filesystem_error;
        const auto read_only_permissions = std::filesystem::perms::owner_read |
            std::filesystem::perms::group_read | std::filesystem::perms::others_read;
        std::filesystem::permissions(
            path, read_only_permissions, std::filesystem::perm_options::replace,
            filesystem_error);
        if (filesystem_error) {
            std::cerr << "[FAIL] failed to make task fixture read-only: "
                      << filesystem_error.message() << '\n';
        }
    }
    return parsed.value;
}

rm_control::modules::trajectory::VendorDragTrajectoryParseResult MakeTrajectory() {
    static const auto trajectory =
        CreateTrajectoryFixture("valid_read_only_trajectory.jsonl", true);
    return trajectory;
}

rm_control::modules::trajectory::VendorDragTrajectoryParseResult
MakeReturnTrajectory() {
    static const auto return_trajectory = [] {
        const auto created = rm_control::modules::trajectory::
            CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
                MakeTrajectory(), TestOutputRoot() / "return_trajectories");
        if (!created.ok) {
            std::cerr << "[FAIL] failed to create return trajectory fixture: "
                      << created.error_message << '\n';
            return rm_control::modules::trajectory::
                VendorDragTrajectoryParseResult{};
        }
        return created.value;
    }();
    return return_trajectory;
}

rm_control::core::ArmO6TrajectoryTaskConfig MakeTaskConfig() {
    rm_control::core::ArmO6TrajectoryTaskConfig config;
    config.validation_limits.maximum_joint_jump_deg = 1.0;
    config.validation_limits.maximum_joint_velocity_deg_per_second = 10.0;
    config.validation_limits.maximum_joint_acceleration_deg_per_second_squared = 100.0;
    config.origin_speed_percent = 5;
    config.plan_speed_percent = 5;
    config.controller_program_id = 1;
    config.grip_pose = {140U, 100U, 55U, 55U, 55U, 55U};
    config.grip_speed_commands = {64U, 64U, 64U, 64U, 64U, 64U};
    config.grip_torque_commands = {80U, 80U, 80U, 80U, 80U, 80U};
    config.maximum_o6_speed_command = 64U;
    config.maximum_o6_torque_command = 80U;
    config.maximum_origin_delta_deg = 5.0;
    config.origin_position_tolerance_deg = 0.5;
    config.program_status_poll_period = std::chrono::milliseconds(0);
    config.program_execution_timeout = std::chrono::milliseconds(50);
    config.hand_status_poll_period = std::chrono::milliseconds(0);
    config.hand_motion_timeout = std::chrono::milliseconds(0);
    return config;
}

rm_control::core::HardwareExecutionGateInput MakeAllowedGate() {
    rm_control::core::HardwareExecutionGateInput gate;
    gate.live_requested = true;
    gate.hardware_execution_enabled = true;
    gate.arm_connected = true;
    gate.hand_connected = true;
    gate.emergency_stop_verified = true;
    gate.safety_limits_loaded = true;
    gate.operator_confirmed = true;
    gate.trajectory_validated = true;
    gate.trajectory_sample_period_confirmed = true;
    gate.vendor_raw_units_per_degree_confirmed = true;
    gate.workspace_and_virtual_wall_verified = true;
    gate.configured_max_speed_percent = 10;
    return gate;
}

void MakeRobotSafetyArraysExplicit(rm_control::tests::MockRobotArm *robot) {
    robot->safety_status.joint_enable_states.fill(1);
    robot->safety_status.joint_error_codes.fill(0);
    robot->safety_status.arm_error_codes.clear();
    robot->safety_status.controller_error_code = 0;
}

rm_control::interfaces::DexterousHandReadiness MakeReadyHand(
    const rm_control::interfaces::O6JointPose &positions) {
    rm_control::interfaces::DexterousHandReadiness readiness;
    readiness.ready = true;
    readiness.device_id = 0x27U;
    readiness.freedom = 6U;
    readiness.direction = 'R';
    readiness.joint_positions = positions;
    return readiness;
}

class SequencedDexterousHand final : public rm_control::interfaces::IDexterousHand {
public:
    std::vector<rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>> readiness_results;
    rm_control::common::Result<void> command_result =
        rm_control::common::Result<void>::success();
    bool *stop_requested_after_command = nullptr;
    int readiness_calls = 0;
    int command_calls = 0;
    rm_control::interfaces::O6MotionCommand last_motion_command{};

    rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
    ReadReadiness() override {
        ++readiness_calls;
        if (readiness_results.empty()) {
            return rm_control::common::Result<
                rm_control::interfaces::DexterousHandReadiness>::failure(
                    "sequenced readiness exhausted");
        }
        const auto result = readiness_results.front();
        if (readiness_results.size() > 1U) {
            readiness_results.erase(readiness_results.begin());
        }
        return result;
    }

    rm_control::common::Result<void> CommandMotion(
        const rm_control::interfaces::O6MotionCommand &command) override {
        ++command_calls;
        last_motion_command = command;
        if (stop_requested_after_command != nullptr) {
            *stop_requested_after_command = true;
        }
        return command_result;
    }
};

void TestCompletesTrajectoryThenCommandsAndVerifiesGrip(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    SequencedDexterousHand hand;
    const auto config = MakeTaskConfig();
    const rm_control::interfaces::O6JointPose open_pose{};
    hand.readiness_results = {
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(open_pose)),
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(open_pose)),
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(config.grip_pose)),
    };
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(result.ok, "valid Mock trajectory and O6 task should complete");
    context->Expect(robot.move_joint_calls == 1, "task should move to trajectory origin once");
    context->Expect(!robot.last_move_joint_blocking,
                    "origin move must be nonblocking so stop requests remain observable");
    context->Expect(robot.program_slot_query_calls == 1,
                    "task should prove the controller program slot is free before moveJ");
    context->Expect(robot.execute_vendor_file_calls == 1,
                    "task should execute the vendor trajectory once");
    context->Expect(robot.program_state_calls == 2,
                    "task should observe running followed by completed state");
    context->Expect(hand.command_calls == 1, "task should command O6 only after trajectory end");
    context->Expect(hand.last_motion_command.target_positions == config.grip_pose,
                    "O6 command should preserve all six target positions");
    context->Expect(hand.last_motion_command.joint_speed_commands ==
                        config.grip_speed_commands,
                    "O6 command should preserve all six speed commands");
    context->Expect(hand.last_motion_command.joint_torque_commands ==
                        config.grip_torque_commands,
                    "O6 command should preserve all six torque commands");
    context->Expect(robot.slow_stop_calls == 0, "successful task should not request slow stop");
    if (result.ok) {
        context->Expect(result.value.observed_running_state,
                        "success report must prove controller running state was observed");
        context->Expect(result.value.final_program_state.raw_state == 0,
                        "success report controller state must be completed");
        const auto &timing = result.value.controller_timing;
        context->Expect(timing.poll_count == 2U,
                        "controller timing report should count both state polls");
        context->Expect(timing.minimum_poll_period_ms <= timing.average_poll_period_ms &&
                            timing.average_poll_period_ms <= timing.maximum_poll_period_ms,
                        "controller timing report periods should remain ordered");
        context->Expect(timing.maximum_absolute_poll_jitter_ms >= 0.0 &&
                            timing.observed_execution_duration_ms >= 0.0,
                        "controller timing report durations should be nonnegative");
    }
}

void ConfigureSuccessfulRoundTripRobot(
    rm_control::tests::MockRobotArm *robot,
    const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
    const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory) {
    MakeRobotSafetyArraysExplicit(robot);
    robot->arm_state.joints_deg = return_trajectory.samples.front().joints_deg;
    robot->program_run_states = {
        rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
        rm_control::common::ControllerProgramRunState{1, 2, 0, 5},
        rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
        rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
        rm_control::common::ControllerProgramRunState{1, 1, 0, 5},
        rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
    };
    robot->completed_program_joint_targets = {
        return_trajectory.samples.back().joints_deg,
        forward_trajectory.samples.back().joints_deg,
    };
}

void ConfigureSuccessfulRoundTripHand(
    SequencedDexterousHand *hand,
    const rm_control::core::ArmO6TrajectoryTaskConfig &config) {
    const rm_control::interfaces::O6JointPose open_pose{};
    hand->readiness_results = {
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(open_pose)),
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(open_pose)),
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(config.grip_pose)),
    };
}

rm_control::core::ArmO6TrajectoryTaskConfig MakeRoundTripTaskConfig() {
    auto config = MakeTaskConfig();
    config.return_controller_program_id = 2;
    config.origin_position_tolerance_deg = 0.05;
    return config;
}

void TestRoundTripReturnsThenReplaysBeforeGrip(TestContext *context) {
    const auto forward_trajectory = MakeTrajectory();
    const auto return_trajectory = MakeReturnTrajectory();
    rm_control::tests::MockRobotArm robot;
    ConfigureSuccessfulRoundTripRobot(
        &robot, return_trajectory, forward_trajectory);
    SequencedDexterousHand hand;
    const auto config = MakeRoundTripTaskConfig();
    ConfigureSuccessfulRoundTripHand(&hand, config);
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.RunFromTrajectoryEnd(
        return_trajectory, forward_trajectory, config, MakeAllowedGate());

    context->Expect(result.ok, "valid reverse and forward trajectories should complete");
    context->Expect(robot.move_joint_calls == 0,
                    "round trip must not use a large direct moveJ to the origin");
    context->Expect(robot.execute_vendor_file_calls == 0,
                    "round trip must not use upload-and-immediately-run compatibility API");
    context->Expect(robot.save_vendor_program_calls == 2 &&
                        robot.run_controller_program_calls == 2,
                    "round trip should save two programs before running two legs");
    context->Expect(robot.vendor_program_save_requests.size() == 2U &&
                        robot.vendor_program_save_requests[0].controller_program_id == 2 &&
                        robot.vendor_program_save_requests[1].controller_program_id == 1,
                    "return program must be saved before the forward program with distinct ids");
    context->Expect(robot.controller_program_run_requests.size() == 2U &&
                        robot.controller_program_run_requests[0].controller_program_id == 2 &&
                        robot.controller_program_run_requests[1].controller_program_id == 1 &&
                        !robot.controller_program_run_requests[0].blocking &&
                        !robot.controller_program_run_requests[1].blocking,
                    "return then forward programs must run nonblocking in strict order");
    context->Expect(robot.program_state_calls == 6,
                    "both legs may observe idle before running and must still prove completion");
    context->Expect(hand.command_calls == 1,
                    "O6 grip must be commanded only after both controller legs complete");
    context->Expect(robot.slow_stop_calls == 0,
                    "successful round trip must not request slow stop");
    if (result.ok) {
        context->Expect(result.value.returned_from_trajectory_end &&
                            result.value.return_observed_running_state &&
                            result.value.observed_running_state,
                        "round-trip report must prove both running states were observed");
        context->Expect(result.value.return_final_program_state.raw_state == 0 &&
                            result.value.final_program_state.raw_state == 0,
                        "round-trip report must contain two completed controller states");
    }
}

void TestRoundTripSettlesSmallReturnEndpointErrorBeforeForwardReplay(
    TestContext *context) {
    const auto forward_trajectory = MakeTrajectory();
    const auto return_trajectory = MakeReturnTrajectory();
    rm_control::tests::MockRobotArm robot;
    ConfigureSuccessfulRoundTripRobot(
        &robot, return_trajectory, forward_trajectory);
    auto drifted_forward_origin = forward_trajectory.samples.front().joints_deg;
    drifted_forward_origin[2] += 0.08F;
    robot.completed_program_joint_targets = {
        drifted_forward_origin,
        forward_trajectory.samples.back().joints_deg,
    };
    SequencedDexterousHand hand;
    const auto config = MakeRoundTripTaskConfig();
    ConfigureSuccessfulRoundTripHand(&hand, config);
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.RunFromTrajectoryEnd(
        return_trajectory, forward_trajectory, config, MakeAllowedGate());

    context->Expect(result.ok,
                    "small return endpoint error should be settled before forward replay");
    context->Expect(robot.move_joint_calls == 1,
                    "round trip should use one low-speed correction moveJ");
    context->Expect(!robot.last_move_joint_blocking,
                    "correction moveJ must be nonblocking for stop observability");
    context->Expect(robot.arm_state.joints_deg == forward_trajectory.samples.back().joints_deg,
                    "successful forward replay should still finish at the forward end point");
    context->Expect(robot.run_controller_program_calls == 2,
                    "settled round trip must still run both controller programs");
    context->Expect(hand.command_calls == 1,
                    "O6 grip must remain after the corrected forward trajectory");
}

void TestRoundTripRejectsCurrentStateAwayFromTrajectoryEnd(TestContext *context) {
    const auto forward_trajectory = MakeTrajectory();
    const auto return_trajectory = MakeReturnTrajectory();
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    robot.arm_state.joints_deg = forward_trajectory.samples[1].joints_deg;
    rm_control::tests::MockDexterousHand hand;
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.RunFromTrajectoryEnd(
        return_trajectory, forward_trajectory, MakeRoundTripTaskConfig(),
        MakeAllowedGate());

    context->Expect(!result.ok,
                    "round trip must reject a robot that is not at the original end point");
    context->Expect(robot.save_vendor_program_calls == 0 &&
                        robot.run_controller_program_calls == 0 &&
                        robot.move_joint_calls == 0,
                    "endpoint mismatch must fail before every upload and motion command");
    context->Expect(hand.command_calls == 0,
                    "endpoint mismatch must prevent O6 grip");
}

void TestRoundTripForwardSaveFailurePreventsMotion(TestContext *context) {
    const auto forward_trajectory = MakeTrajectory();
    const auto return_trajectory = MakeReturnTrajectory();
    rm_control::tests::MockRobotArm robot;
    ConfigureSuccessfulRoundTripRobot(
        &robot, return_trajectory, forward_trajectory);
    robot.save_vendor_program_results = {
        rm_control::common::Result<void>::success(),
        rm_control::common::Result<void>::failure("mock forward save failure"),
    };
    rm_control::tests::MockDexterousHand hand;
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.RunFromTrajectoryEnd(
        return_trajectory, forward_trajectory, MakeRoundTripTaskConfig(),
        MakeAllowedGate());

    context->Expect(!result.ok, "forward program save failure must fail closed");
    context->Expect(robot.save_vendor_program_calls == 2 &&
                        robot.run_controller_program_calls == 0,
                    "both saves may be attempted but no program may run after save failure");
    context->Expect(robot.slow_stop_calls == 0,
                    "save-only failure before motion should not issue an unnecessary slow stop");
    context->Expect(hand.command_calls == 0,
                    "save failure must prevent O6 grip");
}

void TestRoundTripPausedReturnProgramStopsBeforeForward(TestContext *context) {
    const auto forward_trajectory = MakeTrajectory();
    const auto return_trajectory = MakeReturnTrajectory();
    rm_control::tests::MockRobotArm robot;
    ConfigureSuccessfulRoundTripRobot(
        &robot, return_trajectory, forward_trajectory);
    robot.program_run_states = {
        rm_control::common::ControllerProgramRunState{1, 2, 0, 5},
        rm_control::common::ControllerProgramRunState{2, 2, 0, 5},
    };
    rm_control::tests::MockDexterousHand hand;
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.RunFromTrajectoryEnd(
        return_trajectory, forward_trajectory, MakeRoundTripTaskConfig(),
        MakeAllowedGate());

    context->Expect(!result.ok, "paused return program must fail the round trip");
    context->Expect(robot.run_controller_program_calls == 1 &&
                        robot.slow_stop_calls == 1,
                    "paused return must slow-stop and prevent the forward program");
    context->Expect(hand.command_calls == 0,
                    "paused return must prevent O6 grip");
}

void TestOccupiedProgramSlotPreventsOriginMotion(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    robot.controller_program_slot_available = false;
    rm_control::tests::MockDexterousHand hand;
    const auto config = MakeTaskConfig();
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(
            MakeReadyHand(config.grip_pose));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "occupied controller program slot must fail closed");
    context->Expect(robot.program_slot_query_calls == 1,
                    "program slot availability should be queried exactly once in preflight");
    context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
                    "occupied controller slot must be rejected before RM75 moves");
    context->Expect(hand.command_calls == 0,
                    "occupied controller slot must prevent the O6 command");
}

void TestWritableSourceFileIsRejectedBeforeOriginMotion(TestContext *context) {
    const auto trajectory =
        CreateTrajectoryFixture("writable_trajectory.jsonl", false);
    context->Expect(trajectory.samples.size() == 3U,
                    "writable source fixture should contain valid parsed metadata");
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    rm_control::tests::MockDexterousHand hand;
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(trajectory, MakeTaskConfig(), MakeAllowedGate());

    context->Expect(!result.ok, "writable Live trajectory source must be rejected");
    context->Expect(result.error_message.find("read-only") != std::string::npos,
                    "writable source rejection should identify its permission problem");
    context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
                    "writable source must be rejected before moveJ or upload");
}

void TestTamperedReadOnlySourceFileIsRejectedBeforeOriginMotion(TestContext *context) {
    std::string tampered_content = ValidTrajectoryJsonLines();
    const std::size_t value_position = tampered_content.find("100");
    if (value_position != std::string::npos) {
        tampered_content[value_position + 2U] = '1';
    }
    const auto trajectory = CreateTrajectoryFixture(
        "tampered_read_only_trajectory.jsonl", true, tampered_content);
    context->Expect(trajectory.samples.size() == 3U,
                    "tampered source fixture should retain original parsed metadata");
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    rm_control::tests::MockDexterousHand hand;
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(trajectory, MakeTaskConfig(), MakeAllowedGate());

    context->Expect(!result.ok, "tampered read-only trajectory source must be rejected");
    context->Expect(result.error_message.find("SHA-256") != std::string::npos,
                    "same-size source tampering should be detected by SHA-256");
    context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
                    "tampered source must be rejected before moveJ or upload");
}

void TestGateRejectionPreventsEveryMotionCommand(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    rm_control::tests::MockDexterousHand hand;
    auto gate = MakeAllowedGate();
    gate.operator_confirmed = false;
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), MakeTaskConfig(), gate);

    context->Expect(!result.ok, "missing operator confirmation must reject task");
    context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
                    "gate rejection must occur before any RM75 motion command");
    context->Expect(hand.command_calls == 0,
                    "gate rejection must occur before any O6 command");
}

void TestPausedProgramStateSlowStopsAndPreventsGrip(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    robot.program_run_states = {
        rm_control::common::ControllerProgramRunState{1, 1, 0, 5},
        rm_control::common::ControllerProgramRunState{2, 1, 0, 5},
    };
    rm_control::tests::MockDexterousHand hand;
    const auto config = MakeTaskConfig();
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(
            MakeReadyHand(config.grip_pose));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "unexpected paused controller state must fail closed");
    context->Expect(robot.slow_stop_calls == 1,
                    "program state failure should request RM75 slow stop");
    context->Expect(hand.command_calls == 0,
                    "O6 must not move when trajectory completion was not proven");
}

void TestWrongProgramIdentityOrUnsafeSpeedSlowStops(TestContext *context) {
    const auto run_case = [context](int program_id,
                                    int plan_speed_percent,
                                    const std::string &description) {
        rm_control::tests::MockRobotArm robot;
        MakeRobotSafetyArraysExplicit(&robot);
        robot.program_run_states = {
            rm_control::common::ControllerProgramRunState{
                1, program_id, 0, plan_speed_percent},
        };
        rm_control::tests::MockDexterousHand hand;
        const auto config = MakeTaskConfig();
        hand.readiness_result = rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(config.grip_pose));
        rm_control::core::ArmO6TrajectoryTask task(robot, hand);

        const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

        context->Expect(!result.ok, description + " must fail closed");
        context->Expect(robot.slow_stop_calls == 1,
                        description + " must request RM75 slow stop");
        context->Expect(hand.command_calls == 0,
                        description + " must prevent the O6 command");
    };

    run_case(2, 5, "mismatched controller program id");
    run_case(1, 6, "controller plan speed above configured request");
}

void TestPostTrajectoryHandReadinessFailurePreventsGrip(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    SequencedDexterousHand hand;
    const auto config = MakeTaskConfig();
    hand.readiness_results = {
        rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>::success(
            MakeReadyHand(config.grip_pose)),
        rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>::failure(
            "mock O6 disappeared after trajectory"),
    };
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "post-trajectory O6 readiness failure must fail task");
    context->Expect(robot.execute_vendor_file_calls == 1,
                    "readiness failure should be exercised after completed trajectory");
    context->Expect(robot.slow_stop_calls == 1,
                    "post-trajectory readiness failure should request slow stop");
    context->Expect(hand.command_calls == 0,
                    "failed O6 readiness must prevent the grip command");
}

void TestPreGripRobotSafetyRecheckFailurePreventsGrip(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    robot.fail_current_state_on_or_after_call = 3;
    rm_control::tests::MockDexterousHand hand;
    const auto config = MakeTaskConfig();
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(
            MakeReadyHand(config.grip_pose));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "failed pre-grip RM75 state recheck must fail task");
    context->Expect(robot.execute_vendor_file_calls == 1,
                    "RM75 state failure should occur after the trajectory completed");
    context->Expect(robot.current_state_calls == 3,
                    "task should re-read RM75 state immediately before O6 grip");
    context->Expect(robot.slow_stop_calls == 1,
                    "failed pre-grip RM75 recheck should request slow stop");
    context->Expect(hand.command_calls == 0,
                    "failed pre-grip RM75 recheck must prevent the O6 command");
}

void ExpectInvalidO6ConfigRejected(
    TestContext *context,
    const rm_control::core::ArmO6TrajectoryTaskConfig &config,
    const std::string &description) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    rm_control::tests::MockDexterousHand hand;
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto validation =
        rm_control::core::ValidateArmO6TrajectoryTaskConfig(config);
    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!validation.ok,
                    description + " must be rejected by the public pure validator");
    context->Expect(!result.ok, description + " must fail closed");
    context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
                    description + " must fail before RM75 motion");
    context->Expect(hand.command_calls == 0,
                    description + " must fail before the O6 command");
}

void TestRejectsO6ProtocolRangeViolations(TestContext *context) {
    auto invalid_position = MakeTaskConfig();
    invalid_position.grip_pose[1] = 256U;
    ExpectInvalidO6ConfigRejected(context, invalid_position, "O6 position above 255");

    auto invalid_speed = MakeTaskConfig();
    invalid_speed.grip_speed_commands[2] = 256U;
    ExpectInvalidO6ConfigRejected(context, invalid_speed, "O6 speed above 255");

    auto invalid_torque = MakeTaskConfig();
    invalid_torque.grip_torque_commands[3] = 256U;
    ExpectInvalidO6ConfigRejected(context, invalid_torque, "O6 torque above 255");
}

void TestAcceptsO6CommandsAtConfiguredMaximum(TestContext *context) {
    const auto validation =
        rm_control::core::ValidateArmO6TrajectoryTaskConfig(MakeTaskConfig());

    context->Expect(validation.ok,
                    "O6 speed and torque equal to configured maxima should be valid");
}

void TestRejectsO6ConfiguredMaximumViolations(TestContext *context) {
    auto speed_above_configured_maximum = MakeTaskConfig();
    speed_above_configured_maximum.grip_speed_commands[4] = 65U;
    ExpectInvalidO6ConfigRejected(
        context, speed_above_configured_maximum, "O6 speed above configured maximum");

    auto torque_above_configured_maximum = MakeTaskConfig();
    torque_above_configured_maximum.grip_torque_commands[5] = 81U;
    ExpectInvalidO6ConfigRejected(
        context, torque_above_configured_maximum, "O6 torque above configured maximum");
}

void TestRejectsO6MaximumOutsideProtocolRange(TestContext *context) {
    auto invalid_speed_maximum = MakeTaskConfig();
    invalid_speed_maximum.maximum_o6_speed_command = 256U;
    ExpectInvalidO6ConfigRejected(
        context, invalid_speed_maximum, "configured O6 speed maximum above 255");

    auto invalid_torque_maximum = MakeTaskConfig();
    invalid_torque_maximum.maximum_o6_torque_command = 256U;
    ExpectInvalidO6ConfigRejected(
        context, invalid_torque_maximum, "configured O6 torque maximum above 255");
}

void TestO6MotionWriteFailureSlowStops(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    SequencedDexterousHand hand;
    const auto config = MakeTaskConfig();
    const rm_control::interfaces::O6JointPose open_pose{};
    hand.readiness_results = {
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(open_pose)),
    };
    hand.command_result = rm_control::common::Result<void>::failure(
        "mock RS485 write failure");
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "O6 motion write failure must fail the task");
    context->Expect(hand.command_calls == 1,
                    "O6 motion write failure should occur on one composite command");
    context->Expect(robot.slow_stop_calls == 1,
                    "O6 motion write failure should request RM75 slow stop");
}

void TestO6GripReadbackTimeoutSlowStops(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    SequencedDexterousHand hand;
    auto config = MakeTaskConfig();
    config.hand_motion_timeout = std::chrono::milliseconds(0);
    const rm_control::interfaces::O6JointPose open_pose{};
    hand.readiness_results = {
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(open_pose)),
    };
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "unchanged O6 readback must time out");
    context->Expect(result.error_message.find("timeout") != std::string::npos,
                    "O6 readback timeout should preserve timeout context");
    context->Expect(hand.command_calls == 1 && hand.readiness_calls == 3,
                    "timeout path should read open, command once, then read open again");
    context->Expect(robot.slow_stop_calls == 1,
                    "O6 readback timeout should request RM75 slow stop");
}

void TestStopRequestDuringProgramMonitorSlowStops(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    rm_control::tests::MockDexterousHand hand;
    const auto ready = MakeReadyHand({});
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(ready);
    auto config = MakeTaskConfig();
    int stop_callback_calls = 0;
    config.stop_requested = [&stop_callback_calls] {
        ++stop_callback_calls;
        return stop_callback_calls >= 6;
    };
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "stop request during controller monitor must fail task");
    context->Expect(robot.program_state_calls == 1 && robot.slow_stop_calls == 1,
                    "monitor stop request should stop before the next poll and slow-stop RM75");
    context->Expect(hand.command_calls == 0,
                    "monitor stop request must prevent the O6 command");
}

void TestStopRequestDuringGripReadbackSlowStops(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    SequencedDexterousHand hand;
    const auto config_without_callback = MakeTaskConfig();
    const rm_control::interfaces::O6JointPose open_pose{};
    hand.readiness_results = {
        rm_control::common::Result<
            rm_control::interfaces::DexterousHandReadiness>::success(
                MakeReadyHand(open_pose)),
    };
    bool stop_requested = false;
    hand.stop_requested_after_command = &stop_requested;
    auto config = config_without_callback;
    config.stop_requested = [&stop_requested] { return stop_requested; };
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "stop request during O6 readback must fail task");
    context->Expect(hand.command_calls == 1 && hand.readiness_calls == 2,
                    "grip stop request should occur after command and before readback poll");
    context->Expect(robot.slow_stop_calls == 1,
                    "grip readback stop request should request RM75 slow stop");
}

void TestStopRequestDuringOriginMotionStopsBeforeUpload(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    robot.apply_move_joint_target_immediately = false;
    rm_control::tests::MockDexterousHand hand;
    auto config = MakeTaskConfig();
    config.origin_status_poll_period = std::chrono::milliseconds(0);
    int stop_callback_calls = 0;
    config.stop_requested = [&stop_callback_calls] {
        ++stop_callback_calls;
        return stop_callback_calls >= 3;
    };
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(!result.ok, "stop request during origin motion must fail the task");
    context->Expect(robot.move_joint_calls == 1 && robot.execute_vendor_file_calls == 0,
                    "origin stop must occur after move start and before trajectory upload");
    context->Expect(robot.slow_stop_calls == 1,
                    "origin stop request must trigger one RM75 slow stop");
}

void TestControllerTimingReportMeasuresConfiguredPoll(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    MakeRobotSafetyArraysExplicit(&robot);
    rm_control::tests::MockDexterousHand hand;
    auto config = MakeTaskConfig();
    config.program_status_poll_period = std::chrono::milliseconds(2);
    hand.readiness_result = rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::success(
            MakeReadyHand(config.grip_pose));
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);

    const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());

    context->Expect(result.ok, "timed controller monitor should complete with Mock states");
    if (!result.ok) {
        return;
    }
    const auto &timing = result.value.controller_timing;
    const double expected_jitter = std::fabs(timing.average_poll_period_ms - 2.0);
    context->Expect(timing.poll_count == 2U && timing.average_poll_period_ms > 0.0,
                    "timing report should contain one positive measured poll interval");
    context->Expect(timing.minimum_poll_period_ms == timing.average_poll_period_ms &&
                        timing.average_poll_period_ms == timing.maximum_poll_period_ms,
                    "one measured interval should have identical min, max, and average");
    context->Expect(std::fabs(timing.maximum_absolute_poll_jitter_ms - expected_jitter) < 1e-9,
                    "timing report jitter should be absolute deviation from configured poll");
    context->Expect(timing.observed_execution_duration_ms >= timing.average_poll_period_ms,
                    "observed execution duration should cover the measured poll interval");
}

} // namespace

int main() {
    TestContext context;
    TestCompletesTrajectoryThenCommandsAndVerifiesGrip(&context);
    TestRoundTripReturnsThenReplaysBeforeGrip(&context);
    TestRoundTripSettlesSmallReturnEndpointErrorBeforeForwardReplay(&context);
    TestRoundTripRejectsCurrentStateAwayFromTrajectoryEnd(&context);
    TestRoundTripForwardSaveFailurePreventsMotion(&context);
    TestRoundTripPausedReturnProgramStopsBeforeForward(&context);
    TestOccupiedProgramSlotPreventsOriginMotion(&context);
    TestWritableSourceFileIsRejectedBeforeOriginMotion(&context);
    TestTamperedReadOnlySourceFileIsRejectedBeforeOriginMotion(&context);
    TestGateRejectionPreventsEveryMotionCommand(&context);
    TestPausedProgramStateSlowStopsAndPreventsGrip(&context);
    TestWrongProgramIdentityOrUnsafeSpeedSlowStops(&context);
    TestPostTrajectoryHandReadinessFailurePreventsGrip(&context);
    TestPreGripRobotSafetyRecheckFailurePreventsGrip(&context);
    TestRejectsO6ProtocolRangeViolations(&context);
    TestAcceptsO6CommandsAtConfiguredMaximum(&context);
    TestRejectsO6ConfiguredMaximumViolations(&context);
    TestRejectsO6MaximumOutsideProtocolRange(&context);
    TestO6MotionWriteFailureSlowStops(&context);
    TestO6GripReadbackTimeoutSlowStops(&context);
    TestStopRequestDuringProgramMonitorSlowStops(&context);
    TestStopRequestDuringGripReadbackSlowStops(&context);
    TestStopRequestDuringOriginMotionStopsBeforeUpload(&context);
    TestControllerTimingReportMeasuresConfiguredPoll(&context);

    if (context.failures == 0) {
        std::cout << "[PASS] rm_arm_o6_trajectory_task_tests\n";
        return 0;
    }
    std::cerr << context.failures << " test(s) failed\n";
    return 1;
}
