#include <array>
#include <bitset>
#include <cassert>
#include <iostream>
#include <booluarray.hpp>


#include <cstddef>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include "bernstein.hpp"
#include "factorial.hpp"
#include "multiloop.hpp"
#include "quadrature_multipoly.hpp"
#include "binomial.hpp"

#include "real.hpp"
#include "sparkstack.hpp"
#include "utility.hpp"
#include "uvector.hpp"
#include "vector"
#include "xarray.hpp"
#include <chrono>
#include <memory>

#include "organizer/primitive.hpp"
#include "organizer/organizer.hpp"

#include "organizer/blobtree.hpp"


using namespace algoim::organizer;
using namespace algoim;

// void casePolyhedron1()
// {
//     // std::vector<std::shared_ptr<PrimitiveDesc>> ps;

//     // mesh
//     std::vector<uvector3> vertices = {uvector3(-0.8, -0.8, -0.8), uvector3(-0.8, -0.8, 0.8), uvector3(-0.8, 0.8, -0.8),
//                                       uvector3(-0.8, 0.8, 0.8),   uvector3(0.8, -0.8, -0.8), uvector3(0.8, -0.8, 0.8),
//                                       uvector3(0.8, 0.8, -0.8),   uvector3(0.8, 0.8, 0.8)};
//     std::vector<int>      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
//     std::vector<int>      scan     = {4, 8, 12, 16, 20, 24};

//     // ps.emplace_back(std::make_shared<MeshDesc>(vertices, indices, scan));
//     // ps.emplace_back(std::make_shared<SphereDesc>(0.8, 0., 1.));
//     // ps.emplace_back(std::make_shared<PowerTensor>(makeSphere(0.2)));
//     // ps.emplace_back(std::make_shared<MeshDesc>(MeshDesc(vertices, indices, scan)));
//     basicTask(std::make_shared<MeshDesc>(MeshDesc(vertices, indices, scan)));
// }

void casePolyhedron2()
{
    // std::vector<std::shared_ptr<PrimitiveDesc>> ps;

    // mesh
    std::vector<uvector3> vertices = {uvector3(-0.8, -0.8, -0.8), uvector3(-0.8, -0.8, 0.8), uvector3(-0.8, 0.8, -0.8),
                                      uvector3(-0.8, 0.8, 0.8),   uvector3(0.8, -0.8, -0.8), uvector3(0.8, -0.8, 0.8),
                                      uvector3(0.8, 0.8, -0.8),   uvector3(0.8, 0.8, 0.8)};
    std::vector<int>      indices  = {
        3, 2, 0, 1, // left
        6, 2, 3, 7  // top
    };
    std::vector<int> scan = {4, 8};

    // ps.emplace_back(std::make_shared<MeshDesc>(vertices, indices, scan));
    // ps.emplace_back(std::make_shared<SphereDesc>(0.8, 0., 1.));
    // ps.emplace_back(std::make_shared<PowerTensor>(makeSphere(0.2)));
    // ps.emplace_back(std::make_shared<MeshDesc>(MeshDesc(vertices, indices, scan)));
    basicTask(std::make_shared<MeshDesc>(MeshDesc(vertices, indices, scan)));
}

void casePolyhedron3()
{
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions;
    // std::vector<uvector3>                       vertices = {uvector3(-1.6, 0, 0),
    //                                                         uvector3(-1.6, 0, 1.6),
    //                                                         uvector3(-1.6, 1.6, 0),
    //                                                         uvector3(-1.6, 1.6, 1.6),
    //                                                         uvector3(1.6, 0, 0),
    //                                                         uvector3(1.6, 0, 1.6),
    //                                                         uvector3(1.6, 1.6, 0),
    //                                                         uvector3(1.6, 1.6, 1.6)};
    std::vector<uvector3> vertices = {uvector3(-0.8, -0.8, -0.8), uvector3(-0.8, -0.8, 0.8), uvector3(-0.8, 0.8, -0.8),
                                      uvector3(-0.8, 0.8, 0.8),   uvector3(0.8, -0.8, -0.8), uvector3(0.8, -0.8, 0.8),
                                      uvector3(0.8, 0.8, -0.8),   uvector3(0.8, 0.8, 0.8)};
    std::vector<int>      indices  = {3, 2, 0,
                                      1, // left
                                      6, 2, 3,
                                      7, // top
                                      // 7, 3, 1,
                                      // 5,           // front
                                      2, 6, 4, 0}; // back
    std::vector<int>      scan     = {4, 8, 12};
    // std::vector<int>      indices  = {1, 2, 5, //
    //                                   5, 2, 4, //
    //                                   1, 4, 2};
    // std::vector<int>      scan     = {3, 6, 9};
    basicTask(std::make_shared<MeshDesc>(MeshDesc(vertices, indices, scan)));
}

void casePolyhedronSphere()
{
    // PI * r^3 / 6
    auto                  phi0     = std::make_shared<SphereDesc>(SphereDesc(0.8, uvector3(-0.8, 0.8, -0.8), 1.));
    std::vector<uvector3> vertices = {uvector3(-0.8, -0.8, -0.8), uvector3(-0.8, -0.8, 0.8), uvector3(-0.8, 0.8, -0.8),
                                      uvector3(-0.8, 0.8, 0.8),   uvector3(0.8, -0.8, -0.8), uvector3(0.8, -0.8, 0.8),
                                      uvector3(0.8, 0.8, -0.8),   uvector3(0.8, 0.8, 0.8)};
    std::vector<int>      indices  = {3, 2, 0, 1,  // left
                                      6, 2, 3, 7,  // top
                                      2, 6, 4, 0}; // back
    std::vector<int>      scan     = {4, 8, 12};
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions(3);
    primitiveDescriptions[0] = phi0;
    primitiveDescriptions[1] = std::make_shared<MeshDesc>(MeshDesc(vertices, indices, scan));
    basicTask(primitiveDescriptions);
}

void caseTwoCube()
{
    auto phi0 = std::make_shared<CuboidDesc>(CuboidDesc(0.2, 0.6));
    auto phi1 = std::make_shared<CuboidDesc>(CuboidDesc(-0.2, 0.6));
    basicTask({phi0, phi1});
}

void caseCubeSphere()
{
    auto phi0 = std::make_shared<CuboidDesc>(CuboidDesc(0.2, 0.6));
    auto phi1 = std::make_shared<SphereDesc>(SphereDesc(0.6, -0.2, 1.));
    basicTask({phi0, phi1});
}

void caseCone()
{
    const int                                   PRIMITIVE_CNT = 4;
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions(PRIMITIVE_CNT);
    primitiveDescriptions[0] = std::make_shared<CuboidDesc>(CuboidDesc(0.0, 1.6));
    primitiveDescriptions[1] = std::make_shared<CuboidDesc>(CuboidDesc(uvector3(0.6, 0.6, -0.6), 2.));
    primitiveDescriptions[2] = std::make_shared<SphereDesc>(SphereDesc(0.2, uvector3(0.2, -0.7, 0.), 1.));
    primitiveDescriptions[3] = std::make_shared<ConeDesc>(ConeDesc(uvector3(0., -0.2, 0.), 0.4, -0.7, 1));
    organizer::BlobTree blobTree;
    blobTree.structure.resize(PRIMITIVE_CNT * 2 - 1);
    blobTree.primitiveNodeIdx.resize(PRIMITIVE_CNT);
    // blobTree.structure[0]     = {0, 0, NODE_IN_OUT_UNKNOWN, 0, 1, 2}; // cube1
    blobTree.structure[0]     = {1, 0, NODE_IN_OUT_UNKNOWN, 0, 1, 2 - 0};         // cube
    blobTree.structure[1]     = {1, 0, 0, 0, 0, 0};                               // cube2
    blobTree.structure[2]     = {0, OP_UNION, 0, 0, 1, 6 - 2};                    // Union of cubes = opNode1
    blobTree.structure[3]     = {1, 0, NODE_IN_OUT_UNKNOWN, 0, 1, 5 - 3};         // cone
    blobTree.structure[4]     = {1, 0, NODE_IN_OUT_UNKNOWN, 0, 0, 0};             // sphere
    blobTree.structure[5]     = {0, OP_UNION, NODE_IN_OUT_UNKNOWN, 0, 0, 0};      // cone u sphere = n1
    blobTree.structure[6]     = {0, OP_DIFFERENCE, NODE_IN_OUT_UNKNOWN, 0, 0, 0}; // cube - n1 = n2
    blobTree.primitiveNodeIdx = {0, 1, 3, 4};
    quadratureScene(primitiveDescriptions, uvector3(-0.8, -1.3, -1.6), uvector3(1.6, 1.6, 1.5), blobTree, 10);
}

void caseScene()
{
    const int                                   PRIMITIVE_CNT = 6;
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions(PRIMITIVE_CNT);
    primitiveDescriptions[0]                    = std::make_shared<CuboidDesc>(CuboidDesc(0., 1.6));
    primitiveDescriptions[1]                    = std::make_shared<CuboidDesc>(CuboidDesc(uvector3(0.6, 0.6, -0.6), 2.));
    // primitiveDescriptions[1]                    = std::make_shared<SphereDesc>(SphereDesc(1., uvector3(0.6, 0.6, -0.6), 1.));
    primitiveDescriptions[2]                    = std::make_shared<SphereDesc>(SphereDesc(0.7, uvector3(0.8, 0.8, 0.8), 1.));
    primitiveDescriptions[3]                    = std::make_shared<SphereDesc>(SphereDesc(0.5, uvector3(-0.3, -0.8, 0.8), 1.));
    std::vector<uvector3> pyramidBottomVertices = {
        uvector3{-1, -1, 0},
        uvector3{1,  -1, 0},
        uvector3{1,  1,  0},
        uvector3{-1, 1,  0}
    };
    // primitiveDescriptions[4] = std::make_shared<PyramidDesc>(pyramidBottomVertices, uvector3{0, 0, 1});
    primitiveDescriptions[4] = std::make_shared<SphereDesc>(SphereDesc(0.2, uvector3(0.2, -0.7, 0.), 1.));

    // primitiveDescriptions[5] = std::make_shared<SphereDesc>(SphereDesc(0.2, uvector3(0., -0.5, 0.), 1));
    primitiveDescriptions[5] = std::make_shared<ConeDesc>(ConeDesc(uvector3(0., -0.2, 0.), 0.4, -0.7, 1));
    // primitiveDescriptions[6] = std::make_shared<CylinderDesc>(CylinderDesc(uvector3(-0.3, 0.3, 2.3), 0.4, 3.6, 1));
    organizer::BlobTree blobTree;
    blobTree.structure.resize(PRIMITIVE_CNT * 2 - 1);
    blobTree.primitiveNodeIdx.resize(PRIMITIVE_CNT);
    blobTree.structure[0]  = {1, 0, 0, 0, 1, 2};                  // cube1
    blobTree.structure[1]  = {1, 0, 0, 0, 0, 0};                  // cube2
    blobTree.structure[2]  = {0, OP_UNION, 0, 0, 1, 6 - 2};       // Union of cubes = opNode1
    blobTree.structure[3]  = {1, 0, 0, 0, 1, 5 - 3};              // sphere1
    blobTree.structure[4]  = {1, 0, 0, 0, 0, 0};                  // sphere2
    blobTree.structure[5]  = {0, OP_UNION, 0, 0, 0, 0};           // Union of spheres = opNode2
    blobTree.structure[6]  = {0, OP_DIFFERENCE, 0, 0, 1, 10 - 6}; // Difference of opNode1 and opNode2 = opNode3
    blobTree.structure[7]  = {1, 0, 0, 0, 1, 9 - 7};              // Pyramid (sphere3)
    blobTree.structure[8]  = {1, 0, 0, 0, 0, 0};                  // Cone
    blobTree.structure[9]  = {0, OP_UNION, 0, 0, 0, 0};           // UNION of Pyramid (sphere3) and Cone = opNode4
    blobTree.structure[10] = {
        0, OP_DIFFERENCE, 0, 0, 1, 12 - 10};                      // Difference of opNode3 and opNode4 = opNode5
                                             // blobTree.structure[11]    = {1, 0, 0, 0, 0, 0};  // Cylinder
    // blobTree.structure[12]    = {0, 2, 0, 0, 1, 0};  // Difference of opNode5 and Cylinder
    // blobTree.primitiveNodeIdx = {0, 1, 3, 4, 7, 8, 11};
    // quadratureScene(primitiveDescriptions, uvector3(-1., -1.3, -1.6), uvector3(1.6, 1.6, 2.3), blobTree);
    blobTree.primitiveNodeIdx = {0, 1, 3, 4, 7, 8};
    quadratureScene(primitiveDescriptions, uvector3(-0.8, -1.3, -1.6), uvector3(1.6, 1.6, 1.5), blobTree);
}

void caseScene30Objs()
{
    const int                                   PRIMITIVE_CNT = 30;
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions(PRIMITIVE_CNT);
    uvector3                                    sceneSize = 0.8, sceneCenter = 0.;
    for (MultiLoop<3> i(0, 2); ~i; ++i) {
        uvector3 center                    = sceneSize * i() - 0.5 * sceneSize + sceneCenter;
        int      idx                       = organizer::detail::binary2Decimal(i());
        primitiveDescriptions[2 * idx]     = std::make_shared<CuboidDesc>(CuboidDesc(center, 0.16));
        primitiveDescriptions[2 * idx + 1] = std::make_shared<SphereDesc>(SphereDesc(0.15, center, 1.));
    } // 16 primitives
    for (int dim = 0; dim < 3; ++dim) {
        for (MultiLoop<2> i(0, 2); ~i; ++i) {
            uvector3 bottomCenter = add_component(i(), dim, 0) * sceneSize - 0.5 * sceneSize + sceneCenter;
            primitiveDescriptions[organizer::detail::binary2Decimal(i()) + 4 * dim + 16] =
                std::make_shared<CylinderDesc>(CylinderDesc(bottomCenter, 0.1, sceneSize(dim), dim));
        }
    } // 12 primitives
    uvector3 coneBottom                         = uvector3(1, 0, 0) * sceneSize - 0.5 * sceneSize + sceneCenter;
    primitiveDescriptions[28]                   = std::make_shared<ConeDesc>(coneBottom, 0.2, 0.5, 1);
    uvector3              pyramidBottom         = uvector3(0, 0, 1) * sceneSize - 0.5 * sceneSize + sceneCenter;
    std::vector<uvector3> pyramidBottomVertices = {
        0.2 * pyramidBottom + uvector3{-1, 0, -1},
            0.2 * pyramidBottom + uvector3{1,  0, -1},
        0.2 * pyramidBottom + uvector3{1,  0, 1 },
            0.2 * pyramidBottom + uvector3{-1, 0, 1 }
    };
    primitiveDescriptions[29] = std::make_shared<PyramidDesc>(pyramidBottomVertices, pyramidBottom + uvector3{0, 0.7, 0});

    organizer::BlobTree blobTree;
    blobTree.structure.resize(PRIMITIVE_CNT * 2 - 1);
    blobTree.primitiveNodeIdx.resize(PRIMITIVE_CNT);
    blobTree.structure[0]  = {1, 0, 0, 0, 1, 2};                   // cube1
    blobTree.structure[1]  = {1, 0, 0, 0, 0, 0};                   // sphere1
    blobTree.structure[2]  = {0, OP_DIFFERENCE, 0, 0, 1, 6 - 2};   // Dif of = opNode1
    blobTree.structure[3]  = {1, 0, 0, 0, 1, 5 - 3};               // cube2
    blobTree.structure[4]  = {1, 0, 0, 0, 0, 0};                   // sphere2
    blobTree.structure[5]  = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode2
    blobTree.structure[6]  = {0, OP_UNION, 0, 0, 1, 14 - 6};       // Union of = opNode3
    blobTree.structure[7]  = {1, 0, 0, 0, 1, 9 - 7};               // cube3
    blobTree.structure[8]  = {1, 0, 0, 0, 0, 0};                   // sphere3
    blobTree.structure[9]  = {0, OP_DIFFERENCE, 0, 0, 1, 13 - 9};  // Dif of = opNode4
    blobTree.structure[10] = {1, 0, 0, 0, 1, 12 - 10};             // cube4
    blobTree.structure[11] = {1, 0, 0, 0, 0, 0};                   // sphere4
    blobTree.structure[12] = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode5
    blobTree.structure[13] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode6
    blobTree.structure[14] = {0, OP_UNION, 0, 0, 1, 30 - 14};      // Union of = opNode7
    blobTree.structure[15] = {1, 0, 0, 0, 1, 17 - 15};             // cube5
    blobTree.structure[16] = {1, 0, 0, 0, 0, 0};                   // sphere5
    blobTree.structure[17] = {0, OP_DIFFERENCE, 0, 0, 1, 21 - 17}; // Dif of = opNode8
    blobTree.structure[18] = {1, 0, 0, 0, 1, 20 - 18};             // cube6
    blobTree.structure[19] = {1, 0, 0, 0, 0, 0};                   // sphere6
    blobTree.structure[20] = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode9
    blobTree.structure[21] = {0, OP_UNION, 0, 0, 1, 29 - 21};      // Union of = opNode10
    blobTree.structure[22] = {1, 0, 0, 0, 1, 24 - 22};             // cube7
    blobTree.structure[23] = {1, 0, 0, 0, 0, 0};                   // sphere7
    blobTree.structure[24] = {0, OP_DIFFERENCE, 0, 0, 1, 28 - 24}; // Dif of = opNode11
    blobTree.structure[25] = {1, 0, 0, 0, 1, 27 - 25};             // cube8
    blobTree.structure[26] = {1, 0, 0, 0, 0, 0};                   // sphere8
    blobTree.structure[27] = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode12
    blobTree.structure[28] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode13
    blobTree.structure[29] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode14
    blobTree.structure[30] = {0, OP_UNION, 0, 0, 1, 32 - 30};      // Dif of = opNode15

    blobTree.structure[31] = {1, 0, 0, 0, 1, 33 - 31};             // cylinder1
    blobTree.structure[32] = {1, 0, 0, 0, 0, 0};                   // cylinder2
    blobTree.structure[33] = {0, OP_UNION, 0, 0, 1, 37 - 33};      // Union of = opNode16
    blobTree.structure[34] = {1, 0, 0, 0, 1, 36 - 34};             // cylinder3
    blobTree.structure[35] = {1, 0, 0, 0, 0, 0};                   // cylinder4
    blobTree.structure[36] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode17
    blobTree.structure[37] = {0, OP_UNION, 0, 0, 1, 45 - 37};      // Union of = opNode18
    blobTree.structure[38] = {1, 0, 0, 0, 1, 40 - 38};             // cylinder5
    blobTree.structure[39] = {1, 0, 0, 0, 0, 0};                   // cylinder6
    blobTree.structure[40] = {0, OP_UNION, 0, 0, 1, 44 - 40};      // Union of = opNode19
    blobTree.structure[41] = {1, 0, 0, 0, 1, 43 - 41};             // cylinder7
    blobTree.structure[42] = {1, 0, 0, 0, 0, 0};                   // cylinder8
    blobTree.structure[43] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode20
    blobTree.structure[44] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode21
    blobTree.structure[45] = {0, OP_UNION, 0, 0, 1, 53 - 45};      // Union of = opNode22
    blobTree.structure[46] = {1, 0, 0, 0, 1, 48 - 46};             // cylinder9
    blobTree.structure[47] = {1, 0, 0, 0, 0, 0};                   // cylinder10
    blobTree.structure[48] = {0, OP_UNION, 0, 0, 1, 52 - 48};      // Union of = opNode23
    blobTree.structure[49] = {1, 0, 0, 0, 1, 51 - 49};             // cylinder11
    blobTree.structure[50] = {1, 0, 0, 0, 0, 0};                   // cylinder12
    blobTree.structure[51] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode24
    blobTree.structure[52] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode25
    blobTree.structure[53] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode26

    blobTree.structure[54] = {0, OP_UNION, 0, 0, 1, 58 - 54};      // UNION of = opNode27

    blobTree.structure[55] = {1, 0, 0, 0, 1, 57 - 55};             // cone
    blobTree.structure[56] = {1, 0, 0, 0, 0, 0};                   // pyramid
    blobTree.structure[57] = {0, OP_UNION, 0, 0, 1, 59 - 57};      // UNION of = opNode28
    blobTree.structure[58] = {0, OP_UNION, 0, 0, 0, 0};            // UNION of = opNode29

    int leafCnt = 0;
    for (int i = 0; i < blobTree.structure.size(); ++i) {
        if (blobTree.structure[i].isPrimitive) { blobTree.primitiveNodeIdx[leafCnt++] = i; }
    }
    assert(leafCnt == PRIMITIVE_CNT);
    quadratureScene(primitiveDescriptions, sceneCenter - 0.5 * sceneSize - 0.2, sceneCenter + 0.5 * sceneSize + 0.2, blobTree);
}

void caseScene16Objs()
{
    const int                                   PRIMITIVE_CNT = 16;
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions(PRIMITIVE_CNT);
    uvector3                                    sceneSize = 1.6, sceneCenter = 0.;
    for (MultiLoop<3> i(0, 2); ~i; ++i) {
        uvector3 center                    = sceneSize * i() - 0.5 * sceneSize + sceneCenter;
        int      idx                       = organizer::detail::binary2Decimal(i());
        primitiveDescriptions[2 * idx]     = std::make_shared<CuboidDesc>(CuboidDesc(center, 0.16));
        primitiveDescriptions[2 * idx + 1] = std::make_shared<SphereDesc>(SphereDesc(0.075, center, 1.));
    } // 16 primitives

    organizer::BlobTree blobTree;
    blobTree.structure.resize(PRIMITIVE_CNT * 2 - 1);
    blobTree.primitiveNodeIdx.resize(PRIMITIVE_CNT);
    blobTree.structure[0]  = {1, 0, 0, 0, 1, 2};                   // cube1
    blobTree.structure[1]  = {1, 0, 0, 0, 0, 0};                   // sphere1
    blobTree.structure[2]  = {0, OP_DIFFERENCE, 0, 0, 1, 6 - 2};   // Dif of = opNode1
    blobTree.structure[3]  = {1, 0, 0, 0, 1, 5 - 3};               // cube2
    blobTree.structure[4]  = {1, 0, 0, 0, 0, 0};                   // sphere2
    blobTree.structure[5]  = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode2
    blobTree.structure[6]  = {0, OP_UNION, 0, 0, 1, 14 - 6};       // Union of = opNode3
    blobTree.structure[7]  = {1, 0, 0, 0, 1, 9 - 7};               // cube3
    blobTree.structure[8]  = {1, 0, 0, 0, 0, 0};                   // sphere3
    blobTree.structure[9]  = {0, OP_DIFFERENCE, 0, 0, 1, 13 - 9};  // Dif of = opNode4
    blobTree.structure[10] = {1, 0, 0, 0, 1, 12 - 10};             // cube4
    blobTree.structure[11] = {1, 0, 0, 0, 0, 0};                   // sphere4
    blobTree.structure[12] = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode5
    blobTree.structure[13] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode6
    blobTree.structure[14] = {0, OP_UNION, 0, 0, 1, 30 - 14};      // Union of = opNode7
    blobTree.structure[15] = {1, 0, 0, 0, 1, 17 - 15};             // cube5
    blobTree.structure[16] = {1, 0, 0, 0, 0, 0};                   // sphere5
    blobTree.structure[17] = {0, OP_DIFFERENCE, 0, 0, 1, 21 - 17}; // Dif of = opNode8
    blobTree.structure[18] = {1, 0, 0, 0, 1, 20 - 18};             // cube6
    blobTree.structure[19] = {1, 0, 0, 0, 0, 0};                   // sphere6
    blobTree.structure[20] = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode9
    blobTree.structure[21] = {0, OP_UNION, 0, 0, 1, 29 - 21};      // Union of = opNode10
    blobTree.structure[22] = {1, 0, 0, 0, 1, 24 - 22};             // cube7
    blobTree.structure[23] = {1, 0, 0, 0, 0, 0};                   // sphere7
    blobTree.structure[24] = {0, OP_DIFFERENCE, 0, 0, 1, 28 - 24}; // Dif of = opNode11
    blobTree.structure[25] = {1, 0, 0, 0, 1, 27 - 25};             // cube8
    blobTree.structure[26] = {1, 0, 0, 0, 0, 0};                   // sphere8
    blobTree.structure[27] = {0, OP_DIFFERENCE, 0, 0, 0, 0};       // Dif of = opNode12
    blobTree.structure[28] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode13
    blobTree.structure[29] = {0, OP_UNION, 0, 0, 0, 0};            // Union of = opNode14
    blobTree.structure[30] = {0, OP_UNION, 0, 0, 1, 32 - 30};      // Dif of = opNode15

    int leafCnt = 0;
    for (int i = 0; i < blobTree.structure.size(); ++i) {
        if (blobTree.structure[i].isPrimitive) { blobTree.primitiveNodeIdx[leafCnt++] = i; }
    }
    assert(leafCnt == PRIMITIVE_CNT);
    quadratureScene(primitiveDescriptions, sceneCenter - 0.5 * sceneSize - 0.2, sceneCenter + 0.5 * sceneSize + 0.2, blobTree);
}

void caseScene1()
{
    const int                                   PRIMITIVE_CNT = 2;
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions(PRIMITIVE_CNT);
    primitiveDescriptions[0] = std::make_shared<CuboidDesc>(CuboidDesc(0., 1.6));
    primitiveDescriptions[1] = std::make_shared<CuboidDesc>(CuboidDesc(uvector3(0.6, 0.6, -0.6), 2.));
    organizer::BlobTree blobTree;
    blobTree.structure.resize(PRIMITIVE_CNT * 2 - 1);
    blobTree.primitiveNodeIdx.resize(PRIMITIVE_CNT);
    // blobTree.structure[0]     = {0, 0, NODE_IN_OUT_UNKNOWN, 0, 1, 2}; // cube1
    blobTree.structure[0]     = {1, 0, NODE_IN_OUT_UNKNOWN, 0, 1, 2};               // cube1
    blobTree.structure[1]     = {1, 0, NODE_IN_OUT_UNKNOWN, 0, 0, 0};               // cube2
    blobTree.structure[2]     = {0, OP_INTERSECTION, NODE_IN_OUT_UNKNOWN, 0, 0, 0}; // INTERSECTION of cubes = opNode1
    blobTree.primitiveNodeIdx = {0, 1};
    quadratureScene(primitiveDescriptions, uvector3(-0.8, -1.3, -1.6), uvector3(1.6, 1.6, 1.5), blobTree);
}

void caseScene2()
{
    const int                                   PRIMITIVE_CNT = 4;
    std::vector<std::shared_ptr<PrimitiveDesc>> primitiveDescriptions(PRIMITIVE_CNT);
    primitiveDescriptions[0]                    = std::make_shared<CuboidDesc>(CuboidDesc(0., 1.6));
    primitiveDescriptions[1]                    = std::make_shared<CuboidDesc>(CuboidDesc(uvector3(0.6, 0.6, -0.6), 2.));
    primitiveDescriptions[2]                    = std::make_shared<SphereDesc>(SphereDesc(0.7, uvector3(0.8, 0.8, 0.8), 1.));
    primitiveDescriptions[3]                    = std::make_shared<SphereDesc>(SphereDesc(0.5, uvector3(-0.3, -0.8, 0.8), 1.));
    std::vector<uvector3> pyramidBottomVertices = {
        uvector3{-1, -1, 0},
        uvector3{1,  -1, 0},
        uvector3{1,  1,  0},
        uvector3{-1, 1,  0}
    };
    organizer::BlobTree blobTree;
    blobTree.structure.resize(PRIMITIVE_CNT * 2 - 1);
    blobTree.primitiveNodeIdx.resize(PRIMITIVE_CNT);
    // blobTree.structure[0]     = {0, 0, NODE_IN_OUT_UNKNOWN, 0, 1, 2}; // cube1
    blobTree.structure[0]     = {1, 0, NODE_IN_OUT_UNKNOWN, 0, 1, 2 - 0};        // cube1
    blobTree.structure[1]     = {1, 0, NODE_IN_OUT_UNKNOWN, 0, 0, 0};            // cube2
    blobTree.structure[2]     = {0, OP_UNION, NODE_IN_OUT_UNKNOWN, 0, 1, 6 - 2}; // INTERSECTION of cubes = opNode1
    blobTree.structure[3]     = {1, 0, 0, 0, 1, 5 - 3};                          // sphere1
    blobTree.structure[4]     = {1, 0, 0, 0, 0, 0};                              // sphere2
    blobTree.structure[5]     = {0, OP_UNION, 0, 0, 0, 0};                       // Union of spheres = opNode2
    blobTree.structure[6]     = {0, OP_DIFFERENCE, 0, 0, 1, 10 - 6};             // Difference of opNode1 and opNode2 = opNode3
    blobTree.primitiveNodeIdx = {0, 1, 3, 4};
    quadratureScene(primitiveDescriptions, uvector3(-0.8, -1.3, -1.6), uvector3(1.6, 1.6, 1.5), blobTree);
}

void testDeCasteljau()
{
    auto    phiDesc = std::make_shared<SphereDesc>(SphereDesc(0.8, uvector3(0), 1.));
    tensor3 tensor(nullptr, 3), tensor01(nullptr, 3), tensorBernstein(nullptr, 3), transformedTensorBernstein(nullptr, 3);
    algoim_spark_alloc(real, tensor, tensor01, tensorBernstein, transformedTensorBernstein);
    VisiblePrimitiveRep visiblePrimitiveRep{{tensor}, AABB{}, BlobTree()};
    makeSphere(*phiDesc, visiblePrimitiveRep);
    uvector3 xmax = 1, xmin = -1, range = xmax - xmin;
    organizer::detail::powerTransformation(range, xmin, tensor, tensor01);
    organizer::detail::power2BernsteinTensor(tensor01, tensorBernstein);

    // bernstein::deCasteljau(tensorBernstein, uvector3(0), uvector3(0.2690598923241497 + 0.0000001),
    // transformedTensorBernstein);
    bernstein::deCasteljau(tensorBernstein, uvector3(0.2690598923241497 + 0.0000001),
                           uvector3(1 - 0.2690598923241497 - 0.0000001), transformedTensorBernstein);
    uvector3 testX = uvector3(0.5);

    int  sign  = bernstein::uniformSign(transformedTensorBernstein);
    real evalX = evalPower(tensor, testX);

    std::cout << "evalX original rep = " << evalX << std::endl;
    evalX = bernstein::evalBernsteinPoly(tensorBernstein, testX);
    std::cout << "evalX bernstein within 0-1 = " << evalX << std::endl;
    evalX = bernstein::evalBernsteinPoly(transformedTensorBernstein, testX);
    std::cout << "evalX bernstein after Decasteljau = " << evalX << std::endl;
    std::cout << "sign = " << sign << std::endl;
}

void testSubDivideWithDeCasteljau()
{
    auto    phiDesc = std::make_shared<SphereDesc>(SphereDesc(0.7, uvector3(0.8), 1.));
    tensor3 tensor(nullptr, 3), tensor01(nullptr, 3), tensorBernstein(nullptr, 3), transformedTensorBernstein(nullptr, 3);
    algoim_spark_alloc(real, tensor, tensor01, tensorBernstein, transformedTensorBernstein);
    VisiblePrimitiveRep visiblePrimitiveRep{{tensor}, AABB{}, BlobTree()};
    makeSphere(*phiDesc, visiblePrimitiveRep);
    uvector3 xmin = uvector3(-1., -1.3, -1.6), xmax = uvector3(1.6, 1.6, 2.3), range = xmax - xmin;
    organizer::detail::powerTransformation(range, xmin, tensor, tensor01);
    organizer::detail::power2BernsteinTensor(tensor01, tensorBernstein);
    xmin = 0, xmax = 1;

    // bernstein::deCasteljau(tensorBernstein, uvector3(0), uvector3(0.2690598923241497 + 0.0000001),
    // transformedTensorBernstein);
    for (MultiLoop<3> j(0, 2); ~j; ++j) {
        auto aabbSub = organizer::detail::getOneEightCellAABB(AABB{xmin, xmax}, j(), 0);
        bernstein::deCasteljau(tensorBernstein, aabbSub.min, aabbSub.max, transformedTensorBernstein);
        auto transformedTensorVec = xarray2StdVector(transformedTensorBernstein);
        int  sign                 = bernstein::uniformSign(transformedTensorBernstein);
        std::cout << "j(): (" << j(0) << ", " << j(1) << ", " << j(2) << "), sign: " << sign << std::endl;
        if (all(j() == uvector3i(0, 0, 1))) {
            for (MultiLoop<3> k(0, 2); ~k; ++k) {
                auto aabbSubSub = organizer::detail::getOneEightCellAABB(aabbSub, k(), 0);
                bernstein::deCasteljau(tensorBernstein, aabbSubSub.min, aabbSubSub.max, transformedTensorBernstein);
                int sign = bernstein::uniformSign(transformedTensorBernstein);
                std::cout << "k(): (" << k(0) << ", " << k(1) << ", " << k(2) << "), sign: " << sign << std::endl;
            }
        }
    }

    uvector3 testX = uvector3(0.5);
    real     evalX = evalPower(tensor, testX);
    std::cout << "evalX original rep = " << evalX << std::endl;
    evalX = evalPower(tensor01, testX);
    std::cout << "evalX power within 0-1 = " << evalX << std::endl;
    evalX = bernstein::evalBernsteinPoly(tensorBernstein, testX);
    std::cout << "evalX bernstein within 0-1 = " << evalX << std::endl;

    // 切比雪夫
    xmin = uvector3(-1., -1.3, -1.6), xmax = uvector3(1.6, 1.6, 2.3);
    auto phi = [](const uvector<real, 3> &x) {
        return (x(0) - 0.8) * (x(0) - 0.8) + (x(1) - 0.8) * (x(1) - 0.8) + (x(2) - 0.8) * (x(2) - 0.8) - 0.1 * 0.1;
    };
    xarray<real, 3> phipoly(nullptr, 3), subPhiPoly(nullptr, 3);
    algoim_spark_alloc(real, phipoly, subPhiPoly);
    bernstein::bernsteinInterpolate<3>([&](const uvector<real, 3> &x) { return phi(xmin + x * (xmax - xmin)); }, phipoly);
    xmin = 0, xmax = 1;
    for (MultiLoop<3> j(0, 2); ~j; ++j) {
        auto aabbSub = organizer::detail::getOneEightCellAABB(AABB{xmin, xmax}, j(), 0);
        bernstein::deCasteljau(phipoly, aabbSub.min, aabbSub.max, subPhiPoly);
        int sign = bernstein::uniformSign(subPhiPoly);
        std::cout << "j(): (" << j(0) << ", " << j(1) << ", " << j(2) << "), sign: " << sign << std::endl;
    }
}

void testPlaneUniformSign()
{
    uvector3 xmin = -1, xmax = 1, scale = xmax - xmin;
    tensor3  planeTensor(nullptr, 2);
    algoim_spark_alloc(real, planeTensor);
    xarrayInit(planeTensor);
    uvector3 dim                     = 0;
    planeTensor.m(0)                 = -3.01;
    planeTensor.m(0)                 = -3.00001;
    planeTensor.m(0)                 = 3.00001;
    planeTensor.m(0)                 = 3.0000;
    planeTensor.m(uvector3{1, 0, 0}) = 1.0;
    planeTensor.m(uvector3{0, 1, 0}) = 1.0;
    planeTensor.m(uvector3{0, 0, 1}) = 1.0;
    organizer::detail::powerTransformation(scale, xmin, planeTensor);
    organizer::detail::power2BernsteinTensor(planeTensor);
    int sign = bernstein::uniformSign(planeTensor);
    std::cout << "sign = " << sign << std::endl;
}

void testPlaneWithinSubcell()
{
    AABB sceneAABB{uvector3(-1., -1.3, -1.6), uvector3(1.6, 1.6, 1.5)};
    AABB subCell{
        uvector3{0.076923076923076983, 0.17241379310344829, 0.77419354838709675},
        uvector3{0.076923076923077094, 0.1724137931034484,  0.77419354838709686}
    };
    tensor3 planeTensor(nullptr, 3), subPlaneTensor(nullptr, 3);
    algoim_spark_alloc(real, planeTensor, subPlaneTensor);
    xarrayInit(planeTensor);
    planeTensor.m(0)                 = -0.8;
    planeTensor.m(uvector3{0, 0, 1}) = 1;
    organizer::detail::powerTransformation(sceneAABB.size(), sceneAABB.min, planeTensor);
    organizer::detail::power2BernsteinTensor(planeTensor);
    bernstein::deCasteljau(planeTensor, subCell.min, subCell.max, subPlaneTensor);
    int sign = bernstein::uniformSign(subPlaneTensor);
    std::cout << sign << std::endl;
    ImplicitPolyQuadrature<3> ipquad(subPlaneTensor);
    real                      volume = 0;
    ipquad.integrate(AutoMixed, 10, [&](const uvector<real, 3> &x, real w) {
        if (evalBernstein(subPlaneTensor, x)) volume += w * 1;
    });
}

void testBlob()
{
    organizer::Blob blob = {5, 3, 4, 5, 6};

    // std::cout << blob.isPrimitive << std::endl;
    // std::cout << blob.nodeOp << std::endl;
    // std::cout << blob.ignoreMod << std::endl;
    // std::cout << blob.isLeft << std::endl;
    // std::cout << blob.ancestor << std::endl;
    std::cout << sizeof(blob) << std::endl;
    std::cout << sizeof(int) << std::endl;
}

void testTensorInverse()
{
    auto                            phiDesc = std::make_shared<CuboidDesc>(uvector3(0.2, 0.3, 0.5), uvector3(0.8, 1.1, 0.9));
    const int                       faceCnt = 6;
    std::vector<tensor3>            tensors(faceCnt, tensor3(nullptr, 2));
    std::vector<SparkStack<real> *> sparkStackPtrs(faceCnt);
    tensor3                         tensor(nullptr, 1 + faceCnt);
    algoimSparkAllocHeapVector(sparkStackPtrs, tensors);
    algoim_spark_alloc(real, tensor);
    VisiblePrimitiveRep visiblePrimitiveRep{tensors, AABB(), organizer::BlobTree()};
    organizer::detail::compositePower(visiblePrimitiveRep.tensors, 0, 0, 1, tensor);
    makeMesh(*phiDesc, visiblePrimitiveRep);
    uvector3 testX(0.5, 0.7, 0.2);
    real     evalX = evalPower(tensor, testX);
    std::cout << "before inverse, evalX = " << evalX << std::endl;
    inverseTensor(tensor);
    evalX = evalPower(tensor, testX);
    std::cout << "after inverse, evalX = " << evalX << std::endl;
    // free
    for (auto &ptr : sparkStackPtrs) delete ptr;
}

void testRotation()
{
    auto    desc = std::make_shared<SphereDesc>(SphereDesc(0.1, uvector3(0, 0., 0.)));
    tensor3 power(nullptr, 3);
    algoim_spark_alloc(real, power);
    VisiblePrimitiveRep visiblePrimitiveRep{{power}, AABB{}, BlobTree()};
    makeSphere(*desc, visiblePrimitiveRep);
    uvector3 testX(0.5, 0.7, 0.2);
    real     evalX = evalPower(power, testX);
    std::cout << "before rotation, evalX = " << evalX << std::endl;

    tensor3 rotatedPower(nullptr, sum(power.ext()));
    algoim_spark_alloc(real, rotatedPower);
    uvector3 axis = uvector3(4, 2, 3);
    axis          = axis / norm(axis);

    organizer::detail::powerRotation(axis, util::pi / 2, power, rotatedPower);
    evalX = evalPower(rotatedPower, testX);
    std::cout << "after rotation, evalX = " << evalX << std::endl;
    bool b = bernstein::autoReduction(rotatedPower, 1e5 * std::numeric_limits<real>::epsilon());
    std::cout << "auto reduction = " << b << std::endl;
    evalX = evalPower(rotatedPower, testX);
    std::cout << "after reduction, evalX = " << evalX << std::endl;
}

void testBoolVector(const std::vector<bool> &boolVec)
{
    for (auto b : boolVec) { std::cout << b << " "; }
}

void testPrimitive()
{
    // casePolyhedron2();
    // casePolyhedron3();
    // casePolyhedronSphere();
    // testSubDivideWithDeCasteljau();
    // testBlob();
    // testRotation();
    // caseScene();


    // caseScene30Objs();
    caseScene16Objs();

    // caseScene1();
    // caseScene2();
    // caseCone();
    // testPlaneWithinSubcell();
    // caseCubeSphere();
    // caseTwoCube();
    // testTensorInverse();
    // testPlaneUniformSign();

    // std::vector<bool> boolVec = {true, false, false, false, true, true, false};
    // testBoolVector(boolVec);
}