/**
 * @file drag_teach_record_main.cpp
 * @brief RM75-6F 拖动示教原始轨迹采集程序入口。
 *
 * @author 唐永康
 * @date 2026-07-10
 *
 * @details
 * Application 只负责 CLI、配置、依赖组装、人工确认和进程退出码。型号/状态预检、
 * 拖动状态机、内存采样、轨迹校验、唯一目录、SHA-256 和 manifest 均由服务层完成。
 */
#include "flight_robot/realman/realman_arm_adapter.h"
#include "rm_control/config/drag_teach_cli_config.h"
#include "rm_control/config/drag_teach_safety_config.h"
#include "rm_control/core/drag_teach_service.h"

#include <chrono>
#include <cstddef>
#include <iostream>
#include <string>
#include <thread>

namespace {

constexpr int kInvalidArgumentsExitCode = 64;
constexpr int kInvalidSafetyConfigExitCode = 65;
constexpr int kHardwareExecutionRefusedExitCode = 77;
constexpr int kConnectionFailureExitCode = 1;
constexpr int kRecordingFailureExitCode = 2;
constexpr int kMinimumRecordingDurationSeconds = 1;
constexpr int kMaximumRecordingDurationSeconds = 3600;
constexpr int kMinimumSamplePeriodMilliseconds = 10;
constexpr int kMaximumSamplePeriodMilliseconds = 1000;
constexpr const char *kOperatorConfirmationPhrase = "CONFIRM RM75 DRAG TEACH";

rm_control::common::Result<void> ValidateApplicationRecordingConfig(
    const rm_control::config::DragTeachRecordCliConfig &config) {
    if (config.record_config.duration_seconds < kMinimumRecordingDurationSeconds ||
        config.record_config.duration_seconds > kMaximumRecordingDurationSeconds) {
        return rm_control::common::Result<void>::failure(
            "duration_seconds must be in range 1..3600");
    }
    if (config.record_config.sample_period_ms < kMinimumSamplePeriodMilliseconds ||
        config.record_config.sample_period_ms > kMaximumSamplePeriodMilliseconds) {
        return rm_control::common::Result<void>::failure(
            "sample_period_ms must be in range 10..1000");
    }
    if (config.record_config.storage_root.empty() ||
        config.record_config.session_name.empty()) {
        return rm_control::common::Result<void>::failure(
            "storage_root and session_name must not be empty");
    }
    if (!config.record_config.save_vendor_trajectory) {
        return rm_control::common::Result<void>::failure(
            "DragTeachService requires vendor raw trajectory saving; remove --no-vendor-save");
    }
    return rm_control::common::Result<void>::success();
}

bool IsO6ConnectionVerifiedByDeviceService() noexcept {
    // 当前 RM 独立子工程尚未接入 IDexterousHand 设备服务。安全规则要求真实 O6
    // 连接必须来自实际状态读取，因此这里保持 false，禁止用 CLI 或环境变量伪造。
    return false;
}

bool RequestSecondOperatorConfirmation(std::istream &input, std::ostream &output) {
    output << "Type exactly '" << kOperatorConfirmationPhrase
           << "' to authorize RM75 drag teach: " << std::flush;
    std::string confirmation;
    return std::getline(input, confirmation) &&
           confirmation == kOperatorConfirmationPhrase;
}

rm_control::core::DragTeachServiceConfig BuildServiceConfig(
    const rm_control::config::DragTeachRecordCliConfig &cli,
    const rm_control::config::DragTeachSafetyConfig &safety) {
    rm_control::core::DragTeachServiceConfig config;
    config.storage_root = cli.record_config.storage_root;
    config.session_name = cli.record_config.session_name;
    config.sample_period =
        std::chrono::milliseconds(cli.record_config.sample_period_ms);
    config.drag_options = cli.record_config.drag_options;
    config.validation_limits.maximum_joint_jump_deg = safety.max_joint_jump_deg;
    config.validation_limits.maximum_joint_velocity_deg_per_second =
        safety.max_joint_velocity_deg_per_second;
    config.validation_limits.maximum_joint_acceleration_deg_per_second_squared =
        safety.max_joint_acceleration_deg_per_second_squared;
    config.maximum_absolute_force_newton = safety.maximum_absolute_force_newton;
    config.maximum_absolute_torque_newton_meter =
        safety.maximum_absolute_torque_newton_meter;
    const auto expected_sample_count = static_cast<std::size_t>(
        (cli.record_config.duration_seconds * 1000) /
        cli.record_config.sample_period_ms);
    config.maximum_sample_count = expected_sample_count + 2U;
    return config;
}

rm_control::core::HardwareExecutionGateInput BuildExecutionGate(
    const rm_control::config::DragTeachRecordCliConfig &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;
    gate.native_replay_without_speed_control_accepted =
        safety.native_replay_without_speed_control_accepted;
    gate.native_replay_speed_within_config_verified = false;
    return gate;
}

void PrintArtifactSummary(
    const rm_control::modules::trajectory::RawTrajectoryArtifact &artifact) {
    std::cout << "Trajectory points: " << artifact.metadata.trajectory_point_count << '\n'
              << "Recorded samples: " << artifact.metadata.recorded_sample_count << '\n'
              << "Session directory: " << artifact.paths.directory << '\n'
              << "Raw trajectory: " << artifact.paths.raw_trajectory << '\n'
              << "Manifest: " << artifact.paths.manifest << '\n'
              << "Raw bytes: " << artifact.raw_file_size_bytes << '\n'
              << "SHA-256: " << artifact.raw_file_sha256 << '\n'
              << "Save return code: "
              << artifact.metadata.vendor_save_return_code << '\n';
}

} // namespace

/**
 * @brief 安全地执行一次 RM75-6F 拖动示教原始轨迹采集。
 *
 * @reason 入口按 fail-closed 顺序装配所有独立授权条件，任何缺项都在运动 API 前退出。
 *
 * @author 唐永康
 * @date 2026-07-10
 *
 * @param argc 命令行参数数量。
 * @param argv 命令行参数数组；所有外部值在使用前校验。
 * @return 0 表示帮助、DryRun 检查或采集成功；其他值表示参数、门控、连接或采集失败。
 *
 * @calls ParseDragTeachRecordCliArguments、LoadDragTeachSafetyConfig、
 *        RealmanArmAdapter::connect 和 DragTeachService。
 * @safety 只有 `--live`、启用配置、真实设备状态、人工二次确认等条件全部满足才会启动拖动。
 * @note 当前入口没有 O6 设备服务，因此 Live 模式会明确 fail-closed；单元测试通过 Mock 覆盖完整流程。
 */
int main(int argc, char **argv) {
    const auto cli = rm_control::config::ParseDragTeachRecordCliArguments(argc, argv);
    if (!cli.ok) {
        std::cerr << "Invalid arguments: " << cli.error_message << '\n';
        rm_control::config::PrintDragTeachRecordUsage(
            argv == nullptr ? nullptr : argv[0], std::cerr);
        return kInvalidArgumentsExitCode;
    }
    if (cli.value.help_requested) {
        rm_control::config::PrintDragTeachRecordUsage(
            argv == nullptr ? nullptr : argv[0], std::cout);
        return 0;
    }

    const auto application_config = ValidateApplicationRecordingConfig(cli.value);
    const auto safety = rm_control::config::LoadDragTeachSafetyConfig(
        cli.value.safety_config_path);
    if (!application_config.ok || !safety.ok) {
        std::cerr << (!application_config.ok ? application_config.error_message
                                             : safety.error_message)
                  << '\n';
        return kInvalidSafetyConfigExitCode;
    }
    if (!cli.value.live_requested) {
        std::cout << "DryRun: configuration validated; no device was connected and no motion was executed.\n";
        return 0;
    }
    if (!safety.value.hardware_execution_enabled) {
        std::cerr << "Hardware execution refused: hardware_execution_enabled is false.\n";
        return kHardwareExecutionRefusedExitCode;
    }

    flight_robot::realman::RealmanArmAdapter robot;
    const rm_control::common::RobotConnectionConfig connection;
    const auto connected = robot.connect(connection);
    if (!connected.ok) {
        std::cerr << "Failed to connect RM75: " << connected.error_message << '\n';
        return kConnectionFailureExitCode;
    }

    const bool hand_connected = IsO6ConnectionVerifiedByDeviceService();
    const bool operator_confirmed =
        hand_connected && RequestSecondOperatorConfirmation(std::cin, std::cout);
    const auto gate = BuildExecutionGate(cli.value, safety.value,
                                         robot.isConnected(), hand_connected,
                                         operator_confirmed);
    rm_control::core::DragTeachService service(robot);
    const auto start = service.StartRecording(
        BuildServiceConfig(cli.value, safety.value), gate);
    if (!start.ok) {
        std::cerr << "Drag teach start refused: " << start.error_message << '\n';
        return kHardwareExecutionRefusedExitCode;
    }

    std::cout << "Recording started. Manually drag the RM75 under direct supervision.\n";
    std::this_thread::sleep_for(
        std::chrono::seconds(cli.value.record_config.duration_seconds));
    const auto stopped = service.StopRecording();
    if (!stopped.ok) {
        std::cerr << "Drag teach stop/validation failed: "
                  << stopped.error_message << '\n';
        return kRecordingFailureExitCode;
    }
    const auto artifact = service.SaveRawTrajectory();
    if (!artifact.ok) {
        std::cerr << "Raw trajectory finalization failed: "
                  << artifact.error_message << '\n';
        return kRecordingFailureExitCode;
    }
    PrintArtifactSummary(artifact.value);
    return artifact.value.raw_file_present &&
                   artifact.value.metadata.vendor_save_return_code == 0
               ? 0
               : kRecordingFailureExitCode;
}
