#pragma once
#include <cassert>
#include <cmath>
#include <cstddef>
#include <vector>
#include "iostream"
#include "multiloop.hpp"
#include "stack"
#include "polyset.hpp"
#include "sparkstack.hpp"
#include "uvector.hpp"
#include "real.hpp"
#include "xarray.hpp"
#include "binomial.hpp"
#include "bernstein.hpp"
#include "blobtree.hpp"
#include "quaternion.hpp"
#include "factorial.hpp"
#include "minimalPrimitive.hpp"
#include <memory>

namespace algoim::organizer
{


namespace detail
{

template <int N>
void power2BernsteinTensor(const xarray<real, N>& phiPower, xarray<real, N>& phiBernsetin)
{
    xarrayInit(phiBernsetin);
    for (auto i = phiPower.loop(); ~i; ++i) {
        // phi.l(i) = powerFactors.l(i);
        real factorBase = phiPower.l(i);
        if (factorBase == 0) continue;
        auto                           traverseRange = phiPower.ext() - i();
        std::vector<std::vector<real>> decompFactors(N, std::vector<real>(max(traverseRange), 0.));
        for (int dim = 0; dim < N; ++dim) {
            // Sigma
            size_t      nDim      = phiPower.ext()(dim) - 1;
            const real* binomNDim = Binomial::row(nDim);
            for (int j = i(dim); j <= nDim; ++j) {
                const real* binomJ             = Binomial::row(j);
                decompFactors[dim][j - i(dim)] = binomJ[i(dim)] / binomNDim[i(dim)];
            }
        }
        xarray<real, N> subgrid(nullptr, traverseRange);

        for (auto ii = subgrid.loop(); ~ii; ++ii) {
            real factor = factorBase;
            for (int dim = 0; dim < N; ++dim) { factor *= decompFactors[dim][ii(dim)]; }
            phiBernsetin.m(i() + ii()) += factor;
        }
    }
}

template <int N>
void power2BernsteinTensor(xarray<real, N>& tensor)
{
    // TODO: 是否可以直接在原地进行计算
    xarray<real, N> power(nullptr, tensor.ext());
    algoim_spark_alloc(real, power);
    power = tensor;
    xarrayInit(tensor);
    for (auto i = power.loop(); ~i; ++i) {
        // phi.l(i) = powerFactors.l(i);
        real factorBase = power.l(i);
        if (factorBase == 0) continue;
        auto                           traverseRange = power.ext() - i();
        std::vector<std::vector<real>> decompFactors(N, std::vector<real>(max(traverseRange), 0.));
        for (int dim = 0; dim < N; ++dim) {
            // Sigma
            size_t      nDim      = power.ext()(dim) - 1;
            const real* binomNDim = Binomial::row(nDim);
            for (int j = i(dim); j <= nDim; ++j) {
                const real* binomJ             = Binomial::row(j);
                decompFactors[dim][j - i(dim)] = binomJ[i(dim)] / binomNDim[i(dim)];
            }
        }
        xarray<real, N> subgrid(nullptr, traverseRange);

        for (auto ii = subgrid.loop(); ~ii; ++ii) {
            real factor = factorBase;
            for (int dim = 0; dim < N; ++dim) { factor *= decompFactors[dim][ii(dim)]; }
            tensor.m(i() + ii()) += factor;
        }
    }
}

// 先缩放后平移，非原地修改
void powerTransformation(const uvector<real, 3>& scale,
                         const uvector<real, 3>& bias,
                         const xarray<real, 3>&  origin,
                         xarray<real, 3>&        res)
{
    assert(all(origin.ext() == res.ext()));
    std::vector<std::vector<std::vector<real>>> dimOrderExpansion;
    const auto&                                 ext = origin.ext();
    for (int dim = 0; dim < 3; ++dim) {
        dimOrderExpansion.push_back(std::vector<std::vector<real>>(ext(dim)));
        for (int degree = 0; degree < ext(dim); ++degree) {
            const real* binomDegree = Binomial::row(degree);
            dimOrderExpansion[dim][degree].reserve(degree + 1);
            // 根据二项定理展开
            for (int i = 0; i <= degree; ++i) {
                dimOrderExpansion[dim][degree].push_back(binomDegree[i] * pow(scale(dim), i) * pow(bias(dim), degree - i));
            }
        }
    }
    for (auto i = origin.loop(); ~i; ++i) {
        // 迭代器必须按照坐标的升序进行访问，即，访问ijk时，(i-1)jk，i(j-1)k，ij(k-1)必须已经访问过
        real base = origin.l(i);
        res.l(i)  = 0;
        auto exps = i();
        for (MultiLoop<3> j(0, exps + 1); ~j; ++j) {
            real item = base;
            for (int dim = 0; dim < 3; ++dim) { item *= dimOrderExpansion[dim][exps(dim)][j(dim)]; }
            res.m(j()) += item;
        }
    }
}

// 先缩放后平移，原地修改
void powerTransformation(const uvector<real, 3>& scale, const uvector<real, 3>& bias, xarray<real, 3>& phiPower)
{
    std::vector<std::vector<std::vector<real>>> dimOrderExpansion;
    const auto&                                 ext = phiPower.ext();
    for (int dim = 0; dim < 3; ++dim) {
        dimOrderExpansion.push_back(std::vector<std::vector<real>>(ext(dim)));
        for (int degree = 0; degree < ext(dim); ++degree) {
            const real* binomDegree = Binomial::row(degree);
            dimOrderExpansion[dim][degree].reserve(degree + 1);
            // 根据二项定理展开
            for (int i = 0; i <= degree; ++i) {
                dimOrderExpansion[dim][degree].push_back(binomDegree[i] * pow(scale(dim), i) * pow(bias(dim), degree - i));
            }
        }
    }
    for (auto i = phiPower.loop(); ~i; ++i) {
        // 迭代器必须按照坐标的升序进行访问，即，访问ijk时，(i-1)jk，i(j-1)k，ij(k-1)必须已经访问过
        real base     = phiPower.l(i);
        phiPower.l(i) = 0;
        auto exps     = i();
        for (MultiLoop<3> j(0, exps + 1); ~j; ++j) {
            real item = base;
            for (int dim = 0; dim < 3; ++dim) { item *= dimOrderExpansion[dim][exps(dim)][j(dim)]; }
            phiPower.m(j()) += item;
        }
    }
}

// Note: 旋转张量似乎会提高结果张量的阶次，需要旋转后再降阶
void powerRotation(const uvector3& vec, const real theta, const tensor3& originalPower, tensor3& res)
{
    // int maxDegree = sum(originalPower.ext()) - 3 + 1;
    // assert(all(res.ext() == maxDegree));
    // 获得旋转矩阵
    tensor2 rotation(nullptr, uvector2(3, 3));
    algoim_spark_alloc(real, rotation);
    Quaternion q(vec, theta);
    q.getRotation(rotation);
    // 旋转张量
    xarrayInit(res);
    for (auto i = originalPower.loop(); ~i; ++i) {
        real base = originalPower.l(i);
        if (base == 0) continue;
        auto exps = i();
        for (MultiLoop<3> jx(0, exps(0) + 1); ~jx; ++jx) {
            if (sum(jx()) != exps(0)) continue;
            for (MultiLoop<3> jy(0, exps(1) + 1); ~jy; ++jy) {
                if (sum(jy()) != exps(1)) continue;
                for (MultiLoop<3> jz(0, exps(2) + 1); ~jz; ++jz) {
                    if (sum(jz()) != exps(2)) continue;
                    uvector3 sumExps = jx() + jy() + jz();
                    real     item    = base;
                    for (int dim = 0; dim < 3; ++dim) {
                        item *= pow(rotation.m(uvector2(0, dim)), jx(dim));
                        item *= pow(rotation.m(uvector2(1, dim)), jy(dim));
                        item *= pow(rotation.m(uvector2(2, dim)), jz(dim));
                    }
                    item *= Factorial::calculate(exps(0))
                            / (Factorial::calculate(jx(0)) * Factorial::calculate(jx(1)) * Factorial::calculate(jx(2)));
                    item *= Factorial::calculate(exps(1))
                            / (Factorial::calculate(jy(0)) * Factorial::calculate(jy(1)) * Factorial::calculate(jy(2)));
                    item *= Factorial::calculate(exps(2))
                            / (Factorial::calculate(jz(0)) * Factorial::calculate(jz(1)) * Factorial::calculate(jz(2)));
                    assert(sumExps(0) < res.ext(0) && sumExps(1) < res.ext(1) && sumExps(2) < res.ext(2));
                    res.m(sumExps) += item;
                }
            }
        }
    }
}

void affineTransformation() {}

uvector3i getCompositeExt(const std::vector<tensor3>& tensors)
{
    uvector3i resExt = 1;
    for (const auto& t : tensors) { resExt += t.ext() - 1; }
    return resExt;
}

void compositePower(const std::vector<xarray<real, 3>>& powers,
                    int                                 powerIdx,
                    uvector<int, 3>                     powerSum,
                    real                                factor,
                    xarray<real, 3>&                    res)
{
    if (powerIdx == 0) {
        assert(all(res.ext() == getCompositeExt(powers)));
        xarrayInit(res);
    }
    if (powerIdx == powers.size()) {
        res.m(powerSum) += factor;
        return;
    }
    auto& power = powers[powerIdx];
    for (auto i = power.loop(); ~i; ++i) {
        if (power.l(i) == 0) {
            // factor = 0;
            continue;
        }
        compositePower(powers, powerIdx + 1, powerSum + i(), factor * power.l(i), res);
    }
}
} // namespace detail

// class Primitive
// {
// public:
//     virtual void print() = 0;

//     virtual real eval(const uvector3&) { return 0; }
// };

template <int N>
real evalPower(const xarray<real, N>& phi, const uvector<real, N>& x)
{
    real res = 0;
    for (auto i = phi.loop(); ~i; ++i) {
        real item = phi.l(i);
        auto exps = i();
        for (int dim = 0; dim < N; ++dim) { item *= pow(x(dim), exps(dim)); }
        res += item;
    }
    return res;
}

template <int N>
real evalBernstein(const xarray<real, N>& phi, const uvector<real, N>& x)
{
    return bernstein::evalBernsteinPoly(phi, x);
}

void inverseTensor(tensor3& tensor)
{
    for (auto i = tensor.loop(); ~i; ++i) { tensor.l(i) = -tensor.l(i); }
}

// class PowerTensor : public Primitive
// {
// public:
//     xarray<real, 3> tensor;

//     // SparkStack<real>* sparkStackPtr;

//     void print() override { std::cout << "Power" << std::endl; }

//     real eval(const uvector3& p) override { return evalPower(tensor, p); }

//     // PowerTensor() {}

//     PowerTensor(const xarray<real, 3>& t_) : tensor(t_) {}

//     // const xarray<real, 3>& getTensor() { return tensor; }

//     ~PowerTensor() = default;
// };

// class PowerTensorComplex : public Primitive
// {
// public:
//     xarray<real, 3>              compositeTensor; // 复合后的张量
//     SparkStack<real>*            sparkStackPtr;
//     std::vector<xarray<real, 3>> tensors;         // 原始张量

//     // std::vector<PowerTensor> powerTensors; // 原始张量

//     void print() override { std::cout << "PowerTensorComplex" << std::endl; }

//     static void compositePower(const std::vector<PowerTensor>& powers,
//                                int                             powerIdx,
//                                uvector<int, 3>                 powerSum,
//                                real                            factor,
//                                xarray<real, 3>&                res)

//     {
//         // const xarray<real, 3>& tensor
//         if (powerIdx == 0) {
//             {
//                 uvector3 ext(1, 1, 1);
//                 for (auto& t : powers) { ext += t.tensor.ext() - 1; }
//                 assert(all(ext == res.ext()));
//             }
//             xarrayInit(res);
//         }
//         if (powerIdx == powers.size()) {
//             res.m(powerSum) += factor;
//             return;
//         }
//         auto& tensor = powers[powerIdx].tensor;
//         for (auto i = tensor.loop(); ~i; ++i) {
//             if (tensor.l(i) == 0) {
//                 //factor = 0;
//                 continue;
//             }
//             compositePower(powers, powerIdx + 1, powerSum + i(), factor * tensor.l(i), res);
//         }
//     }

//     static void compositePower(const std::vector<xarray<real, 3>>& powers,
//                                int                                 powerIdx,
//                                uvector<int, 3>                     powerSum,
//                                real                                factor,
//                                xarray<real, 3>&                    res)

//     {
//         if (powerIdx == 0) {
//             {
//                 uvector3 ext(1, 1, 1);
//                 for (auto& t : powers) { ext += t.ext() - 1; }
//                 assert(all(ext == res.ext()));
//             }
//             xarrayInit(res);
//         }
//         if (powerIdx == powers.size()) {
//             res.m(powerSum) += factor;
//             return;
//         }
//         auto& power = powers[powerIdx];
//         for (auto i = power.loop(); ~i; ++i) {
//             if (power.l(i) == 0) {
//                // factor = 0;
//                 continue;
//             }
//             compositePower(powers, powerIdx + 1, powerSum + i(), factor * power.l(i), res);
//         }
//     }

//     // PowerTensorComplex() {}

//     PowerTensorComplex(const std::vector<xarray<real, 3>>& ts_, xarray<real, 3>& ct_) : tensors(ts_), compositeTensor(ct_)
//     {
//         uvector3 ext(1);
//         for (auto& t : ts_) { ext += t.ext() - 1; }
//         assert(all(ext == compositeTensor.ext()));
//         compositePower(tensors, 0, uvector3(0), 1, compositeTensor);
//     }

//     PowerTensorComplex(const std::vector<PowerTensor>& pts_, xarray<real, 3>& ct_) : compositeTensor(ct_)
//     {
//         uvector3 ext(1);
//         tensors.resize(pts_.size());
//         for (int i = 0; i < pts_.size(); ++i) {
//             tensors[i]  = pts_[i].tensor;
//             ext        += pts_[i].tensor.ext() - 1;
//         }
//         assert(all(ext == compositeTensor.ext()));
//         compositePower(tensors, 0, uvector3(0), 1, compositeTensor);
//     }

//     PowerTensorComplex(const std::vector<PowerTensorComplex>& ptcs_, xarray<real, 3>& ct_) : compositeTensor(ct_)
//     {
//         std::vector<xarray<real, 3>> originCompositeTensors;
//         uvector3                     ext(1);

//         for (auto& ptc : ptcs_) {
//             for (auto& t : ptc.tensors) { tensors.emplace_back(t); }
//             originCompositeTensors.push_back(ptc.compositeTensor);
//             ext += ptc.compositeTensor.ext() - 1;
//         }
//         compositePower(originCompositeTensors, 0, uvector3(0, 0, 0), 1, compositeTensor);
//     }

//     real eval(const uvector3& p) override { return evalPower(compositeTensor, p); }

//     bool isInside(const uvector3& p)
//     {
//         for (auto& t : tensors) {
//             if (evalPower(t, p) >= 0) { return false; }
//         }
//         return true;
//     }
// };

// class BernsteinPrimitive : public Primitive
// {
// };

// class BernsteinTensor : public BernsteinPrimitive
// {
// public:
//     xarray<real, 3> tensor;

//     void print() override { std::cout << "Bernstein" << std::endl; }

//     real eval(const uvector3& p) override { return evalBernstein(tensor, p); }

//     BernsteinTensor(const PowerTensor& pt_)
//     {
//         auto v0 = xarray2StdVector(pt_.tensor);

//         tensor.ext_ = pt_.tensor.ext();
//         algoim_spark_alloc(real, tensor);
//         auto v1 = xarray2StdVector(pt_.tensor);
//         auto v2 = xarray2StdVector(tensor);
//         detail::power2BernsteinTensor(pt_.tensor, tensor);

//         uvector3 x(0.2, 0.5, 0.6);
//         real     BernsteinValue = bernstein::evalBernsteinPoly(tensor, x);
//         real     PowerValue     = evalPower(pt_.tensor, x);
//         int      a              = 1;
//     }

//     bool isInside(uvector3 p) { return eval(p) < 0; }
// };

// class BernsteinTensorComplex : public BernsteinPrimitive
// {
// public:
//     bool                         fromPower;
//     xarray<real, 3>              compositeTensor; // 复合后的张量
//     std::vector<xarray<real, 3>> tensors;         // 原始张量

//     void print() override { std::cout << "Bernstein Complex" << std::endl; }

//     real eval(const uvector3& p) override { return evalBernstein(compositeTensor, p); }

//     // BernsteinTensorComplex(const PowerTensorComplex& pc_) : fromPower(true), tensors(pc_.tensors)
//     // {
//     //     compositeTensor.ext_ = pc_.compositeTensor.ext();
//     //     algoim_spark_alloc(real, compositeTensor);
//     //     detail::power2BernsteinTensor(pc_.compositeTensor, compositeTensor);
//     // };

//     bool isInside(uvector3 p)
//     {
//         if (fromPower) {
//             for (auto& t : tensors) {
//                 if (evalPower(t, p) >= 0) { return false; }
//             }
//             return true;
//         } else {
//             for (auto& t : tensors) {
//                 if (eval(p) >= 0) { return false; }
//             }
//             return true;
//         }
//         return true;
//     };
// };

bool isInsidePowers(const std::vector<tensor3>& tensors, const uvector3& p)
{
    for (auto& t : tensors) {
        if (evalPower(t, p) >= 0) { return false; }
    }
    return true;
};

bool isInsideBernstein(const tensor3& t, const uvector3& p) { return evalBernstein(t, p) < 0; }

bool isInsideBernsteins(const std::vector<tensor3>& tensors, const uvector3& p)
{
    for (auto& t : tensors) {
        if (evalBernstein(t, p) >= 0) { return false; }
    }
    return true;
}

bool isInsidePower(const tensor3& t, const uvector3& p) { return evalPower(t, p) < 0; }

// class FRep : public Primitive
// {
// private:
//     std::function<real(uvector3)> f;

// public:
//     void print() override { std::cout << "FRep" << std::endl; }

//     real eval(const uvector3& p) override { return f(p); }

//     FRep(std::function<real(uvector3)> f_) : f(f_) {}
// };

enum PrimitiveType { Sphere, Cylinder, Cone, Mesh, BRep };

class PrimitiveDesc
{
public:
    // const static PrimitiveType type;
    PrimitiveDesc() = default;

    virtual void print() {} // 空定义也可以，但是一定要有定义
};

class ParametricSurface
{
public:
    virtual uvector3 eval(uvector2 p) { return uvector3(0, 0, 0); }
};

class BezierSurface : public ParametricSurface
{
private:
    std::vector<std::vector<uvector3>> controlPoints;
};

class NURBSSurface : public ParametricSurface
{
    std::vector<std::vector<uvector3>> controlPoints;
    std::vector<std::vector<real>>     weights;
    std::vector<real>                  knotsU, knotsV;
    int                                degreeU, degreeV;
};

class ParametricCurve
{
public:
    virtual uvector3 eval(real p) { return uvector3(0, 0, 0); }
};

class BezierCurve : public ParametricCurve
{
private:
    std::vector<uvector3> controlPoints;
};

class NURBSCurve : public ParametricCurve
{
private:
    std::vector<uvector3> controlPoints;
    std::vector<real>     weights;
    std::vector<real>     knots;
    int                   degree;
};

class BRepDesc : virtual public PrimitiveDesc
{
public:
    const static PrimitiveType     type = BRep;
    std::vector<uvector3>          vertices;
    std::vector<ParametricCurve>   curves;
    std::vector<ParametricSurface> surfaces;

    void print() override { std::cout << "BRep Description" << std::endl; }

    real eval(const uvector3& p)
    {
        // DOTO: the implicit conversion of Parametric BRep
        return 0;
    }

    BRepDesc(const std::vector<uvector3>&          vs_,
             const std::vector<ParametricCurve>&   cs_,
             const std::vector<ParametricSurface>& ss_)
        : vertices(vs_), curves(cs_), surfaces(ss_)
    {
    }

    BRepDesc(const BRepDesc& brep)
    {
        vertices = brep.vertices;
        curves   = brep.curves;
        surfaces = brep.surfaces;
    }
};

class SphereDesc : virtual public PrimitiveDesc
{
public:
    const static PrimitiveType type = Sphere;
    real                       radius;
    uvector3                   center;
    uvector3                   amplitude;

    SphereDesc(real r_, const uvector3& c_, const uvector3& a_ = 1) : PrimitiveDesc(), radius(r_), center(c_), amplitude(a_) {}

    void print() override { std::cout << "Sphere Description" << std::endl; }
};

class CylinderDesc : virtual public PrimitiveDesc
{
public:
    const static PrimitiveType type = Cylinder;
    uvector3                   node1;
    // uvector3                   node2;
    real                       height;
    real                       radius;
    int                        alignAxis;

    // CylinderDesc(const uvector3& n1_, const uvector3& n2_, real r_) : PrimitiveDesc(), node1(n1_), node2(n2_), radius(r_) {}
    // 现在是只支持轴对齐x,y,z的圆柱
    // TODO: 实现张量积的旋转后换成支持任意方向的
    CylinderDesc(const uvector3& n1, real r, real h, int ax) : PrimitiveDesc(), node1(n1), radius(r), height(h), alignAxis(ax)
    {
        assert(alignAxis >= 0 && alignAxis <= 2);
        // node2             = node1;
        // node2(alignAxis) += h;
    }

    void print() override { std::cout << "Cylinder Description" << std::endl; }
};

class ConeDesc : virtual public PrimitiveDesc
{
public:
    const static PrimitiveType type = Cone;
    // uvector3                   top;
    uvector3                   bottom;
    real                       radiusTop, radiusBottom;
    real                       height;
    int                        alignAxis;

    ConeDesc(const uvector3& n1, real r, real h, int ax)
        : PrimitiveDesc(), bottom(n1), radiusBottom(r), radiusTop(0), height(h), alignAxis(ax)
    {
        assert(alignAxis >= 0 && alignAxis <= 2);
    }

    ConeDesc(const uvector3& bottom, const uvector3& top, real rBottom, real rTop)
        : PrimitiveDesc(), bottom(bottom), radiusBottom(rBottom), radiusTop(rTop)
    {
        assert(radiusTop >= 0 && radiusBottom > radiusTop);
        uvector3 dir  = top - bottom;
        height        = norm(dir);
        dir          /= height;
        if (std::abs(std::abs(dot(dir, uvector3(0, 0, 1))) - 1) < std::numeric_limits<real>::epsilon()) {
            alignAxis = 2;
        } else if (std::abs(std::abs(dot(dir, uvector3(0, 1, 0))) - 1) < std::numeric_limits<real>::epsilon()) {
            alignAxis = 1;
        } else if (std::abs(std::abs(dot(dir, uvector3(1, 0, 0))) - 1) < std::numeric_limits<real>::epsilon()) {
            alignAxis = 0;
        } else {
            assert(false);
        }
        if (dir(alignAxis) < 0) { height = -height; }
    }

    void print() override { std::cout << "Cone Description" << std::endl; }
};

class HalfPlaneDesc : virtual public PrimitiveDesc
{
public:
    // 基点，法向
    uvector3 basePt, normal;

    HalfPlaneDesc(const uvector3& basePt_, const uvector3& normal_) : PrimitiveDesc(), basePt(basePt_), normal(normal_) {}

    // 所有点应该位于一个面
    // 点顺序满足：叉乘指向法向，如 A B C D4点，ABxBC 与 BCxCD 与 normal同向
    HalfPlaneDesc(const std::vector<uvector3>& vertices)
    {
        assert(vertices.size() >= 3);
        uvector3 V01  = vertices[1] - vertices[0];
        uvector3 v02  = vertices[2] - vertices[0];
        normal        = cross(V01, v02);
        normal       /= norm(normal);
        real d        = -dot(normal, vertices[0]);
        basePt        = vertices[0];
        // test other vertices
        for (int i = 3; i < vertices.size(); ++i) {
            assert(dot(normal, vertices[i]) + d < std::numeric_limits<real>::epsilon());
        }
    }
};

class MeshDesc : virtual public PrimitiveDesc
{
public:
    const static PrimitiveType type = Mesh;
    std::vector<uvector3>      vertices;
    std::vector<int>           indices;
    std::vector<int>           indexInclusiveScan;

    MeshDesc(const std::vector<uvector3>& vertices_,
             const std::vector<int>&      indices_,
             const std::vector<int>&      indexInclusiveScan_)
        : PrimitiveDesc(), vertices(vertices_), indices(indices_), indexInclusiveScan(indexInclusiveScan_)
    {
    }

    /**
             @param vertices_ 顶点坐标
             @param indices_ 顶点索引
             @param faces_ 面索引 {顶点索引其实偏移（exclusive scan），顶点数}
     */
    MeshDesc(const std::vector<uvector3>& vertices_, const std::vector<int>& indices_, const std::vector<uvector2i>& faces_)
        : PrimitiveDesc(), vertices(vertices_), indices(indices_)
    {
        indexInclusiveScan.resize(faces_.size());
        for (int i = 0; i < faces_.size(); ++i) { indexInclusiveScan[i] = faces_[i](0) + faces_[i](1); }
        int aaa = 1;
    }

    MeshDesc() = default;

    void print() override { std::cout << "Mesh Description" << std::endl; }
};

class PyramidDesc : public MeshDesc
{
public:
    PyramidDesc(const std::vector<uvector3>& bottomVertices, const uvector3& topVertex) : PrimitiveDesc()
    {
        vertices       = bottomVertices;
        int bottomSize = bottomVertices.size();
        vertices.emplace_back(topVertex);
        indices.reserve(bottomSize * 3 + bottomSize);
        indexInclusiveScan.reserve(bottomSize + 1);
        for (int i = 0; i < bottomSize; ++i) {
            indices.emplace_back(i);
            indices.emplace_back(i == bottomSize - 1 ? 0 : indices.emplace_back(i + 1));
            indices.emplace_back(bottomSize);
            indexInclusiveScan.emplace_back(indexInclusiveScan.empty() ? 3 : indexInclusiveScan.back() + 3);
        }
        for (int i = 0; i < bottomSize; ++i) {
            indices.emplace_back(i);
            indices.emplace_back(i);
        }
        indexInclusiveScan.emplace_back(indexInclusiveScan.back() + bottomSize);
    }
};

class CuboidDesc : public MeshDesc
{
public:
    CuboidDesc(const uvector3& center, const uvector3& size) : MeshDesc()
    {
        vertices.resize(8);
        uvector3 halfSize = size / 2;
        for (int i = 0; i < 8; ++i) {
            vertices[i]     = center;
            vertices[i](0) += (i & 4) ? halfSize(0) : -halfSize(0);
            vertices[i](1) += (i & 2) ? halfSize(1) : -halfSize(1);
            vertices[i](2) += (i & 1) ? halfSize(2) : -halfSize(2);
        }
        indices            = {3, 2, 0, 1,  // left
                              4, 6, 7, 5,  // right
                              6, 2, 3, 7,  // top
                              1, 0, 4, 5,  // bottom
                              7, 3, 1, 5,  // front
                              2, 6, 4, 0}; // back
        indexInclusiveScan = {4, 8, 12, 16, 20, 24};
    }
};

struct VisiblePrimitiveRep {
    std::vector<tensor3> tensors;
    std::vector<AABB>    aabbs;
    AABB                 aabb;
    organizer::BlobTree  subBlobTree;

    bool isEmpty() const { return tensors.empty(); }
};

// hesai
struct Scene {
    // std::vector<CompleteTensorRep> polys;
    // std::vector<VisiblePrimitiveRep> visiblePrimitives;
    // organizer::BlobTree            blobTree;
    std::vector<MinimalPrimitiveRep> minimalReps;
    AABB                             boundary;
};

void makeHalfPlane(const HalfPlaneDesc& halfPlaneDesc, VisiblePrimitiveRep& visiblePrimitive)
{
    assert(visiblePrimitive.tensors.size() == 1);
    auto& tensor = visiblePrimitive.tensors[0];
    assert(all(tensor.ext() == 3));
    xarrayInit(tensor);
    tensor.m(uvector3(0, 0, 0)) = -dot(halfPlaneDesc.normal, halfPlaneDesc.basePt);
    tensor.m(uvector3(1, 0, 0)) = halfPlaneDesc.normal(0);
    tensor.m(uvector3(0, 1, 0)) = halfPlaneDesc.normal(1);
    tensor.m(uvector3(0, 0, 1)) = halfPlaneDesc.normal(2);
    // AABB
    visiblePrimitive.aabb.min   = -std::numeric_limits<real>::max();
    visiblePrimitive.aabb.max   = std::numeric_limits<real>::max();
    for (int dim = 0; dim < 3; ++dim) {
        if (halfPlaneDesc.normal(dim) == 1) {
            visiblePrimitive.aabb.max(dim) = halfPlaneDesc.basePt(dim);
        } else if (halfPlaneDesc.normal(dim) == -1) {
            visiblePrimitive.aabb.min(dim) = halfPlaneDesc.basePt(dim);
        }
    }
    // subBlobTree
    visiblePrimitive.subBlobTree.clear();
    buildNearBalancedBlobTree(visiblePrimitive.subBlobTree, 1);
}

void makeMesh(const MeshDesc& mesh, VisiblePrimitiveRep& visiblePrimitive)
{
    assert(visiblePrimitive.tensors.size() == mesh.indexInclusiveScan.size());
    for (int i = 0; i < mesh.indexInclusiveScan.size(); ++i) {
        const int indexBeg  = i == 0 ? 0 : mesh.indexInclusiveScan[i - 1];
        const int indexSize = mesh.indexInclusiveScan[i] - indexBeg;
        assert(indexSize >= 3);
        auto& planeTensor = visiblePrimitive.tensors[i];
        xarrayInit(planeTensor);
        auto&    vertices                 = mesh.vertices;
        auto&    indices                  = mesh.indices;
        uvector3 V01                      = vertices[indices[indexBeg + 1]] - mesh.vertices[indices[indexBeg]];
        uvector3 V02                      = vertices[indices[indexBeg + 2]] - mesh.vertices[indices[indexBeg]];
        uvector3 N                        = cross(V01, V02);
        N                                /= norm(N);
        real d                            = -dot(N, vertices[indices[indexBeg]]);
        // 法线所指方向为>0区域
        planeTensor.m(uvector3(0, 0, 0))  = d;
        planeTensor.m(uvector3(1, 0, 0))  = N(0);
        planeTensor.m(uvector3(0, 1, 0))  = N(1);
        planeTensor.m(uvector3(0, 0, 1))  = N(2);
        // test other vertices
        for (int j = indexBeg + 3; j < mesh.indexInclusiveScan[i]; ++j) {
            assert(dot(N, vertices[indices[j]]) + d < std::numeric_limits<real>::epsilon());
        }
    }
    // AABB
    for (const auto& v : mesh.vertices) { visiblePrimitive.aabb.extend(v); }
    // subBlobTree
    visiblePrimitive.subBlobTree.clear();
    buildNearBalancedBlobTree(visiblePrimitive.subBlobTree, mesh.indexInclusiveScan.size());
};

void makeSphere(const SphereDesc& sphereDesc, VisiblePrimitiveRep& visiblePrimitive)
{
    assert(visiblePrimitive.tensors.size() == 1);
    auto& tensor = visiblePrimitive.tensors[0];
    assert(all(tensor.ext() == 3));
    xarrayInit(tensor);
    for (int dim = 0; dim < 3; ++dim) {
        uvector<int, 3> idx  = 0;
        tensor.m(idx)       += sphereDesc.center(dim) * sphereDesc.center(dim);
        idx(dim)             = 1;
        tensor.m(idx)        = 2 * sphereDesc.amplitude(dim) * (-sphereDesc.center(dim));
        idx(dim)             = 2;
        tensor.m(idx)        = sphereDesc.amplitude(dim) * sphereDesc.amplitude(dim);
    }
    tensor.m(0) -= sphereDesc.radius * sphereDesc.radius;

    // auto range   = sceneBoundary.size();
    // detail::powerTransformation(range, sceneBoundary.min, tensor);

    // TODO consider the amplitude
    // AABB
    visiblePrimitive.aabb.min = sphereDesc.center - sphereDesc.radius;
    visiblePrimitive.aabb.max = sphereDesc.center + sphereDesc.radius;
};

void makeCylinder(const CylinderDesc& cylinderDesc, VisiblePrimitiveRep& visiblePrimitive)
{
    assert(visiblePrimitive.tensors.size() == 3);
    auto &cylinderSrf = visiblePrimitive.tensors[0], plane1 = visiblePrimitive.tensors[1], plane2 = visiblePrimitive.tensors[2];
    int   alignAxis             = cylinderDesc.alignAxis;
    uvector<int, 3> cylinderExt = 3;
    cylinderExt(alignAxis)      = 1;
    assert(all(cylinderSrf.ext() == cylinderExt));

    xarrayInit(cylinderSrf);
    xarrayInit(plane1);
    xarrayInit(plane2);
    int  dimA = (alignAxis + 1) % 3, dimB = (alignAxis + 2) % 3;
    real a = cylinderDesc.node1(dimA), b = cylinderDesc.node1(dimB), c = cylinderDesc.node1(alignAxis), r = cylinderDesc.radius;
    uvector<int, 3> idx = 0;
    plane1.m(idx)       = c;
    plane2.m(idx)       = -c - cylinderDesc.height;
    cylinderSrf.m(idx)  = a * a + b * b - r * r;
    idx(alignAxis)      = 1;
    plane1.m(idx)       = -1;
    plane2.m(idx)       = 1;
    idx(alignAxis)      = 0;
    idx(dimA)           = 1;
    cylinderSrf.m(idx)  = -2 * a;
    idx(dimA)           = 2;
    cylinderSrf.m(idx)  = 1;

    idx                = 0;
    idx(dimB)          = 1;
    cylinderSrf.m(idx) = -2 * b;
    idx(dimB)          = 2;
    cylinderSrf.m(idx) = 1;

    // auto range = sceneBoundary.size();
    // for (int i = 0; i < 3; ++i) { detail::powerTransformation(range, sceneBoundary.min, visiblePrimitive.tensors[i]); }

    // AABB
    visiblePrimitive.aabb.extend(cylinderDesc.node1);
    auto node2        = cylinderDesc.node1;
    node2(alignAxis) += cylinderDesc.height;
    visiblePrimitive.aabb.extend(node2);
    visiblePrimitive.aabb.min(dimA) -= r;
    visiblePrimitive.aabb.max(dimA) += r;
    visiblePrimitive.aabb.min(dimB) -= r;
    visiblePrimitive.aabb.max(dimB) += r;

    // subBlobTree
    visiblePrimitive.subBlobTree.clear();
    buildNearBalancedBlobTree(visiblePrimitive.subBlobTree, 3);
}

void makeCone(const ConeDesc& coneDesc, VisiblePrimitiveRep& visiblePrimitive)
{
    assert(visiblePrimitive.tensors.size() == 3);
    int alignAxis = coneDesc.alignAxis;
    assert(alignAxis >= 0 && alignAxis < 3);
    auto &coneSrf = visiblePrimitive.tensors[0], &plane1 = visiblePrimitive.tensors[1], &plane2 = visiblePrimitive.tensors[2];
    xarrayInit(coneSrf);
    xarrayInit(plane1);
    xarrayInit(plane2);
    assert(all(coneSrf.ext() == 3));

    // plane1 / plane2 分别为底面 / 顶点所在面
    int  dimA = (alignAxis + 1) % 3, dimB = (alignAxis + 2) % 3;
    real h = coneDesc.height, a = coneDesc.bottom(dimA), b = coneDesc.bottom(dimB), c = coneDesc.bottom(alignAxis) + h;
    real scale = h / (coneDesc.radiusBottom - coneDesc.radiusTop), scaleSquare = scale * scale;
    // f = (scale(x-a))^2 + (scale(y-b))^2 - (z-c)^2
    uvector<int, 3> idx = 0;
    plane1.m(idx)       = util::sign(h) * coneDesc.bottom(alignAxis);
    plane2.m(idx)       = -util::sign(h) * c;
    coneSrf.m(idx)      = scaleSquare * (a * a + b * b) - c * c;
    idx(alignAxis)      = 1;
    plane1.m(idx)       = -util::sign(h);
    plane2.m(idx)       = util::sign(h);
    coneSrf.m(idx)      = 2 * c;
    idx(alignAxis)      = 2;
    coneSrf.m(idx)      = -1;
    idx(alignAxis)      = 0;
    idx(dimA)           = 1;
    coneSrf.m(idx)      = scaleSquare * (-2 * a);
    idx(dimA)           = 2;
    coneSrf.m(idx)      = scaleSquare;
    idx                 = 0;
    idx(dimB)           = 1;
    coneSrf.m(idx)      = scaleSquare * (-2 * b);
    idx(dimB)           = 2;
    coneSrf.m(idx)      = scaleSquare;

    //     auto range = sceneBoundary.size();
    // for (int i = 0; i < 3; ++i) { detail::powerTransformation(range, sceneBoundary.min, visiblePrimitive.tensors[i]); }

    // AABB
    visiblePrimitive.aabb.extend(coneDesc.bottom);
    auto node2        = coneDesc.bottom;
    node2(alignAxis) += coneDesc.height;
    visiblePrimitive.aabb.extend(node2);
    visiblePrimitive.aabb.min(dimA) -= coneDesc.radiusBottom;
    visiblePrimitive.aabb.max(dimA) += coneDesc.radiusBottom;
    visiblePrimitive.aabb.min(dimB) -= coneDesc.radiusBottom;
    visiblePrimitive.aabb.max(dimB) += coneDesc.radiusBottom;

    // subBlobTree
    visiblePrimitive.subBlobTree.clear();
    buildNearBalancedBlobTree(visiblePrimitive.subBlobTree, 3);
}

void mergeSubtree2Leaf(BlobTree&                               blobTree,
                       std::vector<MinimalPrimitiveRep>&       minimalReps,
                       const std::vector<VisiblePrimitiveRep>& visiblePrimitiveReps)
{
    std::vector<int> realLeafIndices;
    for (int i = 0; i < blobTree.structure.size(); ++i) {
        int oldAncestor = blobTree.structure[i].ancestor;
        for (int j = visiblePrimitiveReps.size() - 1; blobTree.primitiveNodeIdx[j] > i; --j) {
            if (blobTree.structure[i].isLeft && oldAncestor + i > blobTree.primitiveNodeIdx[j]) {
                blobTree.structure[i].ancestor += std::max(int(visiblePrimitiveReps[j].subBlobTree.structure.size()) - 1, 0);
            }
        }
    }
    for (int i = 0; i < visiblePrimitiveReps.size(); ++i) {
        int originLeafIdx   = blobTree.primitiveNodeIdx[i];
        int subBlobTreeSize = visiblePrimitiveReps[i].subBlobTree.structure.size();
        if (visiblePrimitiveReps[i].tensors.size() != 1) {
            for (int j = i + 1; j < visiblePrimitiveReps.size(); ++j) {
                blobTree.primitiveNodeIdx[j] += std::max(int(subBlobTreeSize) - 1, 0);
            }
            blobTree.structure[originLeafIdx].isPrimitive = false;
            blobTree.structure[originLeafIdx].nodeOp      = visiblePrimitiveReps[i].subBlobTree.structure.back().nodeOp;
            blobTree.structure.insert(blobTree.structure.begin() + originLeafIdx,
                                      visiblePrimitiveReps[i].subBlobTree.structure.begin(),
                                      visiblePrimitiveReps[i].subBlobTree.structure.end() - 1);

            realLeafIndices.reserve(realLeafIndices.size() + visiblePrimitiveReps[i].subBlobTree.primitiveNodeIdx.size());
            for (auto primitiveIdx : visiblePrimitiveReps[i].subBlobTree.primitiveNodeIdx) {
                realLeafIndices.push_back(primitiveIdx + originLeafIdx);
            }
            minimalReps.reserve(minimalReps.size() + visiblePrimitiveReps[i].tensors.size());
            const auto& aabb = visiblePrimitiveReps[i].aabb;
            for (const auto& tensor : visiblePrimitiveReps[i].tensors) {
                minimalReps.emplace_back(MinimalPrimitiveRep{tensor, aabb});
            }
        } else {
            blobTree.structure[originLeafIdx].isPrimitive = true;
            realLeafIndices.push_back(originLeafIdx);
            minimalReps.emplace_back(MinimalPrimitiveRep{visiblePrimitiveReps[i].tensors[0], visiblePrimitiveReps[i].aabb});
        }
    }
    blobTree.primitiveNodeIdx = realLeafIndices;
}

// void makeMesh(const MeshDesc& mesh, xarray<real, 3>& tensor, std::vector<xarray<real, 3>>& planeTensors, AABB& aabb)
// {
//     uvector3 ext(1 + mesh.indexInclusiveScan.size());
//     assert(all(ext == tensor.ext()));
//     assert(planeTensors.size() == mesh.indexInclusiveScan.size());
//     // for (const auto& index : indices) {
//     for (int i = 0; i < mesh.indexInclusiveScan.size(); ++i) {
//         const int indexBeg  = i == 0 ? 0 : mesh.indexInclusiveScan[i - 1];
//         const int indexSize = mesh.indexInclusiveScan[i] - indexBeg;
//         assert(indexSize >= 3);
//         auto& planeTensor = planeTensors[i];
//         xarrayInit(planeTensor);
//         auto&    vertices                 = mesh.vertices;
//         auto&    indices                  = mesh.indices;
//         uvector3 V01                      = vertices[indices[indexBeg + 1]] - mesh.vertices[indices[indexBeg]];
//         uvector3 V02                      = vertices[indices[indexBeg + 2]] - mesh.vertices[indices[indexBeg]];
//         uvector3 N                        = cross(V01, V02);
//         N                                /= norm(N);
//         real d                            = -dot(N, vertices[indices[indexBeg]]);
//         // 法线所指方向为>0区域
//         planeTensor.m(uvector3(0, 0, 0))  = d;
//         planeTensor.m(uvector3(1, 0, 0))  = N(0);
//         planeTensor.m(uvector3(0, 1, 0))  = N(1);
//         planeTensor.m(uvector3(0, 0, 1))  = N(2);
//         // test other vertices
//         for (int j = indexBeg + 3; j < mesh.indexInclusiveScan[i]; ++j) {
//             assert(dot(N, vertices[indices[j]]) + d < std::numeric_limits<real>::epsilon());
//         }
//     }
//     detail::compositePower(planeTensors, 0, 0, 1, tensor);
//     if (mesh.indexInclusiveScan.size() % 2 == 0) { inverseTensor(tensor); }
//     // AABB
//     for (const auto& v : mesh.vertices) { aabb.extend(v); }
// };

// void makeSphere(const SphereDesc& sphereDesc, xarray<real, 3>& tensor, AABB& aabb)
// {
//     uvector<int, 3> ext = 3;
//     assert(all(ext == tensor.ext()));

//     for (int dim = 0; dim < 3; ++dim) {
//         uvector<int, 3> idx  = 0;
//         tensor.m(idx)       += sphereDesc.center(dim) * sphereDesc.center(dim);
//         idx(dim)             = 1;
//         tensor.m(idx)        = 2 * sphereDesc.amplitude(dim) * (-sphereDesc.center(dim));
//         idx(dim)             = 2;
//         tensor.m(idx)        = sphereDesc.amplitude(dim) * sphereDesc.amplitude(dim);
//     }
//     tensor.m(0) -= sphereDesc.radius * sphereDesc.radius;
//     // TODO consider the amplitude
//     aabb.min     = sphereDesc.center - sphereDesc.radius;
//     aabb.max     = sphereDesc.center + sphereDesc.radius;
// };

// void makeCylinder(const CylinderDesc& cylinderDesc, xarray<real, 3>& tensor, std::vector<tensor3>& rawTensors, AABB& aabb)
// {
//     auto &          cylinderSrf = rawTensors[0], plane1 = rawTensors[1], plane2 = rawTensors[2];
//     int             alignAxis   = cylinderDesc.alignAxis;
//     uvector<int, 3> cylinderExt = 3;
//     cylinderExt(alignAxis)      = 1;
//     assert(rawTensors.size() == 3 && all(cylinderSrf.ext() == cylinderExt) && all(plane1.ext() == uvector3(2))
//            && all(plane2.ext() == uvector3(2)));
//     assert(all(detail::getCompositeExt(rawTensors) == tensor.ext()));

//     int  dimA = (alignAxis + 1) % 3, dimB = (alignAxis + 2) % 3;
//     real a = cylinderDesc.node1(dimA), b = cylinderDesc.node1(dimB), c = cylinderDesc.node1(alignAxis), r =
//     cylinderDesc.radius; uvector<int, 3> idx = 0; plane1.m(idx)       = c; plane2.m(idx)       = -c - cylinderDesc.height;
//     cylinderSrf.m(idx)  = a * a + b * b - r * r;
//     idx(alignAxis)      = 1;
//     plane1.m(idx)       = -1;
//     plane2.m(idx)       = 1;
//     idx(alignAxis)      = 0;
//     idx(dimA)           = 1;
//     cylinderSrf.m(idx)  = -2 * a;
//     idx(dimA)           = 2;
//     cylinderSrf.m(idx)  = 1;

//     idx                = 0;
//     idx(dimB)          = 1;
//     cylinderSrf.m(idx) = -2 * b;
//     idx(dimB)          = 2;
//     cylinderSrf.m(idx) = 1;

//     detail::compositePower(rawTensors, 0, 0, 1, tensor);

//     // AABB
//     aabb.extend(cylinderDesc.node1);
//     auto node2        = cylinderDesc.node1;
//     node2(alignAxis) += cylinderDesc.height;
//     aabb.extend(node2);
//     aabb.min(dimA) -= r;
//     aabb.max(dimA) += r;
//     aabb.min(dimB) -= r;
//     aabb.max(dimB) += r;
// }

// void makeCone(const ConeDesc& coneDesc, tensor3& tensor, std::vector<tensor3>& rawTensors, AABB& aabb)
// {
//     assert(rawTensors.size() == 3 && all(rawTensors[0].ext() == 3) && all(rawTensors[1].ext() == 2)
//            && all(rawTensors[2].ext() == 2));
//     assert(all(tensor.ext() == detail::getCompositeExt(rawTensors)));
//     auto &coneSrf = rawTensors[0], &plane1 = rawTensors[1], &plane2 = rawTensors[2];
//     int   alignAxis = coneDesc.alignAxis;

//     int             dimA = (alignAxis + 1) % 3, dimB = (alignAxis + 2) % 3;
//     real            a = coneDesc.node1(dimA), b = coneDesc.node1(dimB), c = coneDesc.node1(alignAxis);
//     real            scale = coneDesc.height / coneDesc.radius, scaleSquare = scale * scale;
//     // f = (scale(x-a))^2 + (scale(y-b))^2 - (z-c)^2
//     uvector<int, 3> idx = 0;
//     plane1.m(idx)       = c;
//     plane2.m(idx)       = -c - coneDesc.height;
//     coneSrf.m(idx)      = scaleSquare * (a * a + b * b) - c * c;
//     idx(alignAxis)      = 1;
//     plane1.m(idx)       = -1;
//     plane2.m(idx)       = 1;
//     coneSrf.m(idx)      = 2 * c;
//     idx(alignAxis)      = 2;
//     coneSrf.m(idx)      = -1;
//     idx(alignAxis)      = 0;
//     idx(dimA)           = 1;
//     coneSrf.m(idx)      = scaleSquare * (-2 * a);
//     idx(dimA)           = 2;
//     coneSrf.m(idx)      = scaleSquare;
//     idx                 = 0;
//     idx(dimB)           = 1;
//     coneSrf.m(idx)      = scaleSquare * (-2 * b);
//     idx(dimB)           = 2;
//     coneSrf.m(idx)      = scaleSquare;

//     detail::compositePower(rawTensors, 0, 0, 1, tensor);

//     // AABB
//     aabb.extend(coneDesc.node1);
//     auto node2        = coneDesc.node1;
//     node2(alignAxis) += coneDesc.height;
//     aabb.extend(node2);
//     aabb.min(dimA) -= coneDesc.radius;
//     aabb.max(dimA) += coneDesc.radius;
//     aabb.min(dimB) -= coneDesc.radius;
//     aabb.max(dimB) += coneDesc.radius;
// }

// struct CompleteTensorRep {
//     tensor3              compositedBernstein; // 合法输入在[0,1]^3
//     std::vector<tensor3> rawPower;            // 合法输入在场景<min, max>
//     AABB                 aabb;
// };


} // namespace algoim::organizer