cmvr-es/cmvr-es/common/math/support_functions.h

//
// Created by lgv on 11/10/25.
//

#pragma once
#include <cstdint>
#include <cmath>
#include <vector>
#include <algorithm>
#include <sstream>
#include "common/base/logging/logger.h"
#include <Eigen/Core>

class SupportFunctions {
private:
    static constexpr double EPS = 1e-9;
public:
    static constexpr std::int64_t absoluteDifference(const std::int32_t lhs,
                                                     const std::int32_t rhs) noexcept {
        return lhs >= rhs
                   ? static_cast<std::int64_t>(lhs) - static_cast<std::int64_t>(rhs)
                   : static_cast<std::int64_t>(rhs) - static_cast<std::int64_t>(lhs);
    }

    static constexpr std::int64_t cyclicAbsoluteDifference(
        const std::int32_t lhs,
        const std::int32_t rhs,
        const std::int64_t period) noexcept {
        const auto linear_distance = absoluteDifference(lhs, rhs);
        if (period <= 0) {
            return linear_distance;
        }
        const auto wrapped_distance = linear_distance % period;
        return std::min(wrapped_distance, period - wrapped_distance);
    }

    static  std::vector<double> eigen_to_vector(const Eigen::VectorXd &v) {
        return std::vector<double>(v.data(), v.data() + v.size());
    }
    static double normalize_angle(double angle) {
        double a = std::fmod(angle, 2.0 * M_PI);
        if (a < -M_PI) a += 2.0 * M_PI;
        if (a > M_PI) a -= 2.0 * M_PI;

        if (std::abs(a - M_PI) < EPS) return M_PI;
        if (std::abs(a + M_PI) < EPS) return -M_PI;
        return a;
    }

    static bool angle_in_wrap(double x, double L, double U) {
        x = normalize_angle(x);
        L = normalize_angle(L);
        U = normalize_angle(U);
        if (L <= U) return (x > L - EPS && x < U + EPS);
        return (x > L - EPS || x < U + EPS);
    }

    static std::vector<std::pair<double, double> >
    union_intervals(const std::vector<std::pair<double, double> > &in) {
        if (in.empty()) return {};
        std::vector<std::pair<double, double> > v = in;
        std::sort(v.begin(), v.end(), [](auto &a, auto &b) {
            return (a.first < b.first) || (a.first == b.first && a.second < b.second);
        });
        std::vector<std::pair<double, double> > out;
        double L = v[0].first, R = v[0].second;
        for (size_t i = 1; i < v.size(); ++i) {
            if (v[i].first <= R + EPS) R = std::max(R, v[i].second);
            else {
                out.push_back({L, R});
                L = v[i].first;
                R = v[i].second;
            }
        }
        out.push_back({L, R});
        return out;
    }


    static std::vector<std::pair<double, double> > intersect_intervals(
        const std::vector<std::pair<double, double> > &A, const std::vector<std::pair<double, double> > &B) {
        if (A.empty() || B.empty()) return {};

        // 先复制并排序（按起点）
        auto SA = A, SB = B;
        std::sort(SA.begin(), SA.end(),
                  [](auto &x, auto &y) { return x.first < y.first; });
        std::sort(SB.begin(), SB.end(),
                  [](auto &x, auto &y) { return x.first < y.first; });

        // 双指针求交
        std::vector<std::pair<double, double> > out;
        size_t i = 0, j = 0;
        while (i < SA.size() && j < SB.size()) {
            double L = std::max(SA[i].first, SB[j].first);
            double R = std::min(SA[i].second, SB[j].second);
            if (R > L) out.emplace_back(L, R);

            // 谁先结束谁前进
            if (SA[i].second < SB[j].second) ++i;
            else ++j;
        }
        // 合并可能相邻/重叠的小段
        if (out.empty()) return out;

        std::vector<std::pair<double, double> > merged;
        merged.reserve(out.size());
        std::sort(out.begin(), out.end(),
                  [](auto &x, auto &y) { return x.first < y.first; });

        merged.push_back(out[0]);
        for (size_t k = 1; k < out.size(); ++k) {
            if (out[k].first <= merged.back().second + EPS) {
                merged.back().second = std::max(merged.back().second, out[k].second);
            } else {
                merged.push_back(out[k]);
            }
        }
        return merged;
    }


    static bool wraps(double L, double U) {
        L = normalize_angle(L); // [-π, π]
        U = normalize_angle(U); // [-π, π]
        return (L > U); // 在 [-π, π] 规范下仍成立
    }

    static double deg2rad(double deg) { return deg * M_PI / 180.0; }
    static double rad2deg(double rad) { return rad * 180.0 / M_PI; }

    template<class T>
    static constexpr int sign(T x, T eps) {
        return (x > eps) - (x < -eps);
    }

    template<class T>
    static constexpr int sign(T x) {
        return sign(x, T(0));
    }

    template<typename T>
    static T clamp(T v, T lo, T hi) {
        return std::max(lo, std::min(hi, v));
    }

    static void print_intervals(const std::vector<std::pair<double, double> > &intervals) {
        std::ostringstream output;
        for (const auto &interval: intervals) {
            output << "[" << interval.first << ", " << interval.second << "] ";
        }
        if (!output.str().empty()) {
            CMVR_LOG(INFO) << output.str();
        }
    }


    // 假设 T 是合法的刚体变换（旋转正交、det≈+1）
    static  Eigen::Matrix4d invertHomogeneous(const Eigen::Matrix4d& T) {
        Eigen::Matrix3d R = T.block<3,3>(0,0);
        Eigen::Vector3d t = T.block<3,1>(0,3);
        Eigen::Matrix4d Ti = Eigen::Matrix4d::Identity();
        Eigen::Matrix3d Rt = R.transpose();
        Ti.block<3,3>(0,0) = Rt;
        Ti.block<3,1>(0,3) = -Rt * t;
        return Ti;
    }


    static int calcLookahead(double qd_ref,
                                        double qdd_ref,
                                        double v_max,
                                        double dt,
                                        int n_min = 1,
                                        int n_max = 5)
    {
        double v0   = std::abs(qd_ref);
        double a    = std::abs(qdd_ref);
        double vmax = std::abs(v_max);

        // 1) 已经在接近最大速度的匀速阶段：直接用 1 步
        const double vel_sat_ratio = 0.95;  // 95% vmax 就认为是全速段
        if (v0 >= vel_sat_ratio * vmax) {
            return 1;
        }

        // 2) 其他情况再用“加速 + 饱和”的那套公式
        double s_target = vmax * dt;

        if (v0 < 1e-6 && a < 1e-6) {
            return n_min;
        }

        double tau = 0.0;

        if (a < 1e-6) {
            // 近似匀速：v0 * tau = s_target
            tau = s_target / (v0 + 1e-6);
        } else {
            double A = 0.5 * a;
            double B = v0;
            double C = -s_target;

            double disc = B * B - 4.0 * A * C;
            if (disc < 0.0) disc = 0.0;
            double sqrt_disc = std::sqrt(disc);

            double tau_quad = (-B + sqrt_disc) / (2.0 * A);
            double v_at_tau = v0 + a * tau_quad;

            if (v_at_tau <= vmax + 1e-9) {
                tau = tau_quad;
            } else {
                double t_sat = (vmax - v0) / a;
                double s_sat = v0 * t_sat + 0.5 * a * t_sat * t_sat;

                if (s_target <= s_sat) {
                    tau = std::min(tau_quad, t_sat);
                } else {
                    tau = t_sat + (s_target - s_sat) / vmax;
                }
            }
        }

        int n = (int)std::ceil(tau / dt);
        if (n < n_min) n = n_min;
        if (n > n_max) n = n_max;
        return n;
    }
};