//
// 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) {

        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[3] = 0.111;
        joint_angles = { 0.875, M_PI / 2, 0.2644, M_PI / 2, 1,  M_PI/ 4, M_PI/ 5};

        auto pose = slover.calc_total_transform(joint_angles);
        std::cout << "Pose: " << pose << std::endl;

        // slover.set_elbow_config(SRSIkSlover::INWARD);
        // slover.set_wrist_config(SRSIkSlover::INWARD);
        // slover.set_shoulder_config(SRSIkSlover::INWARD);


        Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
        slover.cal_coefficient_matrix(pose, s_mat, w_mat);
        auto limits = slover.calc_arm_angle_limits(s_mat,w_mat);
        for (const auto & limit: limits) {
                for (double psi = limit.first; psi < limit.second; psi+=0.001 ) {
                        auto q = slover.inverse_kinematics(pose,psi);
                        samples.push_back(IkSample{psi, {q[0],q[1],q[2],q[3],q[4],q[5],q[6]}});
                        pose = slover.calc_total_transform(q);
                        // std::cout << "Pose: " << pose << std::endl;
                }
        }

        // Eigen::VectorXd nsparams = Eigen::VectorXd::LinSpaced(1000, -3.1415926, 3.1415926);
        // for (auto psi: nsparams) {
        //         auto q = slover.inverse_kinematics(pose,psi);
        //         samples.push_back(IkSample{psi, {q[0],q[1],q[2],q[3],q[4],q[5],q[6]}});
        //         for (double q1: q) {
        //                 std::cout << q1  << " , " ;
        //         }
        //         std::cout << std::endl;
        //         pose = slover.calc_total_transform(q);
        //         // std::cout << "Pose: " << pose << std::endl;
        // }


        write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples,true, 9);
}

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);
        // 预期输出: (无输出)

}