#include "math.h"
#include "gjk_interface.hpp"

#define POLY_SIMPLIFY_MAX_SIZE 4
using namespace Eigen;

namespace gjk_interface {
    const Vector3d support(const std::vector<Vector3d>& poly, const Vector3d& dir)
    {
        float maxDot = -INFINITY;
        Vector3d supportPoint;

        for (const Vector3d& p : poly) {
            float dot = p.dot(dir);
            if (dot > maxDot) {
                maxDot = dot;
                supportPoint = p;
            }
        }

        return supportPoint;
    }

    Vector3d supportMinkowski(const std::vector<Vector3d>& polyA, const std::vector<Vector3d>& polyB, Vector3d& dir)
    {
        Vector3d simplexPoint = support(polyA, dir);
        const Vector3d oppositeDir = dir * -1;
        simplexPoint = simplexPoint - support(polyB, oppositeDir);
        return simplexPoint;
    }

    Vector3d checkLineSegment3D(const Vector3d& v0, const Vector3d& v1) {
        Vector3d a = v1 - v0;      // 线段方向向量
        Vector3d b = -v1;          // 从v1到原点的向量
        
        float aDotA = a.dot(a);
        Vector3d bParallel = a * (a.dot(b) / aDotA);  // 平行投影
        Vector3d bPerp = b - bParallel;               // 垂直投影（新方向）
        
        return bPerp;
    }

    Vector3d GetNormalToOri(const Vector3d& a, const Vector3d& b, const Vector3d& c) {
        Vector3d ab = b - a;
        Vector3d ac = c - a;
        Vector3d ad = -a;

        Vector3d n_initial = ab.cross(ac);
        Vector3d n_outer = (n_initial.dot(ad) < 0) ? -n_initial : n_initial;
        return n_outer;
    }

    Vector3d GetDir(const std::vector<Vector3d>& simplex) {
        if (simplex.size() == 1) {
            return -simplex[0];
        } else if (simplex.size() == 2) {
            return checkLineSegment3D(simplex[0], simplex[1]);
        } else if (simplex.size() == 3) {
            return GetNormalToOri(simplex[0], simplex[1], simplex[2]);
        }
        return Vector3d(0, 0, 0);
    }

    bool CheckTetrahedronHaveZero(const std::vector<Vector3d>& simplex) {
        Vector3d A = simplex[0];
        Vector3d B = simplex[1];
        Vector3d C = simplex[2];
        Vector3d D = simplex[3];  // 最新点（面ABC的对
        // 构造线性方程组 Ax = b（求解alpha, beta, gamma）
        Matrix3d mat;
        mat.col(0) = A - D;  // x1-x4, y1-y4, z1-z4
        mat.col(1) = B - D;  // x2-x4, y2-y4, z2-z4
        mat.col(2) = C - D;  // x3-x4, y3-y4, z3-z4
        Vector3d b = -D;     // -x4, -y4, -z4

        // 求解方程组：mat * [alpha; beta; gamma] = b
        if (mat.determinant() == 0) {
            return true;
        }
        Vector3d x = mat.inverse() * b;
        float alpha = x(0), beta = x(1), gamma = x(2);
        float delta = 1 - alpha - beta - gamma;

        // 判定4个权重是否均为正（允许1e-6误差）
        const float eps = 1e-6;
        return (alpha > eps) && (beta > eps) && (gamma > eps) && (delta > eps);
    }

    bool CheckTetrahedronHaveZero2(const std::vector<Vector3d>& simplex) {
        Vector3d A = simplex[0];
        Vector3d B = simplex[1];
        Vector3d C = simplex[2];
        Vector3d D = simplex[3];  // 最新点（面ABC的对
        // 构造线性方程组 Ax = b（求解alpha, beta, gamma）
        auto scalar_triple = [](const Vector3d& a, const Vector3d& b, const Vector3d& c) {
            return a.dot(b.cross(c));  // Eigen封装了叉积和点积，底层运算与手动计算一致
        };

        double v0 = scalar_triple(B, C, D);
        double v1 = scalar_triple(A, C, D);
        double v2 = scalar_triple(A, B, D);
        double v3 = scalar_triple(A, B, C);

        // 判断所有符号是否一致（均正或均负）
        bool all_positive = (v0 > 0) && (v1 > 0) && (v2 > 0) && (v3 > 0);
        bool all_negative = (v0 < 0) && (v1 < 0) && (v2 < 0) && (v3 < 0);

        return all_positive || all_negative;
    }

    bool gjkCollisionJudge(const std::vector<Vector3d>& polyA, const std::vector<Vector3d>& polyB)
    {
        // 初始搜索方向（从A中心指向B中心）
        Vector3d centerA(0, 0, 0), centerB(0, 0, 0);
        for (const auto& p : polyA) {
            centerA = centerA + p;
        }
        for (const auto& p : polyB) {
            centerB = centerB + p;
        }
        centerA = centerA / polyA.size();
        centerB = centerB / polyA.size();
        Vector3d dir = centerB - centerA;

        // 初始化单纯形
        std::vector<Vector3d> simplex;
        Vector3d newPoint = supportMinkowski(polyA, polyB, dir);
        simplex.push_back(newPoint);

        int maxIndex = 3;
        while (true) {
            dir = GetDir(simplex);
            if (dir.dot(dir) < 1e-6) {
                return true;
            }
            newPoint = supportMinkowski(polyA, polyB, dir);
            if (newPoint.dot(newPoint) < 1e-6) {
                return true;
            }
            if (newPoint.dot(dir) < -(1e-6)) {
                return false;
            }
            simplex.push_back(newPoint);
            if (simplex.size() == POLY_SIMPLIFY_MAX_SIZE) {
                bool haveZero = CheckTetrahedronHaveZero2(simplex);
                if (haveZero) {
                    return true;
                } else {
                    float maxLength = 0;
                    for (size_t i = 0; i < POLY_SIMPLIFY_MAX_SIZE; i++) {
                        Vector3d simplexPoint = simplex[i];
                        float newLength = simplexPoint.dot(simplexPoint);
                        if (maxLength < newLength) {
                            maxLength = newLength;
                            maxIndex = i;
                        }
                    }
                    simplex.erase(simplex.begin() + maxIndex);
                }
            }
        }

        return false;
    }

    // 从8个顶点提取OBB参数
    OBB createOBBFromVertices(const std::vector<Eigen::Vector3d>& vertices,
        const float length, const float width, const float height) {
        if (vertices.size() != 8) {
            throw std::invalid_argument("OBB创建失败:必须输入8个顶点");
        }

        OBB obb;

        obb.center = (vertices[1] + vertices[2] + vertices[4] - vertices[0]) * (1.0 / 2.0);

        // 正交化（格拉姆-施密特过程），确保3条轴正交
        obb.axis[0] = (vertices[1] - vertices[0]).normalized();
        obb.axis[1] = (vertices[2] - vertices[0]).normalized();
        obb.axis[2] = (vertices[4] - vertices[0]).normalized();

        obb.halfExtent[0] = length * (1.0 / 2.0);
        obb.halfExtent[1] = width * (1.0 / 2.0);
        obb.halfExtent[2] = height * (1.0 / 2.0);

        return obb;
    }

    // 计算OBB在指定轴上的投影范围（min, max）
    std::pair<double, double> projectOBB(const OBB& obb, const Vector3d& axis) {
        // OBB中心在轴上的投影
        double centerProj = obb.center.dot(axis);
        // OBB沿轴的总投影长度（半长在轴上的投影之和）
        double totalExtent = 0.0;
        for (int i = 0; i < 3; ++i) {
            totalExtent += obb.halfExtent[i] * fabs(obb.axis[i].dot(axis));
        }
        return { centerProj - totalExtent, centerProj + totalExtent };
    }

    // 判断两个投影范围是否重叠
    bool areProjectionsOverlapping(double aMin, double aMax, double bMin, double bMax) {
        return !(aMax < bMin - 1e-6 || bMax < aMin - 1e-6);  // 1e-6为浮点误差容限
    }

    // OBB碰撞检测主函数（输入：两个长方体的8个顶点，共16个）
    bool checkOBBCollision(const OBB &obb1, const OBB &obb2) {

        // 2. 定义所有需要检查的分离轴（共15条）
        std::vector<Vector3d> separationAxes;

        // 2.3 两个OBB轴的两两叉积（3×3=9条）
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                Vector3d crossAxis = obb1.axis[i].cross(obb2.axis[j]);
                if (crossAxis.dot(crossAxis) > 1e-6) {  // 排除零向量（轴平行时叉积为零
                    Vector3d crossAxisNormalized = crossAxis.normalized();
                    auto [aMin, aMax] = projectOBB(obb1, crossAxisNormalized);
                    auto [bMin, bMax] = projectOBB(obb2, crossAxisNormalized);
                    if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) {
                        // 找到分离轴，判定为不碰撞
                        return false;
                    }
                }
            }
        }
        // 2.1 第一个OBB的3条轴
        for (int i = 0; i < 3; ++i) {
            auto [aMin, aMax] = projectOBB(obb1, obb1.axis[i]);
            auto [bMin, bMax] = projectOBB(obb2, obb1.axis[i]);
            if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) {
                // 找到分离轴，判定为不碰撞
                return false;
            }
        }
        // 2.2 第二个OBB的3条轴
        for (int i = 0; i < 3; ++i) {
            auto [aMin, aMax] = projectOBB(obb1, obb2.axis[i]);
            auto [bMin, bMax] = projectOBB(obb2, obb2.axis[i]);
            if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) {
                // 找到分离轴，判定为不碰撞
                return false;
            }
        }

        // 所有轴均重叠，判定为碰撞
        return true;
    }
}
