#include "realman_hw_test_common.hpp"

#include <algorithm>
#include <atomic>
#include <csignal>
#include <cmath>
#include <sstream>

namespace {

// 持续力退让的核心安全参数。阈值和步长都偏保守，便于人工轻推验证。
constexpr float kForceThresholdN = 2.0F;
constexpr float kForceReleaseThresholdN = 0.8F;
constexpr float kDangerForceLimitN = 60.0F;
constexpr float kRetreatMinStepMeters = 0.004F;
constexpr float kRetreatMaxStepMeters = 0.014F;
constexpr float kRetreatStepGainMetersPerNewton = 0.0015F;
constexpr float kMaxCumulativeRetreatMeters = 0.200F;
constexpr int kMinMotionSpeedPercent = 2;
constexpr int kMaxMotionSpeedPercent = 15;
constexpr float kSpeedGainPercentPerNewton = 1.5F;
constexpr int kPollIntervalMs = 80;
constexpr int kPostMoveDwellMs = 120;
constexpr int kStartupCountdownSeconds = 3;

// 若实际运动方向变成“迎着推力走”，把这里改成 -1.0F。
constexpr float kRetreatDirectionSign = 1.0F;

std::atomic<bool> g_running(true);

struct Vec3 {
    float x = 0.0F;
    float y = 0.0F;
    float z = 0.0F;
};

void handleSignal(int) {
    g_running.store(false);
}

float norm(const Vec3 &value) {
    return std::sqrt(value.x * value.x + value.y * value.y + value.z * value.z);
}

Vec3 normalize(const Vec3 &value) {
    const float length = norm(value);
    if (length <= 1e-6F) {
        return {};
    }
    return {value.x / length, value.y / length, value.z / length};
}

std::string formatVec3(const Vec3 &value, const char *unit) {
    std::ostringstream out;
    out << std::fixed << std::setprecision(4)
        << "(" << value.x << ", " << value.y << ", " << value.z << ") " << unit;
    return out.str();
}

bool readWorkFrameForce(rm_robot_handle *handle, Vec3 *force) {
    // 使用工作坐标系下的外力，后续目标位姿也在工作坐标系语义下计算，避免坐标混用。
    rm_force_data_t data = {};
    const int result = rm_get_force_data(handle, &data);
    if (result != 0) {
        std::cerr << "rm_get_force_data failed, error=" << result << '\n';
        return false;
    }

    *force = {
        data.work_zero_force_data[0],
        data.work_zero_force_data[1],
        data.work_zero_force_data[2],
    };
    return true;
}

rm_pose_t makeOffsetPose(const Vec3 &direction, float step_meters) {
    // offset 只用于日志展示；实际运动使用绝对 target_pose，兼容当前控制器表现更稳定。
    rm_pose_t offset = {};
    offset.position.x = kRetreatDirectionSign * direction.x * step_meters;
    offset.position.y = kRetreatDirectionSign * direction.y * step_meters;
    offset.position.z = kRetreatDirectionSign * direction.z * step_meters;
    offset.euler.rx = 0.0F;
    offset.euler.ry = 0.0F;
    offset.euler.rz = 0.0F;
    offset.quaternion.w = 0.0F;
    offset.quaternion.x = 0.0F;
    offset.quaternion.y = 0.0F;
    offset.quaternion.z = 0.0F;
    return offset;
}

rm_pose_t makeTargetPose(const rm_pose_t &current_pose, const Vec3 &direction, float step_meters) {
    // 以当前 TCP 位姿为基准生成绝对目标点，避免连续 rm_movel_offset 在部分固件上返回 -7。
    rm_pose_t target = current_pose;
    target.position.x += kRetreatDirectionSign * direction.x * step_meters;
    target.position.y += kRetreatDirectionSign * direction.y * step_meters;
    target.position.z += kRetreatDirectionSign * direction.z * step_meters;
    return target;
}

float computeRetreatStepMeters(float force_norm) {
    // 力越大，退让单步越大；上下限保证小力有响应、大力不突然跳太远。
    const float excess_force = std::max(0.0F, force_norm - kForceReleaseThresholdN);
    const float raw_step = kRetreatMinStepMeters + excess_force * kRetreatStepGainMetersPerNewton;
    return std::clamp(raw_step, kRetreatMinStepMeters, kRetreatMaxStepMeters);
}

int computeSpeedPercent(float force_norm) {
    // 速度随外力增大而提高，但最高限制在 15%，保证人工测试时可控。
    const float excess_force = std::max(0.0F, force_norm - kForceThresholdN);
    const float raw_speed = static_cast<float>(kMinMotionSpeedPercent) +
                            excess_force * kSpeedGainPercentPerNewton;
    return static_cast<int>(std::round(std::clamp(raw_speed,
                                                  static_cast<float>(kMinMotionSpeedPercent),
                                                  static_cast<float>(kMaxMotionSpeedPercent))));
}

std::string offsetDetail(const rm_pose_t &offset) {
    std::ostringstream out;
    out << std::fixed << std::setprecision(4)
        << "dx=" << offset.position.x
        << ", dy=" << offset.position.y
        << ", dz=" << offset.position.z << " m";
    return out.str();
}

bool validateConfig(realman_hw_test::TestSuite *suite) {
    // 参数自检先于任何运动执行，防止误改常量后直接把危险配置跑到真机上。
    bool ok = true;
    ok = suite->expect(kForceThresholdN > kForceReleaseThresholdN,
                       "force_threshold_gt_release") && ok;
    ok = suite->expect(kRetreatMinStepMeters > 0.0F &&
                           kRetreatMinStepMeters <= kRetreatMaxStepMeters &&
                           kRetreatMaxStepMeters <= 0.015F,
                       "retreat_step_range_safe",
                       "min_m=" + std::to_string(kRetreatMinStepMeters) +
                           ", max_m=" + std::to_string(kRetreatMaxStepMeters)) && ok;
    ok = suite->expect(kRetreatStepGainMetersPerNewton > 0.0F &&
                           kRetreatStepGainMetersPerNewton <= 0.003F,
                       "retreat_step_gain_safe",
                       "gain_m_per_N=" + std::to_string(kRetreatStepGainMetersPerNewton)) && ok;
    ok = suite->expect(kMaxCumulativeRetreatMeters <= 0.220F,
                       "max_cumulative_retreat_safe",
                       "max_m=" + std::to_string(kMaxCumulativeRetreatMeters)) && ok;
    ok = suite->expect(kMinMotionSpeedPercent >= 1 &&
                           kMinMotionSpeedPercent <= kMaxMotionSpeedPercent &&
                           kMaxMotionSpeedPercent <= 15,
                       "retreat_speed_range_safe",
                       "min_percent=" + std::to_string(kMinMotionSpeedPercent) +
                           ", max_percent=" + std::to_string(kMaxMotionSpeedPercent)) && ok;
    ok = suite->expect(kSpeedGainPercentPerNewton > 0.0F &&
                           kSpeedGainPercentPerNewton <= 3.0F,
                       "retreat_speed_gain_safe",
                       "gain_percent_per_N=" + std::to_string(kSpeedGainPercentPerNewton)) && ok;
    ok = suite->expect(kRetreatDirectionSign == 1.0F || kRetreatDirectionSign == -1.0F,
                       "retreat_direction_sign_valid",
                       "sign=" + std::to_string(kRetreatDirectionSign)) && ok;
    return ok;
}

} // namespace

int main() {
    using namespace realman_hw_test;

    std::signal(SIGINT, handleSignal);
    std::signal(SIGTERM, handleSignal);

    // 该测试是手动持续模式：启动后一直监听外力，直到 Ctrl+C/SIGTERM 或安全边界触发。
    TestSuite suite("realman_hw_force_retreat_hold");
    RobotSession session(&suite);
    if (!session.connect()) {
        return suite.finish();
    }

    rm_robot_info_t info = {};
    if (!readRobotInfo(session.handle(), &info, &suite)) {
        return suite.finish();
    }
    suite.expect(info.force_type == RM_MODEL_RM_SF_E || info.force_type == RM_MODEL_RM_ISF_E,
                 "six_axis_force_supported",
                 std::string("actual=") + forceTypeName(info.force_type));
    if (!validateConfig(&suite)) {
        return suite.finish();
    }

    std::cout << "\nContinuous force retreat hold mode.\n"
              << "Threshold: " << kForceThresholdN << " N, step: "
              << kRetreatMinStepMeters << ".." << kRetreatMaxStepMeters
              << " m, speed: " << kMinMotionSpeedPercent << ".." << kMaxMotionSpeedPercent
              << "%, max cumulative retreat: " << kMaxCumulativeRetreatMeters << " m.\n"
              << "Press Ctrl+C or send SIGTERM to stop. Keep E-stop reachable.\n";

    for (int second = kStartupCountdownSeconds; second > 0; --second) {
        // 倒计时给操作者留出离开键盘、扶好急停和观察机械臂的时间。
        std::cout << "Hold mode starts in " << second << "...\n";
        sleepMs(1000);
    }

    float cumulative_retreat = 0.0F;
    int move_count = 0;
    bool sequence_ok = true;

    while (g_running.load()) {
        Vec3 force = {};
        if (!readWorkFrameForce(session.handle(), &force)) {
            suite.expect(false, "read_work_frame_force");
            sequence_ok = false;
            break;
        }

        const float force_norm = norm(force);
        if (force_norm > kDangerForceLimitN) {
            suite.expect(false, "force_under_danger_limit", "force_norm_N=" + std::to_string(force_norm));
            rm_set_arm_stop(session.handle());
            sequence_ok = false;
            break;
        }

        if (force_norm < kForceReleaseThresholdN) {
            sleepMs(kPollIntervalMs);
            continue;
        }

        if (force_norm < kForceThresholdN) {
            // 死区内只打印提示不运动，避免传感器噪声导致机械臂抖动。
            std::cout << "Force in deadband: " << formatVec3(force, "N")
                      << ", norm=" << std::fixed << std::setprecision(3) << force_norm << " N\n";
            sleepMs(kPollIntervalMs);
            continue;
        }

        const float step_meters = computeRetreatStepMeters(force_norm);
        const int speed_percent = computeSpeedPercent(force_norm);

        if (cumulative_retreat + step_meters > kMaxCumulativeRetreatMeters + 1e-6F) {
            // 累计退让到边界后立即停止，避免持续推压把机械臂越带越远。
            suite.expect(false,
                         "max_cumulative_retreat_reached",
                         "total_m=" + std::to_string(cumulative_retreat));
            rm_set_arm_stop(session.handle());
            sequence_ok = false;
            break;
        }

        const Vec3 direction = normalize(force);
        const rm_pose_t offset = makeOffsetPose(direction, step_meters);
        rm_current_arm_state_t current_state = {};
        const int state_result = rm_get_current_arm_state(session.handle(), &current_state);
        if (state_result != 0) {
            suite.expect(false, "rm_get_current_arm_state", "error=" + std::to_string(state_result));
            rm_set_arm_stop(session.handle());
            sequence_ok = false;
            break;
        }
        const rm_pose_t target_pose = makeTargetPose(current_state.pose, direction, step_meters);

        std::cout << "Force trigger: " << formatVec3(force, "N")
                  << ", norm=" << std::fixed << std::setprecision(3) << force_norm
                  << " N, step=" << step_meters << " m, speed=" << speed_percent
                  << "%, retreat " << offsetDetail(offset) << '\n';

        const int move_result = rm_movel(session.handle(), target_pose, speed_percent, 0, 0, 1);
        if (move_result != 0) {
            suite.expect(false, "rm_movel", "error=" + std::to_string(move_result));
            rm_set_arm_stop(session.handle());
            sequence_ok = false;
            break;
        }

        ++move_count;
        cumulative_retreat += step_meters;
        suite.info("move_" + std::to_string(move_count),
                   offsetDetail(offset) +
                       ", force_norm_N=" + std::to_string(force_norm) +
                       ", step_m=" + std::to_string(step_meters) +
                       ", speed_percent=" + std::to_string(speed_percent));
        sleepMs(kPostMoveDwellMs);
    }

    suite.info("stop_reason", g_running.load() ? "safety_or_error" : "signal_requested");
    suite.info("move_count", std::to_string(move_count));
    suite.info("cumulative_retreat_m", std::to_string(cumulative_retreat));
    suite.expect(sequence_ok, "force_retreat_hold_sequence_ok");

    return suite.finish();
}
