/**
 * @file trajectory_o6_grasp_main.cpp
 * @brief RM75 本地拖动轨迹执行与 LinkerHand O6 夹握任务入口。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @details
 * Application 只负责命令行、只读配置、依赖组装、DryRun 报告和人工二次确认。
 * 七轴解析/校验、任务状态机、O6 协议和 RM_API2 分别留在各自模块与适配器中。
 */
#include "flight_robot/linker_hand/linker_hand_adapter.h"
#include "flight_robot/realman/realman_arm_adapter.h"
#include "rm_control/config/drag_teach_safety_config.h"
#include "rm_control/config/trajectory_o6_task_cli_config.h"
#include "rm_control/core/arm_o6_trajectory_task.h"
#include "rm_control/core/hardware_execution_gate.h"
#include "rm_control/modules/trajectory/drag_trajectory_validator.h"
#include "rm_control/modules/trajectory/reversed_vendor_trajectory_store.h"
#include "rm_control/modules/trajectory/trajectory_file_store.h"
#include "rm_control/modules/trajectory/trajectory_playback_report_store.h"
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"

#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <csignal>
#include <filesystem>
#include <iomanip>
#include <iostream>
#include <limits>
#include <string>
#include <vector>

namespace {

constexpr int kInvalidArgumentsExitCode = 64;
constexpr int kInvalidInputExitCode = 65;
constexpr int kHardwareExecutionRefusedExitCode = 77;
constexpr int kConnectionFailureExitCode = 1;
constexpr int kTaskFailureExitCode = 2;
constexpr long long kMinimumProgramTimeoutMilliseconds = 60000;
constexpr long long kMaximumProgramTimeoutMilliseconds = 7200000;
constexpr long long kProgramTimeoutMarginMilliseconds = 60000;
constexpr const char *kMissingOperatorConfirmationReason =
    "operator second confirmation is missing";

struct OfflineTaskPreparation {
    rm_control::modules::trajectory::VendorDragTrajectoryParseResult trajectory;
    rm_control::modules::trajectory::VendorDragTrajectoryParseResult return_trajectory;
    rm_control::modules::trajectory::DragTrajectoryValidationReport validation;
    rm_control::modules::trajectory::DragTrajectoryValidationReport return_validation;
    rm_control::core::ArmO6TrajectoryTaskConfig task_config;
    bool return_from_trajectory_end = false;
};

volatile std::sig_atomic_t g_stop_requested = 0;

void HandleStopSignal(int) {
    g_stop_requested = 1;
}

rm_control::common::Result<void> InstallStopSignalHandlers() {
    g_stop_requested = 0;
    if (std::signal(SIGINT, HandleStopSignal) == SIG_ERR ||
        std::signal(SIGTERM, HandleStopSignal) == SIG_ERR) {
        return rm_control::common::Result<void>::failure(
            "failed to install SIGINT/SIGTERM stop handlers");
    }
    return rm_control::common::Result<void>::success();
}

bool StopWasRequestedByProcessSignal() {
    return g_stop_requested != 0;
}

rm_control::modules::trajectory::DragTrajectoryValidationLimits
BuildValidationLimits(const rm_control::config::DragTeachSafetyConfig &safety) {
    rm_control::modules::trajectory::DragTrajectoryValidationLimits limits;
    limits.maximum_joint_jump_deg = safety.max_joint_jump_deg;
    limits.maximum_joint_velocity_deg_per_second =
        safety.max_joint_velocity_deg_per_second;
    limits.maximum_joint_acceleration_deg_per_second_squared =
        safety.max_joint_acceleration_deg_per_second_squared;
    return limits;
}

std::filesystem::path BuildAbsoluteTrajectoryPath(
    const std::filesystem::path &input_path,
    std::error_code *error) {
    const auto absolute_path = std::filesystem::absolute(input_path, *error);
    return *error ? std::filesystem::path{} : absolute_path.lexically_normal();
}

void PrintJointValues(const std::vector<float> &joints_deg) {
    std::cout << '[';
    for (std::size_t index = 0; index < joints_deg.size(); ++index) {
        std::cout << (index == 0U ? "" : ", ") << joints_deg[index];
    }
    std::cout << ']';
}

void PrintOfflineReport(
    const char *trajectory_label,
    const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
    const rm_control::modules::trajectory::DragTrajectoryValidationReport &validation) {
    std::cout << std::fixed << std::setprecision(3)
              << trajectory_label << " file: " << trajectory.source_file << '\n'
              << "Trajectory points: " << trajectory.trajectory_point_count << '\n'
              << "Sample period: " << trajectory.sample_period_ms << " ms (explicit)\n"
              << "Timestamp derived: "
              << (trajectory.timestamp_is_derived ? "true" : "false") << '\n'
              << "Raw units per degree: " << trajectory.raw_units_per_degree << '\n'
              << "Duration: " << validation.duration_ms << " ms\n"
              << "Source bytes: " << trajectory.source_file_size_bytes << '\n'
              << "SHA-256: " << trajectory.source_file_sha256 << '\n'
              << "Maximum jump: "
              << validation.observed_maximum_joint_jump_deg << " deg\n"
              << "Maximum velocity: "
              << validation.observed_maximum_joint_velocity_deg_per_second
              << " deg/s\n"
              << "Maximum acceleration: "
              << validation.observed_maximum_joint_acceleration_deg_per_second_squared
              << " deg/s^2\n"
              << "First RM75 joints (deg): ";
    PrintJointValues(trajectory.samples.front().joints_deg);
    std::cout << "\nLast RM75 joints (deg): ";
    PrintJointValues(trajectory.samples.back().joints_deg);
    std::cout << '\n';
}

rm_control::interfaces::DexterousHandSide ConvertHandSide(
    rm_control::config::O6HandSide side) {
    return side == rm_control::config::O6HandSide::Right
               ? rm_control::interfaces::DexterousHandSide::Right
               : rm_control::interfaces::DexterousHandSide::Left;
}

template <typename SourceArray>
rm_control::interfaces::O6JointPose BuildO6ProtocolValues(
    const SourceArray &source) {
    rm_control::interfaces::O6JointPose values{};
    std::transform(source.begin(), source.end(), values.begin(),
                   [](std::uint8_t value) {
                       return static_cast<std::uint16_t>(value);
                   });
    return values;
}

std::chrono::milliseconds EstimateProgramTimeout(
    long long source_duration_ms,
    int plan_speed_percent) {
    const long double scaled_duration =
        static_cast<long double>(source_duration_ms) * 100.0L /
        static_cast<long double>(plan_speed_percent);
    const long double with_margin =
        scaled_duration + static_cast<long double>(kProgramTimeoutMarginMilliseconds);
    const long double bounded = std::clamp(
        with_margin,
        static_cast<long double>(kMinimumProgramTimeoutMilliseconds),
        static_cast<long double>(kMaximumProgramTimeoutMilliseconds));
    return std::chrono::milliseconds(static_cast<long long>(bounded));
}

rm_control::core::ArmO6TrajectoryTaskConfig BuildTaskConfig(
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const rm_control::config::DragTeachSafetyConfig &safety,
    long long trajectory_duration_ms) {
    rm_control::core::ArmO6TrajectoryTaskConfig config;
    config.validation_limits = BuildValidationLimits(safety);
    config.origin_speed_percent = cli.origin_speed_percent;
    config.plan_speed_percent = cli.plan_speed_percent;
    config.controller_program_id = cli.program_id;
    config.return_controller_program_id = cli.return_program_id;
    config.grip_pose = BuildO6ProtocolValues(cli.grip_pose);
    config.grip_speed_commands = BuildO6ProtocolValues(cli.grip_speed);
    config.grip_torque_commands = BuildO6ProtocolValues(cli.grip_torque);
    config.maximum_o6_speed_command = safety.maximum_o6_speed_command;
    config.maximum_o6_torque_command = safety.maximum_o6_torque_command;
    config.maximum_origin_delta_deg = safety.maximum_origin_move_delta_deg;
    config.maximum_absolute_force_newton = safety.maximum_absolute_force_newton;
    config.maximum_absolute_torque_newton_meter =
        safety.maximum_absolute_torque_newton_meter;
    config.program_execution_timeout =
        EstimateProgramTimeout(trajectory_duration_ms, cli.plan_speed_percent);
    config.stop_requested = StopWasRequestedByProcessSignal;
    return config;
}

rm_control::core::HardwareExecutionGateInput BuildExecutionGate(
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const rm_control::config::DragTeachSafetyConfig &safety,
    bool arm_connected,
    bool hand_connected,
    bool operator_confirmed) {
    rm_control::core::HardwareExecutionGateInput gate;
    gate.live_requested = cli.live_requested;
    gate.hardware_execution_enabled = safety.hardware_execution_enabled;
    gate.arm_connected = arm_connected;
    gate.hand_connected = hand_connected;
    gate.emergency_stop_verified = safety.emergency_stop_verified;
    gate.safety_limits_loaded = safety.safety_limits_loaded;
    gate.operator_confirmed = operator_confirmed;
    gate.trajectory_validated = true;
    gate.trajectory_sample_period_confirmed =
        safety.trajectory_sample_period_confirmed;
    gate.vendor_raw_units_per_degree_confirmed =
        safety.vendor_raw_units_per_degree_confirmed;
    gate.workspace_and_virtual_wall_verified =
        safety.workspace_and_virtual_wall_verified;
    gate.configured_max_speed_percent = safety.max_speed_percent;
    return gate;
}

std::string BuildConfirmationPhrase(const std::string &forward_sha256,
                                    const std::string &return_sha256) {
    std::string phrase = "CONFIRM RM75 O6 " + forward_sha256.substr(
        0U, std::min<std::size_t>(12U, forward_sha256.size()));
    if (!return_sha256.empty()) {
        phrase += " RETURN " + return_sha256.substr(
            0U, std::min<std::size_t>(12U, return_sha256.size()));
    }
    return phrase;
}

bool RequestOperatorConfirmation(const std::string &phrase) {
    std::cout << "Type exactly '" << phrase
              << "' to authorize the displayed RM75 trajectory and O6 grip: "
              << std::flush;
    std::string confirmation;
    return std::getline(std::cin, confirmation) && confirmation == phrase;
}

bool PrintPreConfirmationRejections(
    const rm_control::core::HardwareExecutionGateDecision &decision) {
    bool rejected = false;
    for (const auto &reason : decision.rejection_reasons) {
        if (reason == kMissingOperatorConfirmationReason) {
            continue;
        }
        rejected = true;
        std::cerr << "Hardware execution refused: " << reason << '\n';
    }
    return rejected;
}

void PrintO6Readiness(
    const rm_control::interfaces::DexterousHandReadiness &readiness) {
    std::cout << "O6 readiness: ready=true, device_id=0x" << std::hex
              << static_cast<int>(readiness.device_id) << std::dec
              << ", freedom=" << readiness.freedom
              << ", direction=" << readiness.direction
              << ", hand_version=" << readiness.hand_version << '\n'
              << "O6 current positions: ";
    std::vector<float> positions(readiness.joint_positions.begin(),
                                 readiness.joint_positions.end());
    PrintJointValues(positions);
    std::cout << '\n';
}

void PrintO6ProtocolValues(const char *label,
                           const rm_control::interfaces::O6JointPose &values) {
    std::cout << label << ": [";
    for (std::size_t index = 0U; index < values.size(); ++index) {
        std::cout << (index == 0U ? "" : ", ") << values[index];
    }
    std::cout << "]\n";
}

rm_control::common::Result<void> InitializeO6ToolModbusSession(
    flight_robot::realman::RealmanArmAdapter &robot) {
    rm_control::interfaces::ToolModbusBusConfiguration configuration;
    configuration.port = 1;
    configuration.baudrate = 115200;
    configuration.voltage_type = 3;
    configuration.timeout_units = 10;
    configuration.reset_existing_mode = true;
    configuration.power_cycle_tool = true;
    configuration.power_off_delay_ms = 500;
    configuration.startup_delay_ms = 2000;
    const auto configured = robot.Configure(configuration);
    if (!configured.ok) {
        return rm_control::common::Result<void>::failure(
            "failed to initialize O6 tool Modbus session: " +
            configured.error_message);
    }
    std::cout << "O6 tool Modbus initialized: port="
              << configured.value.port << ", mode=" << configured.value.mode
              << ", baudrate=" << configured.value.baudrate
              << ", voltage_type=" << configured.value.voltage_type
              << ", timeout_units=" << configured.value.timeout_units << '\n';
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> ValidateDisplayedRobotSafety(
    const rm_control::common::RobotSafetyStatus &status) {
    if (!status.real_mode_enabled || !status.arm_power_on ||
        !status.all_joints_enabled || !status.arm_error_free ||
        !status.controller_error_free || status.controller_error_code != 0) {
        return rm_control::common::Result<void>::failure(
            "RM75 safety state rejected: real_mode=" +
            std::to_string(status.real_mode_enabled) + ", arm_power=" +
            std::to_string(status.arm_power_on) + ", joints_enabled=" +
            std::to_string(status.all_joints_enabled) + ", arm_error_free=" +
            std::to_string(status.arm_error_free) + ", controller_error_free=" +
            std::to_string(status.controller_error_free) + ", controller_error_code=" +
            std::to_string(status.controller_error_code) + ", virtual_wall=" +
            std::to_string(status.virtual_wall_enabled) + ", self_collision=" +
            std::to_string(status.self_collision_enabled));
    }
    if (status.emergency_stop_state_available && !status.emergency_stop_normal) {
        return rm_control::common::Result<void>::failure(
            "RM75 reports an abnormal emergency-stop state");
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> ValidateDisplayedForce(
    const rm_control::common::ForceData &force,
    const rm_control::config::DragTeachSafetyConfig &safety) {
    for (std::size_t index = 0; index < force.zero.size(); ++index) {
        const double value = std::fabs(static_cast<double>(force.zero[index]));
        const double limit = index < 3U
                                 ? safety.maximum_absolute_force_newton
                                 : safety.maximum_absolute_torque_newton_meter;
        if (!std::isfinite(value) || value > limit) {
            return rm_control::common::Result<void>::failure(
                "RM75 six-axis force/torque exceeds the configured preflight limit");
        }
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> VerifyLiveReadOnlyState(
    flight_robot::realman::RealmanArmAdapter &robot,
    flight_robot::linker_hand::LinkerHandAdapter &hand,
    const rm_control::config::DragTeachSafetyConfig &safety_config,
    const rm_control::config::TrajectoryO6TaskCliConfig &cli) {
    const auto info = robot.getRobotInfo();
    const auto software = robot.getRobotSoftwareInfo();
    const auto current = robot.getCurrentArmState();
    const auto safety = robot.getRobotSafetyStatus();
    const auto limits = robot.getJointLimits();
    const auto force = robot.getForceData();
    const auto program_slot =
        robot.isControllerProgramSlotAvailable(cli.program_id);
    auto return_program_slot = rm_control::common::Result<bool>::success(true);
    if (cli.return_from_trajectory_end) {
        return_program_slot =
            robot.isControllerProgramSlotAvailable(cli.return_program_id);
    }
    const auto readiness = hand.ReadReadiness();
    if (!info.ok || !software.ok || !current.ok || !safety.ok || !limits.ok ||
        !force.ok || !program_slot.ok || !return_program_slot.ok || !readiness.ok) {
        const std::string error = !info.ok ? info.error_message
                                  : !software.ok ? software.error_message
                                  : !current.ok ? current.error_message
                                  : !safety.ok ? safety.error_message
                                  : !limits.ok ? limits.error_message
                                  : !force.ok ? force.error_message
                                  : !program_slot.ok ? program_slot.error_message
                                  : !return_program_slot.ok
                                      ? return_program_slot.error_message
                                                     : readiness.error_message;
        return rm_control::common::Result<void>::failure(error);
    }
    const auto safety_validation = ValidateDisplayedRobotSafety(safety.value);
    const auto force_validation = ValidateDisplayedForce(force.value, safety_config);
    if (!safety_validation.ok || !force_validation.ok || !program_slot.value ||
        !return_program_slot.value) {
        return rm_control::common::Result<void>::failure(
            !safety_validation.ok ? safety_validation.error_message
            : !force_validation.ok ? force_validation.error_message
            : !program_slot.value
                ? "forward controller program slot is occupied; choose an unused --program-id"
                : "return controller program slot is occupied; choose an unused --return-program-id");
    }
    std::cout << "RM75 confirmed: dof=" << info.value.arm_dof
              << ", SDK=" << software.value.sdk_version
              << ", firmware=" << software.value.firmware_version << '\n'
              << "Current RM75 joints (deg): ";
    PrintJointValues(current.value.joints_deg);
    std::cout << '\n';
    PrintO6ProtocolValues("O6 current torque values", readiness.value.joint_torques);
    PrintO6ProtocolValues("O6 current speed values", readiness.value.joint_speeds);
    PrintO6Readiness(readiness.value);
    return rm_control::common::Result<void>::success();
}

rm_control::modules::trajectory::PlaybackReportO6Values ToPlaybackReportValues(
    const rm_control::interfaces::O6JointPose &values) {
    rm_control::modules::trajectory::PlaybackReportO6Values output{};
    std::copy(values.begin(), values.end(), output.begin());
    return output;
}

rm_control::modules::trajectory::ControllerManagedPlaybackReport BuildPlaybackReport(
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
    const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
    const rm_control::modules::trajectory::DragTrajectoryValidationReport &validation,
    const rm_control::core::ArmO6TrajectoryTaskReport &task_report) {
    rm_control::modules::trajectory::ControllerManagedPlaybackReport report;
    report.report_identifier =
        rm_control::modules::trajectory::timestampString();
    report.source_file = trajectory.source_file;
    report.source_file_sha256 = trajectory.source_file_sha256;
    report.trajectory_point_count = trajectory.trajectory_point_count;
    report.sample_period_ms = trajectory.sample_period_ms;
    report.source_duration_ms = validation.duration_ms;
    report.timestamp_is_derived = trajectory.timestamp_is_derived;
    report.plan_speed_percent = cli.plan_speed_percent;
    report.controller_program_id = cli.program_id;
    report.returned_from_trajectory_end = task_report.returned_from_trajectory_end;
    if (task_report.returned_from_trajectory_end) {
        report.return_source_file = return_trajectory.source_file;
        report.return_source_file_sha256 = return_trajectory.source_file_sha256;
        report.return_controller_program_id = cli.return_program_id;
        report.return_observed_execution_duration_ms =
            task_report.return_controller_timing.observed_execution_duration_ms;
        report.return_program_status_poll_count =
            task_report.return_controller_timing.poll_count;
        report.return_minimum_poll_period_ms =
            task_report.return_controller_timing.minimum_poll_period_ms;
        report.return_maximum_poll_period_ms =
            task_report.return_controller_timing.maximum_poll_period_ms;
        report.return_average_poll_period_ms =
            task_report.return_controller_timing.average_poll_period_ms;
        report.return_maximum_absolute_poll_jitter_ms =
            task_report.return_controller_timing.maximum_absolute_poll_jitter_ms;
        report.return_final_controller_state =
            task_report.return_final_program_state.raw_state;
    }
    report.observed_execution_duration_ms =
        task_report.controller_timing.observed_execution_duration_ms;
    report.program_status_poll_count = task_report.controller_timing.poll_count;
    report.minimum_poll_period_ms = task_report.controller_timing.minimum_poll_period_ms;
    report.maximum_poll_period_ms = task_report.controller_timing.maximum_poll_period_ms;
    report.average_poll_period_ms = task_report.controller_timing.average_poll_period_ms;
    report.maximum_absolute_poll_jitter_ms =
        task_report.controller_timing.maximum_absolute_poll_jitter_ms;
    report.final_controller_state = task_report.final_program_state.raw_state;
    report.o6_device_id = task_report.final_hand_readiness.device_id;
    report.target_positions = ToPlaybackReportValues(BuildO6ProtocolValues(cli.grip_pose));
    report.speed_commands = ToPlaybackReportValues(BuildO6ProtocolValues(cli.grip_speed));
    report.torque_commands = ToPlaybackReportValues(BuildO6ProtocolValues(cli.grip_torque));
    report.final_positions =
        ToPlaybackReportValues(task_report.final_hand_readiness.joint_positions);
    return report;
}

void PrintTaskSuccess(const rm_control::core::ArmO6TrajectoryTaskReport &report,
                      const std::filesystem::path &report_path) {
    std::cout << "Task completed: trajectory_points=" << report.trajectory_point_count
              << ", returned_from_trajectory_end="
              << (report.returned_from_trajectory_end ? "true" : "false");
    if (report.returned_from_trajectory_end) {
        std::cout << ", return_program_state="
                  << report.return_final_program_state.raw_state
                  << ", return_program_status_polls="
                  << report.return_controller_timing.poll_count
                  << ", return_execution_duration_ms="
                  << report.return_controller_timing.observed_execution_duration_ms;
    }
    std::cout
              << ", observed_running_state="
              << (report.observed_running_state ? "true" : "false")
              << ", final_program_state=" << report.final_program_state.raw_state
              << ", program_status_polls=" << report.controller_timing.poll_count
              << ", average_poll_period_ms="
              << report.controller_timing.average_poll_period_ms
              << ", maximum_poll_period_ms="
              << report.controller_timing.maximum_poll_period_ms
              << ", maximum_poll_jitter_ms="
              << report.controller_timing.maximum_absolute_poll_jitter_ms
              << ", observed_execution_duration_ms="
              << report.controller_timing.observed_execution_duration_ms
              << "\nFinal O6 positions: ";
    std::vector<float> positions(report.final_hand_readiness.joint_positions.begin(),
                                 report.final_hand_readiness.joint_positions.end());
    PrintJointValues(positions);
    std::cout << "\nPlayback report: " << report_path << '\n';
}

void PrintRequestedO6Motion(
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const rm_control::config::DragTeachSafetyConfig &safety) {
    PrintO6ProtocolValues(
        "Requested O6 target positions", BuildO6ProtocolValues(cli.grip_pose));
    PrintO6ProtocolValues(
        "Requested O6 speed commands", BuildO6ProtocolValues(cli.grip_speed));
    PrintO6ProtocolValues(
        "Requested O6 torque commands", BuildO6ProtocolValues(cli.grip_torque));
    std::cout << "Configured O6 maximum speed command: "
              << safety.maximum_o6_speed_command << '\n'
              << "Configured O6 maximum torque command: "
              << safety.maximum_o6_torque_command << '\n';
}

rm_control::common::Result<rm_control::core::HardwareExecutionGateInput>
ConfirmLiveExecution(
    flight_robot::realman::RealmanArmAdapter &robot,
    flight_robot::linker_hand::LinkerHandAdapter &hand,
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const rm_control::config::DragTeachSafetyConfig &safety,
    const std::string &forward_source_sha256,
    const std::string &return_source_sha256) {
    const auto read_only_state = VerifyLiveReadOnlyState(
        robot, hand, safety, cli);
    if (!read_only_state.ok) {
        return rm_control::common::Result<
            rm_control::core::HardwareExecutionGateInput>::failure(
                "live readiness failed before motion: " +
                read_only_state.error_message);
    }
    auto gate = BuildExecutionGate(
        cli, safety, robot.isConnected(), true, false);
    rm_control::core::HardwareExecutionGateRequirements requirements;
    requirements.required_speed_percent =
        std::max(cli.origin_speed_percent, cli.plan_speed_percent);
    requirements.require_confirmed_trajectory_sample_period = true;
    requirements.require_confirmed_vendor_raw_units_per_degree = true;
    const auto preliminary =
        rm_control::core::EvaluateHardwareExecutionGate(gate, requirements);
    if (PrintPreConfirmationRejections(preliminary)) {
        return rm_control::common::Result<
            rm_control::core::HardwareExecutionGateInput>::failure(
                "pre-confirmation hardware safety gate rejected execution");
    }
    if (StopWasRequestedByProcessSignal()) {
        return rm_control::common::Result<
            rm_control::core::HardwareExecutionGateInput>::failure(
                "stop requested before operator confirmation");
    }
    gate.operator_confirmed = RequestOperatorConfirmation(
        BuildConfirmationPhrase(forward_source_sha256, return_source_sha256));
    if (!gate.operator_confirmed) {
        return rm_control::common::Result<
            rm_control::core::HardwareExecutionGateInput>::failure(
                "operator confirmation mismatch");
    }
    return rm_control::common::Result<
        rm_control::core::HardwareExecutionGateInput>::success(gate);
}

int ExecuteConfirmedTask(
    flight_robot::realman::RealmanArmAdapter &robot,
    flight_robot::linker_hand::LinkerHandAdapter &hand,
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const OfflineTaskPreparation &preparation,
    const rm_control::core::HardwareExecutionGateInput &gate) {
    rm_control::core::ArmO6TrajectoryTask task(robot, hand);
    const auto result = cli.return_from_trajectory_end
        ? task.RunFromTrajectoryEnd(
              preparation.return_trajectory, preparation.trajectory,
              preparation.task_config, gate)
        : task.Run(preparation.trajectory, preparation.task_config, gate);
    if (!result.ok) {
        std::cerr << "RM75/O6 task failed: " << result.error_message << '\n';
        return kTaskFailureExitCode;
    }
    const auto playback_report = BuildPlaybackReport(
        cli, preparation.trajectory, preparation.return_trajectory,
        preparation.validation, result.value);
    const auto report_path =
        rm_control::modules::trajectory::
            WriteControllerManagedPlaybackReportWithoutOverwrite(
                playback_report,
                rm_control::modules::trajectory::
                    DefaultTrajectoryPlaybackReportDirectory());
    if (!report_path.ok) {
        std::cerr << "RM75/O6 task completed, but report persistence failed: "
                  << report_path.error_message << '\n';
        return kTaskFailureExitCode;
    }
    PrintTaskSuccess(result.value, report_path.value);
    return 0;
}

int RunLiveTask(
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const rm_control::config::DragTeachSafetyConfig &safety,
    const OfflineTaskPreparation &preparation) {
    const auto source_integrity =
        rm_control::core::VerifySourceFileIntegrity(preparation.trajectory);
    const auto return_integrity = !preparation.return_from_trajectory_end
        ? rm_control::common::Result<void>::success()
        : rm_control::core::VerifySourceFileIntegrity(
              preparation.return_trajectory);
    if (!source_integrity.ok || !return_integrity.ok) {
        std::cerr << "Hardware execution refused before connection: "
                  << (!source_integrity.ok ? source_integrity.error_message
                                           : return_integrity.error_message)
                  << '\n';
        return kHardwareExecutionRefusedExitCode;
    }
    if (!safety.hardware_execution_enabled) {
        std::cerr << "Hardware execution refused: hardware_execution_enabled is false.\n";
        return kHardwareExecutionRefusedExitCode;
    }
    const auto signals = InstallStopSignalHandlers();
    if (!signals.ok) {
        std::cerr << "Hardware execution refused: " << signals.error_message << '\n';
        return kHardwareExecutionRefusedExitCode;
    }
    flight_robot::realman::RealmanArmAdapter robot;
    const auto connected = robot.connect(rm_control::common::RobotConnectionConfig{});
    if (!connected.ok) {
        std::cerr << "Failed to connect RM75: " << connected.error_message << '\n';
        return kConnectionFailureExitCode;
    }
    const auto tool_bus = InitializeO6ToolModbusSession(robot);
    if (!tool_bus.ok) {
        std::cerr << "Hardware execution refused: "
                  << tool_bus.error_message << '\n';
        return kHardwareExecutionRefusedExitCode;
    }
    flight_robot::linker_hand::LinkerHandAdapter hand(
        robot, ConvertHandSide(cli.o6_side));
    const auto gate = ConfirmLiveExecution(
        robot, hand, cli, safety, preparation.trajectory.source_file_sha256,
        preparation.return_from_trajectory_end
            ? preparation.return_trajectory.source_file_sha256
            : std::string{});
    if (!gate.ok) {
        std::cerr << "Hardware execution refused: " << gate.error_message << '\n';
        return kHardwareExecutionRefusedExitCode;
    }
    return ExecuteConfirmedTask(
        robot, hand, cli, preparation, gate.value);
}

rm_control::common::Result<OfflineTaskPreparation> PrepareOfflineTask(
    const rm_control::config::TrajectoryO6TaskCliConfig &cli,
    const rm_control::config::DragTeachSafetyConfig &safety) {
    std::error_code path_error;
    const auto trajectory_path = BuildAbsoluteTrajectoryPath(
        cli.trajectory_path, &path_error);
    if (path_error) {
        return rm_control::common::Result<OfflineTaskPreparation>::failure(
            "failed to resolve trajectory path: " + path_error.message());
    }
    const auto trajectory =
        rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(
            trajectory_path, cli.sample_period_ms,
            safety.vendor_raw_units_per_degree);
    if (!trajectory.ok) {
        return rm_control::common::Result<OfflineTaskPreparation>::failure(
            "trajectory parse failed: " + trajectory.error_message);
    }
    const auto validation = rm_control::modules::trajectory::ValidateDragTrajectory(
        trajectory.value.samples, BuildValidationLimits(safety));
    if (!validation.ok) {
        return rm_control::common::Result<OfflineTaskPreparation>::failure(
            "trajectory validation failed: " + validation.error_message);
    }
    const auto task_config = BuildTaskConfig(
        cli, safety, validation.value.duration_ms);
    const auto config_validation = cli.return_from_trajectory_end
        ? rm_control::core::ValidateArmO6RoundTripTaskConfig(task_config)
        : rm_control::core::ValidateArmO6TrajectoryTaskConfig(task_config);
    if (!config_validation.ok) {
        return rm_control::common::Result<OfflineTaskPreparation>::failure(
            "task configuration validation failed: " +
            config_validation.error_message);
    }

    OfflineTaskPreparation preparation;
    preparation.trajectory = trajectory.value;
    preparation.validation = validation.value;
    preparation.task_config = task_config;
    preparation.return_from_trajectory_end = cli.return_from_trajectory_end;
    if (cli.return_from_trajectory_end) {
        const auto return_trajectory = rm_control::modules::trajectory::
            CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
                trajectory.value,
                rm_control::modules::trajectory::
                    DefaultReversedVendorTrajectoryDirectory());
        if (!return_trajectory.ok) {
            return rm_control::common::Result<OfflineTaskPreparation>::failure(
                "failed to create return trajectory: " +
                return_trajectory.error_message);
        }
        const auto return_validation =
            rm_control::modules::trajectory::ValidateDragTrajectory(
                return_trajectory.value.samples, BuildValidationLimits(safety));
        if (!return_validation.ok) {
            return rm_control::common::Result<OfflineTaskPreparation>::failure(
                "return trajectory validation failed: " +
                return_validation.error_message);
        }
        preparation.return_trajectory = return_trajectory.value;
        preparation.return_validation = return_validation.value;
    }
    return rm_control::common::Result<OfflineTaskPreparation>::success(preparation);
}

} // namespace

/**
 * @brief 安全执行 RM75 本地轨迹与 O6 夹握集成任务。
 * @reason 按离线校验、只读硬件确认、人工二次确认、统一状态机的顺序 fail-closed。
 * @author 唐永康
 * @date 2026-07-11
 * @param argc 命令行参数数量。
 * @param argv 命令行参数数组，全部外部输入在硬件连接前校验。
 * @return 0 表示帮助、DryRun 或完整任务成功；其他值表示输入、门控、连接或任务失败。
 * @calls ParseVendorDragTrajectoryJsonLines、RealmanArmAdapter、LinkerHandAdapter、ArmO6TrajectoryTask。
 * @safety 默认 DryRun；只有 --live、配置、设备状态、轨迹校验和动态人工口令全部满足才运动。
 * @note 本程序不会自动配置工具电压/RS485，不覆盖源轨迹或已有控制器程序，也不自动重试 O6 写入。
 */
int main(int argc, char **argv) {
    const auto cli = rm_control::config::ParseTrajectoryO6TaskCliArguments(argc, argv);
    if (!cli.ok) {
        std::cerr << "Invalid arguments: " << cli.error_message << '\n';
        rm_control::config::PrintTrajectoryO6TaskUsage(
            argv == nullptr ? nullptr : argv[0], std::cerr);
        return kInvalidArgumentsExitCode;
    }
    if (cli.value.help_requested) {
        rm_control::config::PrintTrajectoryO6TaskUsage(
            argv == nullptr ? nullptr : argv[0], std::cout);
        return 0;
    }

    const auto safety = rm_control::config::LoadDragTeachSafetyConfig(
        cli.value.safety_config_path);
    if (!safety.ok) {
        std::cerr << safety.error_message << '\n';
        return kInvalidInputExitCode;
    }
    const auto preparation = PrepareOfflineTask(cli.value, safety.value);
    if (!preparation.ok) {
        std::cerr << preparation.error_message << '\n';
        return kInvalidInputExitCode;
    }
    PrintOfflineReport(
        "Forward trajectory", preparation.value.trajectory,
        preparation.value.validation);
    if (preparation.value.return_from_trajectory_end) {
        PrintOfflineReport(
            "Return trajectory", preparation.value.return_trajectory,
            preparation.value.return_validation);
        std::cout << "Round-trip controller programs: return="
                  << cli.value.return_program_id
                  << ", forward=" << cli.value.program_id << '\n';
    }
    PrintRequestedO6Motion(cli.value, safety.value);
    if (!cli.value.live_requested) {
        std::cout << "DryRun complete: no device was connected and no motion was executed.\n";
        return 0;
    }
    return RunLiveTask(cli.value, safety.value, preparation.value);
}
