//
// Created by lgv on 2025/11/3.
//


#include "gtest/gtest.h"
#include "manif/SE3.h"
#include "srs_ik/srs_ik_slover.h"
#include "srs_ik/ik_limit_analyzer.h"
#include <iostream>
#include <iomanip>
#include <fstream>

using namespace manif;
using namespace cmvr::utils;

struct IkSample {
        double psi;
        std::array<double,7> q;  // q1..q7
};



bool write_ik_samples_csv(const std::string& filepath,
                          const std::vector<IkSample>& samples,
                          bool write_header,
                          int precision)
{
        std::ofstream ofs(filepath, std::ios::out | std::ios::trunc);
        if (!ofs.is_open()) return false;

        // 固定小数点（避免本地化成逗号）
        ofs.imbue(std::locale::classic());
        ofs << std::fixed << std::setprecision(precision);

        if (write_header) {
                ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n";
        }
        for (const auto& s : samples) {
                ofs << s.psi;
                for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i];
                ofs << '\n';
        }
        return true;
}

TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
    using std::cout;
    using std::endl;

    // std::cout << std::fixed << std::setprecision(7);

    SRSIkSlover slover;
    std::vector<IkSample> samples;
    samples.reserve(4096);

    std::vector<double> joint_angles(7, 0);
    joint_angles = { 0.875, 0.22, 0.2644, M_PI / 2, 1.0, 1.99, 1.56 };

    // 目标位姿：FK(joint_angles)
    const auto target_pose = slover.calc_total_transform(joint_angles);
    cout << "Target Pose (FK from seed joints):\n" << target_pose << endl;

    // 系数矩阵 & ψ 扫描区间
    Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
    slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
    auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);

    // 误差统计
    const double kPosTol = 1e-4;     // 位置容差（m）
    const double kRotTol = 1e-3;     // 姿态容差（rad）≈ 0.0573°
    double max_pos_err = 0.0, max_rot_err = 0.0;
    double sum_pos_err = 0.0, sum_rot_err = 0.0;
    size_t total = 0, bad = 0;

    // 便捷引用
    const Eigen::Vector3d p_target = target_pose.block<3,1>(0,3);
    const Eigen::Matrix3d R_target = target_pose.block<3,3>(0,0);

    auto clamp = [](double x, double lo, double hi) {
        return std::max(lo, std::min(hi, x));
    };

    auto rot_err_rad = [&](const Eigen::Matrix3d& R) -> double {
        Eigen::Matrix3d dR = R_target.transpose() * R;
        double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
        return std::acos(c); // [0, pi]
    };

    // 表头
    cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";

    // ψ 扫描
    for (const auto& limit : limits) {
        const double psi_lo = limit.first;
        const double psi_hi = limit.second;

        for (double psi = psi_lo; psi < psi_hi; psi += 0.001) {
            // IK 解
            auto q = slover.inverse_kinematics(target_pose, psi);
            if (q.size() != 7 || std::any_of(q.begin(), q.end(),
                    [](double v){ return !std::isfinite(v); })) {
                ++bad;
                ++total;
                cout << psi << ", nan, nan, nan\n";
                continue;
            }

            // 用 IK 解做 FK，计算误差
            const auto T_fk = slover.calc_total_transform(q);
            const Eigen::Vector3d p_fk = T_fk.block<3,1>(0,3);
            const Eigen::Matrix3d R_fk = T_fk.block<3,3>(0,0);

            const double pos_err = (p_fk - p_target).norm();
            const double rot_err = rot_err_rad(R_fk);
            const double rot_err_deg = rot_err * 180.0 / M_PI;

            // 打印每个样本的误差（一行，便于看 sweep）
            cout << psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";

            // 断言（不立即中止）
            SCOPED_TRACE(testing::Message() << "psi=" << psi);
            EXPECT_LT(pos_err, kPosTol);
            EXPECT_LT(rot_err, kRotTol);

            // 统计
            max_pos_err = std::max(max_pos_err, pos_err);
            max_rot_err = std::max(max_rot_err, rot_err);
            sum_pos_err += pos_err;
            sum_rot_err += rot_err;
            ++total;

            // 保留你原先的采样输出
            samples.push_back(IkSample{psi, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
        }
    }

    // 摘要打印
    cout << "\nSummary:\n"
         << " total=" << total
         << "  bad=" << bad
         << "  pos_err_max=" << max_pos_err << " m"
         << "  rot_err_max=" << max_rot_err << " rad (" << max_rot_err * 180.0 / M_PI << " deg)\n"
         << " pos_err_mean=" << (total ? (sum_pos_err / total) : 0.0) << " m"
         << "  rot_err_mean=" << (total ? (sum_rot_err / total) : 0.0) << " rad ("
         << (total ? (sum_rot_err / total) * 180.0 / M_PI : 0.0) << " deg)\n";

    // 文件输出（与原逻辑一致）
    write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);

    // 最终强约束（避免全是 EXPECT_* 时忽略失败）
    ASSERT_LT(max_pos_err, 10 * kPosTol) << "Max position error too large.";
    ASSERT_LT(max_rot_err, 10 * kRotTol) << "Max rotation error too large.";
}

TEST(SRS_IK_TEST,INTERSECT_TEST) {
        // 测试 1: 有交集的区间
        std::vector<std::pair<double, double>> A = {{-3.0, -1.0}, {1.0, 4.0}};
        std::vector<std::pair<double, double>> B = {{-2.0, 0.5}, {2.5, 5.0}};

        std::cout << "Test 1: Intersecting intervals" << std::endl;
        auto result1 = IkLimitAnalyzer::intersect(A, B);
        IkLimitAnalyzer::print_intervals(result1);
        // 预期输出: [-2.0, -1.0] [2.5, 4.0]

        // 测试 2: 相邻但不重叠的区间
        std::vector<std::pair<double, double>> C = {{-3.0, -1.0}, {2.0, 4.0}};
        std::vector<std::pair<double, double>> D = {{-1.0, 0.0}, {1.0, 3.0}};

        std::cout << "Test 2: Adjacent intervals" << std::endl;
        auto result2 = IkLimitAnalyzer::intersect(C, D);
        IkLimitAnalyzer::print_intervals(result2);
        // 预期输出:  [2.0, 3.0]

        // 测试 3: 无交集的区间
        std::vector<std::pair<double, double>> E = {{-5.0, -3.0}, {2.0, 4.0}};
        std::vector<std::pair<double, double>> F = {{5.0, 6.0}, {7.0, 8.0}};

        std::cout << "Test 3: Non-intersecting intervals" << std::endl;
        auto result3 = IkLimitAnalyzer::intersect(E, F);
        IkLimitAnalyzer::print_intervals(result3);
        // 预期输出: (无输出)

        // 测试 4: 一个空的区间集
        std::vector<std::pair<double, double>> G = {};
        std::vector<std::pair<double, double>> H = {{1.0, 2.0}, {3.0, 4.0}};

        std::cout << "Test 4: Empty intervals" << std::endl;
        auto result4 = IkLimitAnalyzer::intersect(G, H);
        IkLimitAnalyzer::print_intervals(result4);
        // 预期输出: (无输出)

}