#include "rm_control/core/arm_o6_trajectory_task.h"

#include "rm_control/common/rm75_constants.h"
#include "rm_control/modules/trajectory/sha256_file_checksum.h"

#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <filesystem>
#include <optional>
#include <sstream>
#include <string>
#include <system_error>
#include <thread>

namespace rm_control::core {
namespace {

constexpr int kMinimumSpeedPercent = 1;
constexpr int kMaximumSpeedPercent = 100;
constexpr int kMinimumControllerProgramId = 1;
constexpr int kMaximumControllerProgramId = 100;
constexpr std::uint16_t kMaximumO6CommandValue = 255U;

struct RobotPreflightSnapshot {
    common::RobotJointLimits joint_limits;
    common::ArmState arm_state;
};

struct ProgramMonitorResult {
    common::ControllerProgramRunState final_state;
    ControllerProgramTimingReport timing;
    bool observed_running_state = false;
};

struct ProgramTimingAccumulator {
    std::chrono::steady_clock::time_point monitor_started_at;
    std::optional<std::chrono::steady_clock::time_point> previous_poll_started_at;
    ControllerProgramTimingReport report;
    double total_poll_period_ms = 0.0;
    std::size_t poll_period_count = 0U;
};

struct TaskPreparation {
    modules::trajectory::DragTrajectoryValidationReport trajectory_validation;
    RobotPreflightSnapshot robot_preflight;
};

struct RoundTripTaskPreparation {
    modules::trajectory::DragTrajectoryValidationReport forward_validation;
    RobotPreflightSnapshot robot_preflight;
};

struct ControllerProgramLegRequest {
    const modules::trajectory::VendorDragTrajectoryParseResult *trajectory = nullptr;
    int controller_program_id = 0;
    const char *phase_name = nullptr;
};

enum class TrajectoryEndpointCompletionMode {
    StrictValidation,
    SettleWithLowSpeedMoveJ,
};

common::Result<std::uintmax_t> InspectReadOnlyRegularSourceFile(
    const std::filesystem::path &path) {
    std::error_code filesystem_error;
    const std::filesystem::file_status status =
        std::filesystem::symlink_status(path, filesystem_error);
    if (filesystem_error) {
        return common::Result<std::uintmax_t>::failure(
            "failed to inspect trajectory source " + path.string() + ": " +
            filesystem_error.message());
    }
    if (std::filesystem::is_symlink(status) || !std::filesystem::is_regular_file(status)) {
        return common::Result<std::uintmax_t>::failure(
            "trajectory source must be a non-symlink regular file: " + path.string());
    }
    constexpr std::filesystem::perms kAllWritePermissions =
        std::filesystem::perms::owner_write | std::filesystem::perms::group_write |
        std::filesystem::perms::others_write;
    if (status.permissions() == std::filesystem::perms::unknown ||
        (status.permissions() & kAllWritePermissions) != std::filesystem::perms::none) {
        return common::Result<std::uintmax_t>::failure(
            "trajectory source must be read-only for owner, group, and others: " +
            path.string());
    }
    const std::uintmax_t file_size = std::filesystem::file_size(path, filesystem_error);
    if (filesystem_error) {
        return common::Result<std::uintmax_t>::failure(
            "failed to read trajectory source size " + path.string() + ": " +
            filesystem_error.message());
    }
    return common::Result<std::uintmax_t>::success(file_size);
}

common::Result<void> ValidateO6CommandValues(
    const interfaces::O6JointPose &commands,
    std::uint16_t configured_maximum,
    const std::string &command_name) {
    if (configured_maximum > kMaximumO6CommandValue) {
        return common::Result<void>::failure(
            "configured maximum " + command_name + " exceeds O6 protocol range 0..255");
    }
    for (std::size_t joint = 0U; joint < commands.size(); ++joint) {
        if (commands[joint] > kMaximumO6CommandValue ||
            commands[joint] > configured_maximum) {
            return common::Result<void>::failure(
                "O6 " + command_name + " at joint " + std::to_string(joint) +
                " must be in range 0.." + std::to_string(configured_maximum));
        }
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateO6MotionConfig(const ArmO6TrajectoryTaskConfig &config) {
    const auto position = ValidateO6CommandValues(
        config.grip_pose, kMaximumO6CommandValue, "target position command");
    const auto speed = ValidateO6CommandValues(
        config.grip_speed_commands, config.maximum_o6_speed_command, "speed command");
    const auto torque = ValidateO6CommandValues(
        config.grip_torque_commands, config.maximum_o6_torque_command, "torque command");
    if (!position.ok) {
        return position;
    }
    if (!speed.ok) {
        return speed;
    }
    if (!torque.ok) {
        return torque;
    }
    if (config.grip_position_tolerance > kMaximumO6CommandValue) {
        return common::Result<void>::failure(
            "O6 grip position tolerance must be in range 0..255");
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateTaskConfigImpl(const ArmO6TrajectoryTaskConfig &config) {
    const bool speeds_valid = config.origin_speed_percent >= kMinimumSpeedPercent &&
                              config.origin_speed_percent <= kMaximumSpeedPercent &&
                              config.plan_speed_percent >= kMinimumSpeedPercent &&
                              config.plan_speed_percent <= kMaximumSpeedPercent;
    const bool program_id_valid = config.controller_program_id >= kMinimumControllerProgramId &&
                                  config.controller_program_id <= kMaximumControllerProgramId;
    const bool scalar_limits_valid = std::isfinite(config.maximum_origin_delta_deg) &&
        config.maximum_origin_delta_deg > 0.0 &&
        std::isfinite(config.origin_position_tolerance_deg) &&
        config.origin_position_tolerance_deg > 0.0 &&
        config.origin_position_tolerance_deg <= config.maximum_origin_delta_deg &&
        std::isfinite(config.maximum_absolute_force_newton) &&
        config.maximum_absolute_force_newton > 0.0 &&
        std::isfinite(config.maximum_absolute_torque_newton_meter) &&
        config.maximum_absolute_torque_newton_meter > 0.0;
    const bool timing_valid = config.origin_status_poll_period.count() >= 0 &&
        config.origin_motion_timeout.count() > 0 &&
        config.program_status_poll_period.count() >= 0 &&
        config.program_execution_timeout.count() > 0 &&
        config.hand_status_poll_period.count() >= 0 && config.hand_motion_timeout.count() >= 0;
    const auto o6_motion_check = ValidateO6MotionConfig(config);
    if (!speeds_valid || !program_id_valid || !scalar_limits_valid || !timing_valid ||
        !o6_motion_check.ok) {
        if (!o6_motion_check.ok) {
            return o6_motion_check;
        }
        return common::Result<void>::failure(
            "RM75 O6 task configuration contains an invalid speed, limit, program id, or timeout");
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateRoundTripTaskConfig(
    const ArmO6TrajectoryTaskConfig &config) {
    const auto base_config = ValidateTaskConfigImpl(config);
    if (!base_config.ok) {
        return base_config;
    }
    if (config.return_controller_program_id < kMinimumControllerProgramId ||
        config.return_controller_program_id > kMaximumControllerProgramId) {
        return common::Result<void>::failure(
            "return controller program id must be in range 1..100");
    }
    if (config.return_controller_program_id == config.controller_program_id) {
        return common::Result<void>::failure(
            "return and forward controller program ids must be different");
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateTrajectoryMetadata(
    const modules::trajectory::VendorDragTrajectoryParseResult &trajectory) {
    if (trajectory.source_file.empty() || trajectory.samples.empty() ||
        trajectory.trajectory_point_count != trajectory.samples.size()) {
        return common::Result<void>::failure(
            "parsed vendor trajectory path, samples, and point count are inconsistent");
    }
    if (trajectory.sample_period_ms <= 0 || trajectory.raw_units_per_degree <= 0 ||
        !trajectory.timestamp_is_derived || trajectory.source_file_sha256.empty()) {
        return common::Result<void>::failure(
            "parsed vendor trajectory is missing explicit units, derived timing, or SHA-256");
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateReversedTrajectoryPair(
    const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
    const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory) {
    if (return_trajectory.source_file == forward_trajectory.source_file) {
        return common::Result<void>::failure(
            "return and forward trajectories must use different read-only files");
    }
    if (return_trajectory.trajectory_point_count !=
            forward_trajectory.trajectory_point_count ||
        return_trajectory.sample_period_ms != forward_trajectory.sample_period_ms ||
        return_trajectory.raw_units_per_degree !=
            forward_trajectory.raw_units_per_degree) {
        return common::Result<void>::failure(
            "return trajectory metadata does not match the forward trajectory");
    }
    const std::size_t point_count = forward_trajectory.samples.size();
    for (std::size_t return_index = 0U; return_index < point_count; ++return_index) {
        const std::size_t forward_index = point_count - 1U - return_index;
        if (return_trajectory.samples[return_index].joints_deg !=
            forward_trajectory.samples[forward_index].joints_deg) {
            return common::Result<void>::failure(
                "return trajectory is not the exact point-order reverse at sample " +
                std::to_string(return_index));
        }
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateRobotIdentity(const common::RobotInfo &info) {
    if (!info.is_rm75 || info.arm_dof != static_cast<int>(common::kRm75JointCount)) {
        return common::Result<void>::failure(
            "integrated trajectory task requires an RM75 with exactly seven joints");
    }
    if (!info.six_axis_force_sensor_available) {
        return common::Result<void>::failure(
            "integrated trajectory task requires an available six-axis force sensor");
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateRobotSafetyStatus(const common::RobotSafetyStatus &status) {
    if (!status.real_mode_enabled || !status.arm_power_on || !status.all_joints_enabled) {
        return common::Result<void>::failure(
            "robot safety preflight requires real mode, arm power, and all joints enabled");
    }
    if (!status.arm_error_free || !status.controller_error_free ||
        status.controller_error_code != 0) {
        return common::Result<void>::failure(
            "robot safety preflight found a nonzero arm or controller error");
    }
    // RM_API2 v1.1.5 明确说明虚拟墙和自碰撞只在仿真模式生效；真机任务改由
    // HardwareExecutionGate 的现场工作区确认、实时关节/控制器错误和六维力门控保护。
    if (status.emergency_stop_state_available && !status.emergency_stop_normal) {
        return common::Result<void>::failure(
            "robot safety preflight reports an abnormal emergency-stop state");
    }
    const bool joint_states_safe = std::all_of(
        status.joint_enable_states.begin(), status.joint_enable_states.end(),
        [](int state) { return state == 1; }) &&
        std::all_of(status.joint_error_codes.begin(), status.joint_error_codes.end(),
                    [](int code) { return code == 0; }) &&
        std::all_of(status.arm_error_codes.begin(), status.arm_error_codes.end(),
                    [](int code) { return code == 0; });
    return joint_states_safe
        ? common::Result<void>::success()
        : common::Result<void>::failure(
              "robot safety preflight found a disabled joint or joint error code");
}

common::Result<void> ValidateJointLimitSnapshot(const common::RobotJointLimits &limits) {
    for (std::size_t joint = 0; joint < common::kRm75JointCount; ++joint) {
        const bool finite = std::isfinite(limits.minimum_position_deg[joint]) &&
                            std::isfinite(limits.maximum_position_deg[joint]) &&
                            std::isfinite(limits.maximum_speed_deg_per_second[joint]) &&
                            std::isfinite(
                                limits.maximum_acceleration_deg_per_second_squared[joint]);
        const bool ordered = limits.minimum_position_deg[joint] <
                             limits.maximum_position_deg[joint];
        const bool positive_motion_limits = limits.maximum_speed_deg_per_second[joint] > 0.0F &&
            limits.maximum_acceleration_deg_per_second_squared[joint] > 0.0F;
        if (!finite || !ordered || !positive_motion_limits) {
            return common::Result<void>::failure(
                "invalid controller joint limit snapshot at RM75 joint " +
                std::to_string(joint));
        }
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateJointsWithinLimits(
    const std::vector<float> &joints_deg,
    const common::RobotJointLimits &limits,
    const std::string &context) {
    if (joints_deg.size() != common::kRm75JointCount) {
        return common::Result<void>::failure(context + " must contain exactly seven joints");
    }
    for (std::size_t joint = 0; joint < common::kRm75JointCount; ++joint) {
        if (!std::isfinite(joints_deg[joint]) ||
            joints_deg[joint] < limits.minimum_position_deg[joint] ||
            joints_deg[joint] > limits.maximum_position_deg[joint]) {
            return common::Result<void>::failure(
                context + " exceeds controller position limits at RM75 joint " +
                std::to_string(joint));
        }
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateEveryTrajectoryPointWithinLimits(
    const std::vector<common::TrajectorySample> &samples,
    const common::RobotJointLimits &limits) {
    for (std::size_t sample = 0; sample < samples.size(); ++sample) {
        const auto result = ValidateJointsWithinLimits(
            samples[sample].joints_deg, limits, "trajectory sample " + std::to_string(sample));
        if (!result.ok) {
            return result;
        }
    }
    return common::Result<void>::success();
}

bool PoseIsFinite(const common::Pose &pose) {
    return std::isfinite(pose.x) && std::isfinite(pose.y) && std::isfinite(pose.z) &&
           std::isfinite(pose.rx) && std::isfinite(pose.ry) && std::isfinite(pose.rz);
}

common::Result<void> ValidateArmState(const common::ArmState &state,
                                      const common::RobotJointLimits &limits) {
    const auto joints = ValidateJointsWithinLimits(state.joints_deg, limits, "current arm state");
    if (!joints.ok) {
        return joints;
    }
    const bool errors_clear = std::all_of(
        state.error_codes.begin(), state.error_codes.end(), [](int code) { return code == 0; });
    if (!PoseIsFinite(state.tcp_pose) || !errors_clear) {
        return common::Result<void>::failure(
            "current arm state contains a non-finite TCP pose or nonzero error code");
    }
    return common::Result<void>::success();
}

common::Result<void> ValidateOriginDifference(const common::ArmState &state,
                                              const std::vector<float> &origin_deg,
                                              double maximum_delta_deg,
                                              const std::string &context) {
    for (std::size_t joint = 0; joint < common::kRm75JointCount; ++joint) {
        const double delta = std::fabs(static_cast<double>(state.joints_deg[joint]) -
                                       static_cast<double>(origin_deg[joint]));
        if (delta > maximum_delta_deg) {
            std::ostringstream message;
            message << context << " at RM75 joint " << joint << ": delta=" << delta
                    << " degree, allowed=" << maximum_delta_deg << " degree";
            return common::Result<void>::failure(message.str());
        }
    }
    return common::Result<void>::success();
}

template <std::size_t Size>
bool AllFinite(const std::array<float, Size> &values) {
    return std::all_of(values.begin(), values.end(),
                       [](float value) { return std::isfinite(value); });
}

common::Result<void> ValidateForceData(const common::ForceData &force,
                                       const ArmO6TrajectoryTaskConfig &config) {
    if (!AllFinite(force.raw) || !AllFinite(force.zero) || !AllFinite(force.work_zero) ||
        !AllFinite(force.tool_zero)) {
        return common::Result<void>::failure(
            "six-axis force snapshot contains a non-finite value");
    }
    for (std::size_t component = 0; component < 3U; ++component) {
        if (std::fabs(static_cast<double>(force.zero[component])) >
            config.maximum_absolute_force_newton) {
            return common::Result<void>::failure(
                "external force exceeds the configured newton limit");
        }
    }
    for (std::size_t component = 3U; component < force.zero.size(); ++component) {
        if (std::fabs(static_cast<double>(force.zero[component])) >
            config.maximum_absolute_torque_newton_meter) {
            return common::Result<void>::failure(
                "external torque exceeds the configured newton-meter limit");
        }
    }
    return common::Result<void>::success();
}

common::Result<interfaces::DexterousHandReadiness> ReadReadyHand(
    interfaces::IDexterousHand &hand,
    const std::string &context) {
    const auto readiness = hand.ReadReadiness();
    if (!readiness.ok) {
        return common::Result<interfaces::DexterousHandReadiness>::failure(
            context + ": " + readiness.error_message);
    }
    if (!readiness.value.ready) {
        return common::Result<interfaces::DexterousHandReadiness>::failure(
            context + ": O6 readiness is false");
    }
    return readiness;
}

common::Result<RobotPreflightSnapshot> ReadAndValidatePreflight(
    interfaces::IRobotArm &robot,
    const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
    const ArmO6TrajectoryTaskConfig &config) {
    if (!robot.isConnected()) {
        return common::Result<RobotPreflightSnapshot>::failure(
            "integrated trajectory task requires an active RM75 connection");
    }
    const auto info = robot.getRobotInfo();
    const auto safety = robot.getRobotSafetyStatus();
    const auto limits = robot.getJointLimits();
    const auto state = robot.getCurrentArmState();
    const auto force = robot.getForceData();
    if (!info.ok || !safety.ok || !limits.ok || !state.ok || !force.ok) {
        const std::string error = !info.ok ? info.error_message
            : !safety.ok ? safety.error_message
            : !limits.ok ? limits.error_message
            : !state.ok ? state.error_message : force.error_message;
        return common::Result<RobotPreflightSnapshot>::failure(
            "RM75 preflight query failed: " + error);
    }
    const auto identity_check = ValidateRobotIdentity(info.value);
    const auto safety_check = ValidateRobotSafetyStatus(safety.value);
    const auto limits_check = ValidateJointLimitSnapshot(limits.value);
    const auto state_check = ValidateArmState(state.value, limits.value);
    const auto force_check = ValidateForceData(force.value, config);
    const auto trajectory_check = ValidateEveryTrajectoryPointWithinLimits(
        trajectory.samples, limits.value);
    if (!identity_check.ok || !safety_check.ok || !limits_check.ok || !state_check.ok ||
        !force_check.ok || !trajectory_check.ok) {
        const auto &failed = !identity_check.ok ? identity_check
            : !safety_check.ok ? safety_check : !limits_check.ok ? limits_check
            : !state_check.ok ? state_check : !force_check.ok ? force_check : trajectory_check;
        return common::Result<RobotPreflightSnapshot>::failure(failed.error_message);
    }
    return common::Result<RobotPreflightSnapshot>::success({limits.value, state.value});
}

void WaitForNextPoll(std::chrono::milliseconds poll_period) {
    if (poll_period.count() > 0) {
        std::this_thread::sleep_for(poll_period);
    } else {
        std::this_thread::yield();
    }
}

bool StopWasRequested(const ArmO6TrajectoryTaskConfig &config) {
    return config.stop_requested && config.stop_requested();
}

common::Result<void> WaitForTrajectoryOrigin(
    interfaces::IRobotArm &robot,
    const common::RobotJointLimits &joint_limits,
    const std::vector<float> &origin_deg,
    const ArmO6TrajectoryTaskConfig &config) {
    const auto deadline = std::chrono::steady_clock::now() + config.origin_motion_timeout;
    while (true) {
        if (StopWasRequested(config)) {
            return common::Result<void>::failure(
                "stop requested while RM75 was moving to the trajectory origin");
        }
        const auto safety = robot.getRobotSafetyStatus();
        const auto state = robot.getCurrentArmState();
        if (!safety.ok || !state.ok) {
            return common::Result<void>::failure(
                "trajectory-origin RM75 query failed: " +
                (!safety.ok ? safety.error_message : state.error_message));
        }
        const auto safety_check = ValidateRobotSafetyStatus(safety.value);
        const auto state_check = ValidateArmState(state.value, joint_limits);
        if (!safety_check.ok || !state_check.ok) {
            return !safety_check.ok ? safety_check : state_check;
        }
        const auto origin_check = ValidateOriginDifference(
            state.value, origin_deg, config.origin_position_tolerance_deg,
            "RM75 has not reached the trajectory origin");
        if (origin_check.ok) {
            return common::Result<void>::success();
        }
        if (std::chrono::steady_clock::now() >= deadline) {
            return common::Result<void>::failure(
                origin_check.error_message + " before the configured timeout");
        }
        WaitForNextPoll(config.origin_status_poll_period);
    }
}

void RecordProgramPollStarted(
    ProgramTimingAccumulator *timing,
    std::chrono::steady_clock::time_point poll_started_at,
    std::chrono::milliseconds target_poll_period) {
    ++timing->report.poll_count;
    if (!timing->previous_poll_started_at.has_value()) {
        timing->previous_poll_started_at = poll_started_at;
        return;
    }
    const double actual_period_ms = std::chrono::duration<double, std::milli>(
        poll_started_at - *timing->previous_poll_started_at).count();
    if (timing->poll_period_count == 0U) {
        timing->report.minimum_poll_period_ms = actual_period_ms;
    } else {
        timing->report.minimum_poll_period_ms = std::min(
            timing->report.minimum_poll_period_ms, actual_period_ms);
    }
    timing->report.maximum_poll_period_ms = std::max(
        timing->report.maximum_poll_period_ms, actual_period_ms);
    timing->report.maximum_absolute_poll_jitter_ms = std::max(
        timing->report.maximum_absolute_poll_jitter_ms,
        std::fabs(actual_period_ms - static_cast<double>(target_poll_period.count())));
    timing->total_poll_period_ms += actual_period_ms;
    ++timing->poll_period_count;
    timing->previous_poll_started_at = poll_started_at;
}

ControllerProgramTimingReport FinishProgramTiming(
    const ProgramTimingAccumulator &timing,
    std::chrono::steady_clock::time_point monitor_finished_at) {
    ControllerProgramTimingReport report = timing.report;
    if (timing.poll_period_count > 0U) {
        report.average_poll_period_ms =
            timing.total_poll_period_ms / static_cast<double>(timing.poll_period_count);
    }
    report.observed_execution_duration_ms = std::chrono::duration<double, std::milli>(
        monitor_finished_at - timing.monitor_started_at).count();
    return report;
}

common::Result<void> ValidateObservedProgramIdentityAndSpeed(
    const common::ControllerProgramRunState &state,
    int expected_program_id,
    int maximum_speed_percent) {
    if (state.program_id != expected_program_id) {
        return common::Result<void>::failure(
            "controller reported program id " + std::to_string(state.program_id) +
            " while monitoring requested id " +
            std::to_string(expected_program_id));
    }
    if (state.plan_speed_percent < kMinimumSpeedPercent ||
        state.plan_speed_percent > maximum_speed_percent) {
        return common::Result<void>::failure(
            "controller reported plan speed " +
            std::to_string(state.plan_speed_percent) +
            "% outside the requested safe range 1.." +
            std::to_string(maximum_speed_percent) + "%");
    }
    return common::Result<void>::success();
}

common::Result<ProgramMonitorResult> MonitorControllerProgram(
    interfaces::IRobotArm &robot,
    int expected_program_id,
    int maximum_speed_percent,
    const std::string &phase_name,
    const ArmO6TrajectoryTaskConfig &config) {
    const auto monitor_started_at = std::chrono::steady_clock::now();
    const auto deadline = monitor_started_at + config.program_execution_timeout;
    ProgramTimingAccumulator timing;
    timing.monitor_started_at = monitor_started_at;
    ProgramMonitorResult monitor;
    while (true) {
        if (StopWasRequested(config)) {
            return common::Result<ProgramMonitorResult>::failure(
                "stop requested while monitoring the RM75 controller program");
        }
        RecordProgramPollStarted(
            &timing, std::chrono::steady_clock::now(), config.program_status_poll_period);
        const auto state = robot.getControllerProgramRunState();
        if (!state.ok) {
            return common::Result<ProgramMonitorResult>::failure(
                phase_name + " failed to query controller program state: " +
                state.error_message);
        }
        if (state.value.raw_state == 1) {
            const auto identity = ValidateObservedProgramIdentityAndSpeed(
                state.value, expected_program_id, maximum_speed_percent);
            if (!identity.ok) {
                return common::Result<ProgramMonitorResult>::failure(identity.error_message);
            }
            monitor.observed_running_state = true;
        } else if (state.value.raw_state == 0 && monitor.observed_running_state) {
            monitor.final_state = state.value;
            monitor.timing = FinishProgramTiming(timing, std::chrono::steady_clock::now());
            return common::Result<ProgramMonitorResult>::success(monitor);
        } else if (state.value.raw_state == 2) {
            return common::Result<ProgramMonitorResult>::failure(
                phase_name + " controller program unexpectedly entered paused state");
        } else if (state.value.raw_state != 0) {
            return common::Result<ProgramMonitorResult>::failure(
                phase_name + " controller program returned unknown raw state " +
                std::to_string(state.value.raw_state));
        }
        if (std::chrono::steady_clock::now() >= deadline) {
            return common::Result<ProgramMonitorResult>::failure(
                phase_name +
                " controller program did not show running followed by completed before timeout");
        }
        WaitForNextPoll(config.program_status_poll_period);
    }
}

bool GripPoseReached(const interfaces::O6JointPose &actual,
                     const interfaces::O6JointPose &target,
                     std::uint16_t tolerance) {
    for (std::size_t joint = 0; joint < interfaces::kO6JointCount; ++joint) {
        const int delta = std::abs(static_cast<int>(actual[joint]) -
                                   static_cast<int>(target[joint]));
        if (delta > static_cast<int>(tolerance)) {
            return false;
        }
    }
    return true;
}

common::Result<interfaces::DexterousHandReadiness> WaitForGripPose(
    interfaces::IDexterousHand &hand,
    const ArmO6TrajectoryTaskConfig &config) {
    const auto deadline = std::chrono::steady_clock::now() + config.hand_motion_timeout;
    while (true) {
        if (StopWasRequested(config)) {
            return common::Result<interfaces::DexterousHandReadiness>::failure(
                "stop requested while waiting for O6 grip readback");
        }
        const auto readiness = ReadReadyHand(hand, "O6 grip readback failed");
        if (!readiness.ok) {
            return readiness;
        }
        if (GripPoseReached(readiness.value.joint_positions, config.grip_pose,
                            config.grip_position_tolerance)) {
            return readiness;
        }
        if (std::chrono::steady_clock::now() >= deadline) {
            return common::Result<interfaces::DexterousHandReadiness>::failure(
                "O6 joint positions did not reach the commanded grip pose before timeout");
        }
        WaitForNextPoll(config.hand_status_poll_period);
    }
}

common::Result<ArmO6TrajectoryTaskReport> FailAfterMotionAttempt(
    interfaces::IRobotArm &robot,
    const std::string &error_message) {
    const auto stop = robot.slowStopMotion();
    if (stop.ok) {
        return common::Result<ArmO6TrajectoryTaskReport>::failure(error_message);
    }
    return common::Result<ArmO6TrajectoryTaskReport>::failure(
        error_message + "; slowStopMotion also failed: " + stop.error_message);
}

common::Result<void> RequirePermissionForSpeed(const HardwareExecutionGateInput &gate_input,
                                               int required_speed_percent,
                                               const std::string &context) {
    HardwareExecutionGateRequirements requirements;
    requirements.required_speed_percent = required_speed_percent;
    requirements.require_confirmed_trajectory_sample_period = true;
    requirements.require_confirmed_vendor_raw_units_per_degree = true;
    const auto permission = RequireHardwareExecutionPermission(gate_input, requirements);
    return permission.ok ? permission : common::Result<void>::failure(
        context + ": " + permission.error_message);
}

common::Result<TaskPreparation> PrepareTask(
    interfaces::IRobotArm &robot,
    interfaces::IDexterousHand &hand,
    const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
    const ArmO6TrajectoryTaskConfig &config,
    const HardwareExecutionGateInput &gate_input) {
    if (StopWasRequested(config)) {
        return common::Result<TaskPreparation>::failure(
            "stop requested before RM75/O6 task preparation");
    }
    const auto config_check = ValidateArmO6TrajectoryTaskConfig(config);
    const auto metadata_check = ValidateTrajectoryMetadata(trajectory);
    const auto validation = modules::trajectory::ValidateDragTrajectory(
        trajectory.samples, config.validation_limits);
    if (!config_check.ok || !metadata_check.ok || !validation.ok) {
        const std::string error = !config_check.ok ? config_check.error_message
            : !metadata_check.ok ? metadata_check.error_message : validation.error_message;
        return common::Result<TaskPreparation>::failure(error);
    }
    const auto source_integrity = VerifySourceFileIntegrity(trajectory);
    if (!source_integrity.ok) {
        return common::Result<TaskPreparation>::failure(
            "pre-motion trajectory integrity check failed: " +
            source_integrity.error_message);
    }
    const auto permission = RequirePermissionForSpeed(
        gate_input, std::max(config.origin_speed_percent, config.plan_speed_percent),
        "initial RM75 motion permission refused");
    if (!permission.ok) {
        return common::Result<TaskPreparation>::failure(permission.error_message);
    }
    const auto preflight = ReadAndValidatePreflight(robot, trajectory, config);
    if (!preflight.ok) {
        return common::Result<TaskPreparation>::failure(preflight.error_message);
    }
    const auto origin = ValidateOriginDifference(
        preflight.value.arm_state, trajectory.samples.front().joints_deg,
        config.maximum_origin_delta_deg, "trajectory origin is too far from current RM75 state");
    const auto hand_ready = ReadReadyHand(hand, "O6 preflight failed");
    if (!origin.ok || !hand_ready.ok) {
        return common::Result<TaskPreparation>::failure(
            !origin.ok ? origin.error_message : hand_ready.error_message);
    }
    const auto slot = robot.isControllerProgramSlotAvailable(config.controller_program_id);
    if (!slot.ok || !slot.value) {
        return common::Result<TaskPreparation>::failure(
            !slot.ok ? "controller program slot query failed: " + slot.error_message
                     : "controller program slot is occupied; refusing to overwrite it");
    }
    return common::Result<TaskPreparation>::success({validation.value, preflight.value});
}

common::Result<void> ValidateRobotAtTrajectoryEndpoint(
    interfaces::IRobotArm &robot,
    const common::RobotJointLimits &joint_limits,
    const std::vector<float> &expected_joints_deg,
    const ArmO6TrajectoryTaskConfig &config,
    const std::string &context) {
    const auto safety = robot.getRobotSafetyStatus();
    const auto state = robot.getCurrentArmState();
    const auto force = robot.getForceData();
    if (!safety.ok || !state.ok || !force.ok) {
        const std::string error = !safety.ok ? safety.error_message
            : !state.ok ? state.error_message : force.error_message;
        return common::Result<void>::failure(context + " query failed: " + error);
    }
    const auto safety_check = ValidateRobotSafetyStatus(safety.value);
    const auto state_check = ValidateArmState(state.value, joint_limits);
    const auto force_check = ValidateForceData(force.value, config);
    const auto endpoint_check = ValidateOriginDifference(
        state.value, expected_joints_deg, config.origin_position_tolerance_deg,
        context);
    if (!safety_check.ok || !state_check.ok || !force_check.ok || !endpoint_check.ok) {
        return !safety_check.ok ? safety_check
            : !state_check.ok ? state_check
            : !force_check.ok ? force_check : endpoint_check;
    }
    return common::Result<void>::success();
}

common::Result<common::ArmState> ReadAndValidateRobotStateForEndpoint(
    interfaces::IRobotArm &robot,
    const common::RobotJointLimits &joint_limits,
    const ArmO6TrajectoryTaskConfig &config,
    const std::string &context) {
    const auto safety = robot.getRobotSafetyStatus();
    const auto state = robot.getCurrentArmState();
    const auto force = robot.getForceData();
    if (!safety.ok || !state.ok || !force.ok) {
        const std::string error = !safety.ok ? safety.error_message
            : !state.ok ? state.error_message : force.error_message;
        return common::Result<common::ArmState>::failure(context + " query failed: " + error);
    }
    const auto safety_check = ValidateRobotSafetyStatus(safety.value);
    const auto state_check = ValidateArmState(state.value, joint_limits);
    const auto force_check = ValidateForceData(force.value, config);
    if (!safety_check.ok || !state_check.ok || !force_check.ok) {
        const auto &failed = !safety_check.ok ? safety_check
            : !state_check.ok ? state_check : force_check;
        return common::Result<common::ArmState>::failure(failed.error_message);
    }
    return common::Result<common::ArmState>::success(state.value);
}

common::Result<void> SettleRobotAtTrajectoryEndpointWithinOriginLimit(
    interfaces::IRobotArm &robot,
    const common::RobotJointLimits &joint_limits,
    const std::vector<float> &expected_joints_deg,
    const ArmO6TrajectoryTaskConfig &config,
    const std::string &context) {
    const auto state = ReadAndValidateRobotStateForEndpoint(
        robot, joint_limits, config, context);
    if (!state.ok) {
        return common::Result<void>::failure(state.error_message);
    }
    const auto strict_endpoint = ValidateOriginDifference(
        state.value, expected_joints_deg, config.origin_position_tolerance_deg, context);
    if (strict_endpoint.ok) {
        return common::Result<void>::success();
    }
    const auto correction_limit = ValidateOriginDifference(
        state.value, expected_joints_deg, config.maximum_origin_delta_deg,
        context + " low-speed correction exceeds safe origin move limit");
    if (!correction_limit.ok) {
        return common::Result<void>::failure(strict_endpoint.error_message);
    }
    if (StopWasRequested(config)) {
        return common::Result<void>::failure(
            "stop requested before low-speed endpoint correction");
    }
    const auto move = robot.moveJ(
        expected_joints_deg, config.origin_speed_percent, 0, 0, false);
    if (!move.ok) {
        return common::Result<void>::failure(
            context + " low-speed correction failed: " + move.error_message);
    }
    return WaitForTrajectoryOrigin(robot, joint_limits, expected_joints_deg, config);
}

common::Result<RoundTripTaskPreparation> PrepareRoundTripTask(
    interfaces::IRobotArm &robot,
    interfaces::IDexterousHand &hand,
    const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
    const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
    const ArmO6TrajectoryTaskConfig &config,
    const HardwareExecutionGateInput &gate_input) {
    if (StopWasRequested(config)) {
        return common::Result<RoundTripTaskPreparation>::failure(
            "stop requested before RM75 round-trip task preparation");
    }
    const auto config_check = ValidateRoundTripTaskConfig(config);
    const auto return_metadata = ValidateTrajectoryMetadata(return_trajectory);
    const auto forward_metadata = ValidateTrajectoryMetadata(forward_trajectory);
    const auto pair_check = ValidateReversedTrajectoryPair(
        return_trajectory, forward_trajectory);
    const auto return_validation = modules::trajectory::ValidateDragTrajectory(
        return_trajectory.samples, config.validation_limits);
    const auto forward_validation = modules::trajectory::ValidateDragTrajectory(
        forward_trajectory.samples, config.validation_limits);
    if (!config_check.ok || !return_metadata.ok || !forward_metadata.ok ||
        !pair_check.ok || !return_validation.ok || !forward_validation.ok) {
        const std::string error = !config_check.ok ? config_check.error_message
            : !return_metadata.ok ? return_metadata.error_message
            : !forward_metadata.ok ? forward_metadata.error_message
            : !pair_check.ok ? pair_check.error_message
            : !return_validation.ok ? return_validation.error_message
                                    : forward_validation.error_message;
        return common::Result<RoundTripTaskPreparation>::failure(error);
    }
    const auto return_integrity = VerifySourceFileIntegrity(return_trajectory);
    const auto forward_integrity = VerifySourceFileIntegrity(forward_trajectory);
    if (!return_integrity.ok || !forward_integrity.ok) {
        return common::Result<RoundTripTaskPreparation>::failure(
            "pre-motion round-trip trajectory integrity check failed: " +
            (!return_integrity.ok ? return_integrity.error_message
                                  : forward_integrity.error_message));
    }
    const auto permission = RequirePermissionForSpeed(
        gate_input, config.plan_speed_percent,
        "initial RM75 round-trip permission refused");
    if (!permission.ok) {
        return common::Result<RoundTripTaskPreparation>::failure(
            permission.error_message);
    }
    const auto preflight = ReadAndValidatePreflight(
        robot, forward_trajectory, config);
    if (!preflight.ok) {
        return common::Result<RoundTripTaskPreparation>::failure(
            preflight.error_message);
    }
    const auto endpoint = ValidateOriginDifference(
        preflight.value.arm_state, return_trajectory.samples.front().joints_deg,
        config.origin_position_tolerance_deg,
        "RM75 current state must already match the forward trajectory end point");
    const auto hand_ready = ReadReadyHand(hand, "O6 round-trip preflight failed");
    if (!endpoint.ok || !hand_ready.ok) {
        return common::Result<RoundTripTaskPreparation>::failure(
            !endpoint.ok ? endpoint.error_message : hand_ready.error_message);
    }
    const auto return_slot = robot.isControllerProgramSlotAvailable(
        config.return_controller_program_id);
    const auto forward_slot = robot.isControllerProgramSlotAvailable(
        config.controller_program_id);
    if (!return_slot.ok || !forward_slot.ok || !return_slot.value ||
        !forward_slot.value) {
        const std::string error = !return_slot.ok ? return_slot.error_message
            : !forward_slot.ok ? forward_slot.error_message
            : !return_slot.value ? "return controller program slot is occupied"
                                 : "forward controller program slot is occupied";
        return common::Result<RoundTripTaskPreparation>::failure(
            "round-trip controller slot preflight failed: " + error);
    }
    return common::Result<RoundTripTaskPreparation>::success(
        {forward_validation.value, preflight.value});
}

common::Result<void> SaveRoundTripControllerPrograms(
    interfaces::IRobotArm &robot,
    const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
    const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
    const ArmO6TrajectoryTaskConfig &config) {
    const std::array<ControllerProgramLegRequest, 2> legs{{
        {&return_trajectory, config.return_controller_program_id, "return trajectory"},
        {&forward_trajectory, config.controller_program_id, "forward trajectory"},
    }};
    for (const auto &leg : legs) {
        if (StopWasRequested(config)) {
            return common::Result<void>::failure(
                std::string("stop requested before saving ") + leg.phase_name);
        }
        const auto integrity = VerifySourceFileIntegrity(*leg.trajectory);
        if (!integrity.ok) {
            return common::Result<void>::failure(
                std::string(leg.phase_name) +
                " integrity check failed immediately before upload: " +
                integrity.error_message);
        }
        common::VendorDragTrajectoryExecutionRequest request;
        request.trajectory_path = leg.trajectory->source_file;
        request.plan_speed_percent = config.plan_speed_percent;
        request.controller_program_id = leg.controller_program_id;
        const auto saved = robot.saveVendorDragTrajectoryProgram(request);
        if (!saved.ok) {
            return common::Result<void>::failure(
                std::string("failed to save ") + leg.phase_name +
                " without motion: " + saved.error_message);
        }
    }
    return common::Result<void>::success();
}

common::Result<ProgramMonitorResult> ExecuteSavedControllerProgramLeg(
    interfaces::IRobotArm &robot,
    const ControllerProgramLegRequest &leg,
    const ArmO6TrajectoryTaskConfig &config,
    const HardwareExecutionGateInput &gate_input,
    const common::RobotJointLimits &joint_limits,
    TrajectoryEndpointCompletionMode endpoint_completion_mode) {
    if (leg.trajectory == nullptr || leg.phase_name == nullptr) {
        return common::Result<ProgramMonitorResult>::failure(
            "internal round-trip controller program leg is invalid");
    }
    if (StopWasRequested(config)) {
        return common::Result<ProgramMonitorResult>::failure(
            std::string("stop requested before running ") + leg.phase_name);
    }
    const auto permission = RequirePermissionForSpeed(
        gate_input, config.plan_speed_percent,
        std::string(leg.phase_name) + " permission refused");
    if (!permission.ok) {
        return common::Result<ProgramMonitorResult>::failure(
            permission.error_message);
    }
    const auto start_state = ValidateRobotAtTrajectoryEndpoint(
        robot, joint_limits, leg.trajectory->samples.front().joints_deg,
        config, std::string(leg.phase_name) + " start point mismatch");
    if (!start_state.ok) {
        return common::Result<ProgramMonitorResult>::failure(
            start_state.error_message);
    }
    common::ControllerProgramRunRequest run_request;
    run_request.controller_program_id = leg.controller_program_id;
    run_request.speed_percent = config.plan_speed_percent;
    run_request.blocking = false;
    const auto run = robot.runControllerProgram(run_request);
    if (!run.ok) {
        return common::Result<ProgramMonitorResult>::failure(
            std::string("failed to run ") + leg.phase_name + ": " +
            run.error_message);
    }
    const auto monitor = MonitorControllerProgram(
        robot, leg.controller_program_id, config.plan_speed_percent,
        leg.phase_name, config);
    if (!monitor.ok) {
        return monitor;
    }
    const std::string end_context =
        std::string(leg.phase_name) + " end point mismatch";
    const auto end_state =
        endpoint_completion_mode == TrajectoryEndpointCompletionMode::StrictValidation
            ? ValidateRobotAtTrajectoryEndpoint(
                  robot, joint_limits, leg.trajectory->samples.back().joints_deg,
                  config, end_context)
            : SettleRobotAtTrajectoryEndpointWithinOriginLimit(
                  robot, joint_limits, leg.trajectory->samples.back().joints_deg,
                  config, end_context);
    if (!end_state.ok) {
        return common::Result<ProgramMonitorResult>::failure(
            end_state.error_message);
    }
    return monitor;
}

common::Result<ProgramMonitorResult> ExecuteArmTrajectory(
    interfaces::IRobotArm &robot,
    const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
    const ArmO6TrajectoryTaskConfig &config,
    const HardwareExecutionGateInput &gate_input,
    const common::RobotJointLimits &joint_limits) {
    if (StopWasRequested(config)) {
        return common::Result<ProgramMonitorResult>::failure(
            "stop requested before moving RM75 to the trajectory origin");
    }
    const auto move = robot.moveJ(
        trajectory.samples.front().joints_deg, config.origin_speed_percent, 0, 0, false);
    if (!move.ok) {
        return common::Result<ProgramMonitorResult>::failure(
            "failed to move RM75 to trajectory origin: " + move.error_message);
    }
    const auto origin = WaitForTrajectoryOrigin(
        robot, joint_limits, trajectory.samples.front().joints_deg, config);
    if (!origin.ok) {
        return common::Result<ProgramMonitorResult>::failure(origin.error_message);
    }
    const auto permission = RequirePermissionForSpeed(
        gate_input, config.plan_speed_percent, "RM75 trajectory permission refused");
    if (!permission.ok) {
        return common::Result<ProgramMonitorResult>::failure(permission.error_message);
    }
    common::VendorDragTrajectoryExecutionRequest request;
    request.trajectory_path = trajectory.source_file;
    request.plan_speed_percent = config.plan_speed_percent;
    request.controller_program_id = config.controller_program_id;
    const auto source_integrity = VerifySourceFileIntegrity(trajectory);
    if (!source_integrity.ok) {
        return common::Result<ProgramMonitorResult>::failure(
            "pre-upload trajectory integrity check failed: " +
            source_integrity.error_message);
    }
    if (StopWasRequested(config)) {
        return common::Result<ProgramMonitorResult>::failure(
            "stop requested before uploading the RM75 controller program");
    }
    const auto execute = robot.executeVendorDragTrajectoryFile(request);
    if (!execute.ok) {
        return common::Result<ProgramMonitorResult>::failure(
            "failed to start RM75 vendor trajectory: " + execute.error_message);
    }
    return MonitorControllerProgram(
        robot, config.controller_program_id, config.plan_speed_percent,
        "forward trajectory", config);
}

common::Result<void> RecheckRobotBeforeGrip(
    interfaces::IRobotArm &robot,
    const common::RobotJointLimits &joint_limits,
    const ArmO6TrajectoryTaskConfig &config) {
    const auto safety = robot.getRobotSafetyStatus();
    const auto state = robot.getCurrentArmState();
    const auto force = robot.getForceData();
    if (!safety.ok || !state.ok || !force.ok) {
        const std::string error = !safety.ok ? safety.error_message
            : !state.ok ? state.error_message : force.error_message;
        return common::Result<void>::failure(
            "pre-grip RM75 safety query failed: " + error);
    }
    const auto safety_check = ValidateRobotSafetyStatus(safety.value);
    const auto state_check = ValidateArmState(state.value, joint_limits);
    const auto force_check = ValidateForceData(force.value, config);
    if (!safety_check.ok || !state_check.ok || !force_check.ok) {
        return !safety_check.ok ? safety_check : !state_check.ok ? state_check : force_check;
    }
    return common::Result<void>::success();
}

common::Result<interfaces::DexterousHandReadiness> ExecuteO6Grip(
    interfaces::IRobotArm &robot,
    interfaces::IDexterousHand &hand,
    const ArmO6TrajectoryTaskConfig &config,
    const HardwareExecutionGateInput &gate_input,
    const common::RobotJointLimits &joint_limits) {
    const auto robot_safety = RecheckRobotBeforeGrip(robot, joint_limits, config);
    if (!robot_safety.ok) {
        return common::Result<interfaces::DexterousHandReadiness>::failure(
            robot_safety.error_message);
    }
    const auto readiness = ReadReadyHand(hand, "O6 readiness after trajectory failed");
    if (!readiness.ok) {
        return readiness;
    }
    const auto permission = RequirePermissionForSpeed(
        gate_input, 0, "O6 grip permission refused");
    if (!permission.ok) {
        return common::Result<interfaces::DexterousHandReadiness>::failure(
            permission.error_message);
    }
    if (StopWasRequested(config)) {
        return common::Result<interfaces::DexterousHandReadiness>::failure(
            "stop requested before commanding the O6 grip");
    }
    interfaces::O6MotionCommand motion_command;
    motion_command.target_positions = config.grip_pose;
    motion_command.joint_speed_commands = config.grip_speed_commands;
    motion_command.joint_torque_commands = config.grip_torque_commands;
    const auto command = hand.CommandMotion(motion_command);
    if (!command.ok) {
        return common::Result<interfaces::DexterousHandReadiness>::failure(
            "failed to command O6 grip motion: " + command.error_message);
    }
    return WaitForGripPose(hand, config);
}

} // namespace

common::Result<void> ValidateArmO6TrajectoryTaskConfig(
    const ArmO6TrajectoryTaskConfig &config) {
    return ValidateTaskConfigImpl(config);
}

common::Result<void> ValidateArmO6RoundTripTaskConfig(
    const ArmO6TrajectoryTaskConfig &config) {
    return ValidateRoundTripTaskConfig(config);
}

common::Result<void> VerifySourceFileIntegrity(
    const modules::trajectory::VendorDragTrajectoryParseResult &trajectory) {
    if (trajectory.source_file.empty() || trajectory.source_file_sha256.empty()) {
        return common::Result<void>::failure(
            "trajectory source path and expected SHA-256 must not be empty");
    }
    const auto size_before_digest =
        InspectReadOnlyRegularSourceFile(trajectory.source_file);
    if (!size_before_digest.ok ||
        size_before_digest.value != trajectory.source_file_size_bytes) {
        return common::Result<void>::failure(
            !size_before_digest.ok ? size_before_digest.error_message
                                   : "trajectory source size differs from parsed metadata");
    }
    const auto digest = modules::trajectory::CalculateFileSha256(trajectory.source_file);
    if (!digest.ok) {
        return common::Result<void>::failure(digest.error_message);
    }
    const auto size_after_digest =
        InspectReadOnlyRegularSourceFile(trajectory.source_file);
    if (!size_after_digest.ok ||
        size_after_digest.value != trajectory.source_file_size_bytes) {
        return common::Result<void>::failure(
            !size_after_digest.ok ? size_after_digest.error_message
                                  : "trajectory source changed while calculating SHA-256");
    }
    if (digest.value != trajectory.source_file_sha256) {
        return common::Result<void>::failure(
            "trajectory source SHA-256 differs from parsed metadata");
    }
    return common::Result<void>::success();
}

ArmO6TrajectoryTask::ArmO6TrajectoryTask(interfaces::IRobotArm &robot,
                                         interfaces::IDexterousHand &hand) noexcept
    : robot_(robot), hand_(hand) {}

common::Result<ArmO6TrajectoryTaskReport> ArmO6TrajectoryTask::Run(
    const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
    const ArmO6TrajectoryTaskConfig &config,
    const HardwareExecutionGateInput &gate_input) {
    const auto preparation = PrepareTask(robot_, hand_, trajectory, config, gate_input);
    if (!preparation.ok) {
        return common::Result<ArmO6TrajectoryTaskReport>::failure(
            preparation.error_message);
    }
    const auto monitor = ExecuteArmTrajectory(
        robot_, trajectory, config, gate_input, preparation.value.robot_preflight.joint_limits);
    if (!monitor.ok) {
        return FailAfterMotionAttempt(robot_, monitor.error_message);
    }
    const auto grip_readback = ExecuteO6Grip(
        robot_, hand_, config, gate_input, preparation.value.robot_preflight.joint_limits);
    if (!grip_readback.ok) {
        return FailAfterMotionAttempt(robot_, grip_readback.error_message);
    }

    ArmO6TrajectoryTaskReport report;
    report.trajectory_validation = preparation.value.trajectory_validation;
    report.final_program_state = monitor.value.final_state;
    report.final_hand_readiness = grip_readback.value;
    report.controller_timing = monitor.value.timing;
    report.trajectory_point_count = trajectory.trajectory_point_count;
    report.observed_running_state = monitor.value.observed_running_state;
    return common::Result<ArmO6TrajectoryTaskReport>::success(report);
}

common::Result<ArmO6TrajectoryTaskReport>
ArmO6TrajectoryTask::RunFromTrajectoryEnd(
    const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
    const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
    const ArmO6TrajectoryTaskConfig &config,
    const HardwareExecutionGateInput &gate_input) {
    const auto preparation = PrepareRoundTripTask(
        robot_, hand_, return_trajectory, forward_trajectory, config, gate_input);
    if (!preparation.ok) {
        return common::Result<ArmO6TrajectoryTaskReport>::failure(
            preparation.error_message);
    }
    const auto saved = SaveRoundTripControllerPrograms(
        robot_, return_trajectory, forward_trajectory, config);
    if (!saved.ok) {
        return common::Result<ArmO6TrajectoryTaskReport>::failure(
            saved.error_message);
    }

    const ControllerProgramLegRequest return_leg{
        &return_trajectory, config.return_controller_program_id,
        "return trajectory"};
    const auto return_monitor = ExecuteSavedControllerProgramLeg(
        robot_, return_leg, config, gate_input,
        preparation.value.robot_preflight.joint_limits,
        TrajectoryEndpointCompletionMode::SettleWithLowSpeedMoveJ);
    if (!return_monitor.ok) {
        return FailAfterMotionAttempt(robot_, return_monitor.error_message);
    }

    const ControllerProgramLegRequest forward_leg{
        &forward_trajectory, config.controller_program_id,
        "forward trajectory"};
    const auto forward_monitor = ExecuteSavedControllerProgramLeg(
        robot_, forward_leg, config, gate_input,
        preparation.value.robot_preflight.joint_limits,
        TrajectoryEndpointCompletionMode::StrictValidation);
    if (!forward_monitor.ok) {
        return FailAfterMotionAttempt(robot_, forward_monitor.error_message);
    }

    const auto grip_readback = ExecuteO6Grip(
        robot_, hand_, config, gate_input,
        preparation.value.robot_preflight.joint_limits);
    if (!grip_readback.ok) {
        return FailAfterMotionAttempt(robot_, grip_readback.error_message);
    }

    ArmO6TrajectoryTaskReport report;
    report.trajectory_validation = preparation.value.forward_validation;
    report.return_final_program_state = return_monitor.value.final_state;
    report.final_program_state = forward_monitor.value.final_state;
    report.final_hand_readiness = grip_readback.value;
    report.return_controller_timing = return_monitor.value.timing;
    report.controller_timing = forward_monitor.value.timing;
    report.trajectory_point_count = forward_trajectory.trajectory_point_count;
    report.returned_from_trajectory_end = true;
    report.return_observed_running_state =
        return_monitor.value.observed_running_state;
    report.observed_running_state =
        forward_monitor.value.observed_running_state;
    return common::Result<ArmO6TrajectoryTaskReport>::success(report);
}

} // namespace rm_control::core
