#include "rm_control/core/drag_teach_service.h"

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

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <exception>

namespace rm_control::core {
namespace {

constexpr std::chrono::milliseconds kMinimumSamplePeriod{10};
constexpr std::chrono::milliseconds kMaximumSamplePeriod{1000};

bool AllFinite(const std::vector<float> &values) {
    return std::all_of(values.begin(), values.end(), [](float value) {
        return std::isfinite(static_cast<double>(value));
    });
}

bool AllFinite(const common::Pose &pose) {
    return std::isfinite(static_cast<double>(pose.x)) &&
           std::isfinite(static_cast<double>(pose.y)) &&
           std::isfinite(static_cast<double>(pose.z)) &&
           std::isfinite(static_cast<double>(pose.rx)) &&
           std::isfinite(static_cast<double>(pose.ry)) &&
           std::isfinite(static_cast<double>(pose.rz));
}

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(static_cast<double>(value));
    });
}

common::TrajectorySample MakeSample(
    std::size_t sample_index,
    long long elapsed_ms,
    const common::ArmState &state) {
    common::TrajectorySample sample;
    sample.sample_index = sample_index;
    sample.elapsed_ms = elapsed_ms;
    sample.joints_deg = state.joints_deg;
    sample.tcp_pose = state.tcp_pose;
    return sample;
}

std::string StateName(DragTeachServiceState state) {
    switch (state) {
    case DragTeachServiceState::Idle: return "Idle";
    case DragTeachServiceState::Recording: return "Recording";
    case DragTeachServiceState::Recorded: return "Recorded";
    case DragTeachServiceState::Saved: return "Saved";
    case DragTeachServiceState::AtOrigin: return "AtOrigin";
    case DragTeachServiceState::Replaying: return "Replaying";
    case DragTeachServiceState::Paused: return "Paused";
    case DragTeachServiceState::Stopped: return "Stopped";
    case DragTeachServiceState::Faulted: return "Faulted";
    }
    return "Unknown";
}

common::Result<void> ValidateJointLimitSnapshot(
    const common::RobotJointLimits &limits,
    const DragTeachServiceConfig &config) {
    for (std::size_t joint_index = 0;
         joint_index < common::kRm75JointCount;
         ++joint_index) {
        const double minimum = limits.minimum_position_deg[joint_index];
        const double maximum = limits.maximum_position_deg[joint_index];
        const double maximum_speed = limits.maximum_speed_deg_per_second[joint_index];
        const double maximum_acceleration =
            limits.maximum_acceleration_deg_per_second_squared[joint_index];
        if (!std::isfinite(minimum) || !std::isfinite(maximum) ||
            !std::isfinite(maximum_speed) || !std::isfinite(maximum_acceleration) ||
            minimum >= maximum || maximum_speed <= 0.0 || maximum_acceleration <= 0.0) {
            return common::Result<void>::failure(
                "invalid controller limits for RM75 joint " +
                std::to_string(joint_index + 1));
        }
        if (config.validation_limits.maximum_joint_velocity_deg_per_second >
                maximum_speed ||
            config.validation_limits.maximum_joint_acceleration_deg_per_second_squared >
                maximum_acceleration) {
            return common::Result<void>::failure(
                "configured trajectory limits exceed controller limits for RM75 joint " +
                std::to_string(joint_index + 1));
        }
    }
    return common::Result<void>::success();
}

} // namespace

DragTeachService::DragTeachService(interfaces::IRobotArm &robot)
    : robot_(robot) {}

DragTeachService::~DragTeachService() noexcept {
    try {
        RequestSamplingStop();
        (void)JoinSamplingThread();
        (void)StopDragTeachIfActive();

        bool native_motion_possibly_active = false;
        {
            std::lock_guard<std::mutex> lock(mutex_);
            native_motion_possibly_active = native_motion_possibly_active_;
        }
        const DragTeachServiceState current_state = state();
        if (native_motion_possibly_active ||
            current_state == DragTeachServiceState::Replaying ||
            current_state == DragTeachServiceState::Paused) {
            (void)robot_.stopDragTrajectoryReplay();
        }
    } catch (...) {
        // 析构只能 best-effort 执行减险停止，禁止异常越过析构边界。
    }
}

common::Result<void> DragTeachService::StartRecording(
    const DragTeachServiceConfig &config,
    const HardwareExecutionGateInput &gate_input) {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const auto state_result = RequireState(DragTeachServiceState::Idle, "StartRecording");
    if (!state_result.ok) {
        return state_result;
    }
    const auto config_result = ValidateRecordingConfig(config);
    if (!config_result.ok) {
        return config_result;
    }

    config_ = config;
    const auto preflight_result = ReadAndValidateRobotPreflight(config);
    if (!preflight_result.ok) {
        return preflight_result;
    }
    const auto permission = RequireHardwareExecutionPermission(gate_input);
    if (!permission.ok) {
        return common::Result<void>::failure(
            "StartRecording refused: " + permission.error_message);
    }

    {
        std::lock_guard<std::mutex> lock(mutex_);
        samples_.clear();
        sampling_error_.clear();
        sampling_failed_.store(false);
        validation_report_available_ = false;
        artifact_available_ = false;
    }
    return StartHardwareAndSamplingThread();
}

common::Result<StopRecordingSummary> DragTeachService::StopRecording() {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const DragTeachServiceState current_state = state();
    if (current_state != DragTeachServiceState::Recording &&
        current_state != DragTeachServiceState::Faulted) {
        return common::Result<StopRecordingSummary>::failure(
            "StopRecording requires Recording or Faulted state, actual=" +
            StateName(current_state));
    }

    RequestSamplingStop();
    const auto join_result = JoinSamplingThread();
    if (!join_result.ok) {
        state_ = DragTeachServiceState::Faulted;
        return common::Result<StopRecordingSummary>::failure(
            join_result.error_message);
    }
    const auto stop_result = StopDragTeachIfActive();
    if (!stop_result.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        state_ = DragTeachServiceState::Faulted;
        return common::Result<StopRecordingSummary>::failure(stop_result.error_message);
    }

    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (sampling_failed_.load()) {
            state_ = DragTeachServiceState::Faulted;
            return common::Result<StopRecordingSummary>::failure(
                sampling_error_.empty() ? "drag teach sampling failed without details"
                                        : sampling_error_);
        }
    }
    return ValidateStoppedRecording();
}

common::Result<modules::trajectory::RawTrajectoryArtifact>
DragTeachService::SaveRawTrajectory() {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const auto state_result = RequireState(DragTeachServiceState::Recorded,
                                           "SaveRawTrajectory");
    if (!state_result.ok) {
        return common::Result<modules::trajectory::RawTrajectoryArtifact>::failure(
            state_result.error_message);
    }

    const auto paths = modules::trajectory::CreateUniqueRawTrajectorySession(
        config_.storage_root, config_.session_name);
    if (!paths.ok) {
        return common::Result<modules::trajectory::RawTrajectoryArtifact>::failure(
            paths.error_message);
    }

    const common::VendorTrajectorySaveAttempt save_attempt =
        robot_.saveDragTeachTrajectory(paths.value.raw_trajectory);
    const auto csv_result = modules::trajectory::writeTrajectoryCsvWithoutOverwrite(
        paths.value.synchronized_csv, samples_);
    modules::trajectory::RawTrajectoryManifestMetadata metadata;
    metadata.robot_model = "RM75-6F (arm_model=" +
                           std::to_string(robot_info_.arm_model) + ")";
    metadata.sdk_version = software_info_.sdk_version;
    metadata.firmware_version = software_info_.firmware_version;
    metadata.trajectory_point_count = save_attempt.point_count > 0
                                          ? static_cast<std::uint64_t>(save_attempt.point_count)
                                          : 0U;
    metadata.recorded_sample_count = static_cast<std::uint64_t>(samples_.size());
    metadata.vendor_save_api_called = save_attempt.api_called;
    metadata.vendor_save_return_code = save_attempt.return_code;
    metadata.vendor_save_error_message = save_attempt.error_message;
    metadata.synchronized_csv_write_succeeded = csv_result.ok;
    const auto artifact = modules::trajectory::FinalizeRawTrajectorySession(
        paths.value, metadata);
    if (!artifact.ok) {
        return artifact;
    }

    {
        std::lock_guard<std::mutex> lock(mutex_);
        last_artifact_ = artifact.value;
        artifact_available_ = true;
        state_ = save_attempt.api_called && save_attempt.return_code == 0 &&
                         artifact.value.raw_file_present && csv_result.ok
                     ? DragTeachServiceState::Saved
                     : DragTeachServiceState::Recorded;
    }
    if (!csv_result.ok) {
        return common::Result<modules::trajectory::RawTrajectoryArtifact>::failure(
            csv_result.error_message + "; manifest=" +
            artifact.value.paths.manifest.string());
    }
    return artifact;
}

common::Result<void> DragTeachService::MoveToTrajectoryOrigin(
    const HardwareExecutionGateInput &gate_input) {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const DragTeachServiceState current_state = state();
    if (current_state != DragTeachServiceState::Saved &&
        current_state != DragTeachServiceState::Stopped) {
        return common::Result<void>::failure(
            "MoveToTrajectoryOrigin requires Saved or Stopped state, actual=" +
            StateName(current_state));
    }
    const auto permission = RequirePermission(gate_input, OriginRequirements());
    if (!permission.ok) {
        return permission;
    }
    const auto preflight = ValidateRuntimeMotionPreflight();
    if (!preflight.ok) {
        return preflight;
    }
    {
        std::lock_guard<std::mutex> lock(mutex_);
        native_motion_possibly_active_ = true;
    }
    const auto result = robot_.moveToDragTrajectoryOrigin(true);
    if (result.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        native_motion_possibly_active_ = false;
        state_ = DragTeachServiceState::AtOrigin;
    }
    return result;
}

common::Result<void> DragTeachService::ReplayLastDragTrajectory(
    const HardwareExecutionGateInput &gate_input) {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const auto state_result = RequireState(DragTeachServiceState::AtOrigin,
                                           "ReplayLastDragTrajectory");
    if (!state_result.ok) {
        return state_result;
    }
    const auto permission = RequirePermission(gate_input, ReplayRequirements());
    if (!permission.ok) {
        return permission;
    }
    const auto preflight = ValidateRuntimeMotionPreflight();
    if (!preflight.ok) {
        return preflight;
    }
    const auto origin_match = ValidateCurrentStateMatchesTrajectoryOrigin();
    if (!origin_match.ok) {
        return origin_match;
    }
    {
        std::lock_guard<std::mutex> lock(mutex_);
        native_motion_possibly_active_ = true;
    }
    const auto result = robot_.replayLastDragTrajectory(false);
    if (result.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        state_ = DragTeachServiceState::Replaying;
    }
    return result;
}

common::Result<void> DragTeachService::PauseReplay() {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const auto state_result = RequireState(DragTeachServiceState::Replaying,
                                           "PauseReplay");
    if (!state_result.ok) {
        return state_result;
    }
    const auto result = robot_.pauseDragTrajectoryReplay();
    if (result.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        state_ = DragTeachServiceState::Paused;
    }
    return result;
}

common::Result<void> DragTeachService::ContinueReplay(
    const HardwareExecutionGateInput &gate_input) {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const auto state_result = RequireState(DragTeachServiceState::Paused,
                                           "ContinueReplay");
    if (!state_result.ok) {
        return state_result;
    }
    const auto permission = RequirePermission(gate_input, ReplayRequirements());
    if (!permission.ok) {
        return permission;
    }
    const auto preflight = ValidateRuntimeMotionPreflight();
    if (!preflight.ok) {
        return preflight;
    }
    const auto result = robot_.continueDragTrajectoryReplay();
    if (result.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        state_ = DragTeachServiceState::Replaying;
    }
    return result;
}

common::Result<void> DragTeachService::StopReplay() {
    std::lock_guard<std::mutex> operation_lock(operation_mutex_);
    const DragTeachServiceState current_state = state();
    bool native_motion_possibly_active = false;
    {
        std::lock_guard<std::mutex> lock(mutex_);
        native_motion_possibly_active = native_motion_possibly_active_;
    }
    if (!native_motion_possibly_active &&
        current_state != DragTeachServiceState::Replaying &&
        current_state != DragTeachServiceState::Paused) {
        return common::Result<void>::failure(
            "StopReplay requires Replaying or Paused state, actual=" +
            StateName(current_state));
    }
    const auto result = robot_.stopDragTrajectoryReplay();
    if (result.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        native_motion_possibly_active_ = false;
        state_ = DragTeachServiceState::Stopped;
    }
    return result;
}

DragTeachServiceState DragTeachService::state() const noexcept {
    return state_.load();
}

std::size_t DragTeachService::recordedSampleCount() const {
    std::lock_guard<std::mutex> lock(mutex_);
    return samples_.size();
}

common::Result<void> DragTeachService::ValidateRecordingConfig(
    const DragTeachServiceConfig &config) const {
    if (config.storage_root.empty() || config.session_name.empty()) {
        return common::Result<void>::failure(
            "drag teach storage_root and session_name must not be empty");
    }
    if (config.sample_period < kMinimumSamplePeriod ||
        config.sample_period > kMaximumSamplePeriod) {
        return common::Result<void>::failure(
            "drag teach sample_period must be in range 10..1000 ms");
    }
    const auto &limits = config.validation_limits;
    if (!std::isfinite(limits.maximum_joint_jump_deg) ||
        !std::isfinite(limits.maximum_joint_velocity_deg_per_second) ||
        !std::isfinite(limits.maximum_joint_acceleration_deg_per_second_squared) ||
        limits.maximum_joint_jump_deg <= 0.0 ||
        limits.maximum_joint_velocity_deg_per_second <= 0.0 ||
        limits.maximum_joint_acceleration_deg_per_second_squared <= 0.0) {
        return common::Result<void>::failure(
            "drag trajectory validation limits must be finite and greater than zero");
    }
    if (!std::isfinite(config.maximum_absolute_force_newton) ||
        !std::isfinite(config.maximum_absolute_torque_newton_meter) ||
        config.maximum_absolute_force_newton <= 0.0 ||
        config.maximum_absolute_torque_newton_meter <= 0.0) {
        return common::Result<void>::failure(
            "force and torque safety thresholds must be finite and greater than zero");
    }
    if (config.maximum_sample_count < 2U) {
        return common::Result<void>::failure(
            "maximum_sample_count must allow at least two samples");
    }
    return common::Result<void>::success();
}

common::Result<void> DragTeachService::ReadAndValidateRobotPreflight(
    const DragTeachServiceConfig &config) {
    if (!robot_.isConnected()) {
        return common::Result<void>::failure(
            "DragTeachService requires an active robot connection");
    }
    const auto info = robot_.getRobotInfo();
    if (!info.ok) {
        return common::Result<void>::failure(info.error_message);
    }
    const auto identity = ValidateRobotIdentity(info.value, config.drag_options);
    if (!identity.ok) {
        return identity;
    }
    const auto software = robot_.getRobotSoftwareInfo();
    const auto safety = robot_.getRobotSafetyStatus();
    const auto joint_limits = robot_.getJointLimits();
    if (!software.ok || !safety.ok || !joint_limits.ok) {
        return common::Result<void>::failure(
            !software.ok ? software.error_message
                         : !safety.ok ? safety.error_message
                                      : joint_limits.error_message);
    }
    const auto safety_validation = ValidateRobotSafetyStatus(safety.value);
    if (!safety_validation.ok) {
        return safety_validation;
    }
    const auto limit_validation = ValidateJointLimitSnapshot(joint_limits.value, config);
    if (!limit_validation.ok) {
        return limit_validation;
    }
    robot_info_ = info.value;
    software_info_ = software.value;
    joint_limits_ = joint_limits.value;
    return ValidateCurrentStateAndForce();
}

common::Result<void> DragTeachService::ValidateRobotIdentity(
    const common::RobotInfo &info,
    const common::DragTeachStartOptions &options) const {
    if (!info.is_rm75 ||
        info.arm_dof != static_cast<int>(common::kRm75JointCount)) {
        return common::Result<void>::failure(
            "drag teach requires an RM75 with exactly seven joints");
    }
    if (options.mode == common::DragTeachMode::SixDofForcePositionAndOrientation &&
        !info.six_axis_force_sensor_available) {
        return common::Result<void>::failure(
            "six-dof force drag teach requires a six-axis force sensor");
    }
    if (options.mode == common::DragTeachMode::SixDofForcePositionAndOrientation &&
        info.controller_version != 3) {
        return common::Result<void>::failure(
            "rm_start_multi_drag_teach requires a generation-3 controller");
    }
    return common::Result<void>::success();
}

common::Result<void> DragTeachService::ValidateRobotSafetyStatus(
    const common::RobotSafetyStatus &status) const {
    if (!status.real_mode_enabled || !status.arm_power_on) {
        return common::Result<void>::failure(
            "robot preflight requires real mode and arm power on");
    }
    if (!status.all_joints_enabled || !status.arm_error_free ||
        !status.controller_error_free) {
        return common::Result<void>::failure(
            "robot preflight found disabled joints or nonzero errors");
    }
    if (!status.virtual_wall_enabled) {
        return common::Result<void>::failure(
            "robot preflight requires the configured virtual wall to be enabled");
    }
    if (!status.self_collision_enabled) {
        return common::Result<void>::failure(
            "robot preflight requires self-collision detection to be enabled");
    }
    if (status.emergency_stop_state_available && !status.emergency_stop_normal) {
        return common::Result<void>::failure(
            "robot preflight reports an abnormal emergency-stop state");
    }
    return common::Result<void>::success();
}

common::Result<void> DragTeachService::ValidateCurrentStateAndForce() {
    const auto state_result = robot_.getCurrentArmState();
    const auto force_result = robot_.getForceData();
    if (!state_result.ok || !force_result.ok) {
        return common::Result<void>::failure(
            !state_result.ok ? state_result.error_message : force_result.error_message);
    }
    const auto state_validation = ValidateSampledArmState(state_result.value);
    if (!state_validation.ok) {
        return state_validation;
    }
    const common::ForceData &force = force_result.value;
    if (!AllFinite(force.raw) || !AllFinite(force.zero) ||
        !AllFinite(force.work_zero) || !AllFinite(force.tool_zero)) {
        return common::Result<void>::failure(
            "robot preflight force data contains a non-finite value");
    }
    for (std::size_t component_index = 0; component_index < 3U; ++component_index) {
        if (std::fabs(static_cast<double>(force.zero[component_index])) >
            config_.maximum_absolute_force_newton) {
            return common::Result<void>::failure(
                "robot preflight external force exceeds configured newton limit");
        }
    }
    for (std::size_t component_index = 3U; component_index < force.zero.size();
         ++component_index) {
        if (std::fabs(static_cast<double>(force.zero[component_index])) >
            config_.maximum_absolute_torque_newton_meter) {
            return common::Result<void>::failure(
                "robot preflight external torque exceeds configured newton-meter limit");
        }
    }
    return common::Result<void>::success();
}

common::Result<void> DragTeachService::ValidateRuntimeMotionPreflight() {
    if (!robot_.isConnected()) {
        return common::Result<void>::failure(
            "runtime motion preflight requires an active robot connection");
    }
    const auto info = robot_.getRobotInfo();
    const auto safety = robot_.getRobotSafetyStatus();
    const auto joint_limits = robot_.getJointLimits();
    if (!info.ok || !safety.ok || !joint_limits.ok) {
        return common::Result<void>::failure(
            !info.ok ? info.error_message
                     : !safety.ok ? safety.error_message
                                  : joint_limits.error_message);
    }
    const auto identity = ValidateRobotIdentity(info.value, config_.drag_options);
    const auto safety_validation = ValidateRobotSafetyStatus(safety.value);
    const auto limit_validation = ValidateJointLimitSnapshot(joint_limits.value, config_);
    if (!identity.ok || !safety_validation.ok || !limit_validation.ok) {
        return !identity.ok ? identity
                            : !safety_validation.ok ? safety_validation
                                                    : limit_validation;
    }
    joint_limits_ = joint_limits.value;
    return ValidateCurrentStateAndForce();
}

common::Result<void> DragTeachService::ValidateSampledArmState(
    const common::ArmState &state) const {
    if (state.joints_deg.size() != common::kRm75JointCount ||
        !AllFinite(state.joints_deg) || !AllFinite(state.tcp_pose) ||
        !state.error_codes.empty()) {
        return common::Result<void>::failure(
            "RM75 state requires seven finite joints, a finite TCP pose, and no arm errors");
    }
    for (std::size_t joint_index = 0;
         joint_index < common::kRm75JointCount;
         ++joint_index) {
        const float position = state.joints_deg[joint_index];
        if (position < joint_limits_.minimum_position_deg[joint_index] ||
            position > joint_limits_.maximum_position_deg[joint_index]) {
            return common::Result<void>::failure(
                "RM75 joint " + std::to_string(joint_index + 1) +
                " is outside the controller position limits");
        }
    }
    return common::Result<void>::success();
}

common::Result<void>
DragTeachService::ValidateCurrentStateMatchesTrajectoryOrigin() {
    const auto current = robot_.getCurrentArmState();
    if (!current.ok) {
        return common::Result<void>::failure(current.error_message);
    }
    const auto state_validation = ValidateSampledArmState(current.value);
    if (!state_validation.ok) {
        return state_validation;
    }
    if (samples_.empty()) {
        return common::Result<void>::failure(
            "cannot verify trajectory origin without recorded samples");
    }
    for (std::size_t joint_index = 0;
         joint_index < common::kRm75JointCount;
         ++joint_index) {
        const double difference = std::fabs(
            static_cast<double>(current.value.joints_deg[joint_index]) -
            static_cast<double>(samples_.front().joints_deg[joint_index]));
        if (difference > config_.validation_limits.maximum_joint_jump_deg) {
            return common::Result<void>::failure(
                "current RM75 state differs from trajectory origin at joint " +
                std::to_string(joint_index + 1));
        }
    }
    return common::Result<void>::success();
}

common::Result<void> DragTeachService::StartHardwareAndSamplingThread() {
    {
        std::lock_guard<std::mutex> lock(mutex_);
        // 通信超时不能证明控制器未执行，因此发送前就标记为“可能仍在拖动”。
        hardware_drag_active_ = true;
    }
    common::Result<void> start_result;
    try {
        start_result = robot_.startDragTeach(config_.drag_options);
    } catch (const std::exception &error) {
        start_result = common::Result<void>::failure(
            std::string("startDragTeach threw an exception: ") + error.what());
    } catch (...) {
        start_result = common::Result<void>::failure(
            "startDragTeach threw a non-standard exception");
    }
    if (!start_result.ok) {
        const auto stop_result = StopDragTeachIfActive();
        state_ = DragTeachServiceState::Faulted;
        return common::Result<void>::failure(
            start_result.error_message +
            (stop_result.ok ? "; stopDragTeach completed after uncertain start"
                            : "; stopDragTeach also failed: " +
                                  stop_result.error_message));
    }
    {
        std::lock_guard<std::mutex> lock(mutex_);
        state_ = DragTeachServiceState::Recording;
    }
    sampling_stop_requested_.store(false);
    try {
        sampling_thread_ = std::thread(&DragTeachService::SamplingLoop, this);
    } catch (const std::exception &error) {
        const auto stop_result = StopDragTeachIfActive();
        std::lock_guard<std::mutex> lock(mutex_);
        state_ = DragTeachServiceState::Faulted;
        return common::Result<void>::failure(
            std::string("failed to start drag sampling thread: ") + error.what() +
            (stop_result.ok ? "" : "; " + stop_result.error_message));
    }
    return common::Result<void>::success();
}

void DragTeachService::SamplingLoop() noexcept {
    try {
        RunSamplingLoop();
    } catch (const std::exception &error) {
        RecordSamplingFailure(error.what());
    } catch (...) {
        RecordSamplingFailure("unhandled non-standard sampling exception");
    }
}

void DragTeachService::RunSamplingLoop() {
    const auto start_time = std::chrono::steady_clock::now();
    std::size_t sample_index = 0;
    while (!sampling_stop_requested_.load()) {
        if (sample_index >= config_.maximum_sample_count) {
            RecordSamplingFailure("maximum drag teach sample count reached");
            return;
        }
        const auto state_result = robot_.getCurrentArmState();
        if (!state_result.ok) {
            RecordSamplingFailure(state_result.error_message);
            return;
        }
        const auto state_validation = ValidateSampledArmState(state_result.value);
        if (!state_validation.ok) {
            RecordSamplingFailure(state_validation.error_message);
            return;
        }
        const auto now = std::chrono::steady_clock::now();
        const long long elapsed_ms =
            std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time).count();
        {
            std::lock_guard<std::mutex> lock(mutex_);
            samples_.push_back(MakeSample(sample_index, elapsed_ms, state_result.value));
        }
        ++sample_index;
        const auto next_time = start_time + config_.sample_period * sample_index;
        if (now > next_time + config_.sample_period) {
            RecordSamplingFailure(
                "drag teach sampling missed more than one configured period");
            return;
        }
        std::unique_lock<std::mutex> lock(mutex_);
        sampling_condition_.wait_until(lock, next_time, [this] {
            return sampling_stop_requested_.load();
        });
    }
}

void DragTeachService::RecordSamplingFailure(std::string_view message) noexcept {
    sampling_failed_.store(true);
    try {
        bool drag_stopped = false;
        std::string stop_error;
        try {
            const auto stop_result = robot_.stopDragTeach();
            drag_stopped = stop_result.ok;
            if (!stop_result.ok) {
                stop_error = stop_result.error_message;
            }
        } catch (const std::exception &error) {
            stop_error = error.what();
        } catch (...) {
            stop_error = "unknown stopDragTeach exception";
        }

        std::lock_guard<std::mutex> lock(mutex_);
        if (drag_stopped) {
            hardware_drag_active_ = false;
        }
        state_ = DragTeachServiceState::Faulted;
        sampling_error_ = "drag teach sampling failed: " + std::string(message);
        if (!drag_stopped) {
            sampling_error_ += "; stopDragTeach failed and will be retried: " +
                               stop_error;
        }
    } catch (...) {
        state_.store(DragTeachServiceState::Faulted);
    }
}

void DragTeachService::RequestSamplingStop() noexcept {
    sampling_stop_requested_.store(true);
    sampling_condition_.notify_all();
}

common::Result<void> DragTeachService::JoinSamplingThread() {
    try {
        if (sampling_thread_.joinable()) {
            sampling_thread_.join();
        }
        return common::Result<void>::success();
    } catch (const std::exception &error) {
        return common::Result<void>::failure(
            std::string("failed to join drag sampling thread: ") + error.what());
    } catch (...) {
        return common::Result<void>::failure(
            "failed to join drag sampling thread: unknown exception");
    }
}

common::Result<void> DragTeachService::StopDragTeachIfActive() {
    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (!hardware_drag_active_) {
            return common::Result<void>::success();
        }
    }
    const auto result = robot_.stopDragTeach();
    if (result.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        hardware_drag_active_ = false;
    }
    return result;
}

common::Result<StopRecordingSummary>
DragTeachService::ValidateStoppedRecording() {
    const auto validation = modules::trajectory::ValidateDragTrajectory(
        samples_, config_.validation_limits);
    if (!validation.ok) {
        std::lock_guard<std::mutex> lock(mutex_);
        state_ = DragTeachServiceState::Faulted;
        return common::Result<StopRecordingSummary>::failure(
            validation.error_message);
    }

    StopRecordingSummary summary;
    summary.recorded_sample_count = samples_.size();
    summary.validation_report = validation.value;
    {
        std::lock_guard<std::mutex> lock(mutex_);
        validation_report_ = validation.value;
        validation_report_available_ = true;
        state_ = DragTeachServiceState::Recorded;
    }
    return common::Result<StopRecordingSummary>::success(summary);
}

HardwareExecutionGateRequirements DragTeachService::OriginRequirements() const {
    HardwareExecutionGateRequirements requirements;
    requirements.required_speed_percent =
        kNativeTrajectoryOriginRequiredSpeedPercent;
    return requirements;
}

HardwareExecutionGateRequirements DragTeachService::ReplayRequirements() const {
    HardwareExecutionGateRequirements requirements;
    requirements.require_native_replay_without_speed_control_acceptance = true;
    requirements.require_native_replay_speed_verification = true;
    return requirements;
}

common::Result<void> DragTeachService::RequirePermission(
    const HardwareExecutionGateInput &gate_input,
    const HardwareExecutionGateRequirements &requirements) const {
    const auto permission =
        RequireHardwareExecutionPermission(gate_input, requirements);
    if (!permission.ok) {
        return common::Result<void>::failure(
            "native drag trajectory action refused: " +
            permission.error_message);
    }
    return common::Result<void>::success();
}

common::Result<void> DragTeachService::RequireState(
    DragTeachServiceState expected,
    const std::string &operation_name) const {
    const DragTeachServiceState actual = state();
    if (actual != expected) {
        return common::Result<void>::failure(
            operation_name + " requires " + StateName(expected) +
            " state, actual=" + StateName(actual));
    }
    return common::Result<void>::success();
}

} // namespace rm_control::core
