#include <polyfem/SplineBasis3d.hpp>

#include <polyfem/QuadraticBSpline3d.hpp>
#include <polyfem/HexQuadrature.hpp>

#include <polysolve/LinearSolver.hpp>
#include <polyfem/MeshNodes.hpp>
#include <polyfem/FEBasis3d.hpp>
#include <polyfem/Types.hpp>

#include <polyfem/Common.hpp>
#include <polyfem/auto_q_bases.hpp>

#include <Eigen/Sparse>

#include <cassert>
#include <iostream>
#include <vector>
#include <array>
#include <map>
#include <numeric>

//TODO carefull with simplices

namespace polyfem
{
    using namespace Eigen;
    using namespace polysolve;

    namespace
    {
        class SpaceMatrix
        {
        public:
            inline const int &operator()(const int i, const int j, const int k) const
            {
                return space_[k](i,j);
            }

            inline int &operator()(const int i, const int j, const int k)
            {
                return space_[k](i,j);
            }

            bool is_k_regular = false;
            int x,y,z;
            int edge_id;

            bool is_regular(const int xx, const int yy, const int zz) const
            {
                if(!is_k_regular) return true;

                return !((x == 1 && y == yy && z == zz) || (x == xx && y == 1 && z == zz) || (x == xx && y == yy && z == 1));
                // space(1, y, z).size() <= 1 && space(x, 1, z).size() <= 1 && space(x, y, 1).size() <= 1
            }

        private:
            std::array<Matrix<int, 3, 3> , 3> space_;
        };

        bool is_edge_singular(const Navigation3D::Index &index, const Mesh3D &mesh)
        {
            std::vector<int> ids;
            mesh.get_edge_elements_neighs(index.edge, ids);

            if(ids.size() == 4 || mesh.is_boundary_edge(index.edge))
                return false;

            for(auto idx : ids)
            {
                if(mesh.is_polytope(idx))
                    return false;
            }

            return true;
        }

        void print_local_space(const SpaceMatrix &space)
        {
            for(int k = 2; k >= 0; --k)
            {
                for(int j=2; j >=0; --j)
                {
                    for(int i=0; i < 3; ++i)
                    {
                        // if(space(i, j, k).size() > 0){
                            // for(std::size_t l = 0; l < space(i, j, k).size(); ++l)
                        std::cout<<space(i, j, k)<<"\t";
                        // }
                        // else
                            // std::cout<<"x\t";
                    }
                    std::cout<<std::endl;
                }

                std::cout<<"\n"<<std::endl;
            }
        }

        int node_id_from_face_index(const Mesh3D &mesh, MeshNodes &mesh_nodes, const Navigation3D::Index &index)
        {
            int el_id = mesh.switch_element(index).element;
            if(el_id >= 0 && mesh.is_cube(el_id))
            {
                return mesh_nodes.node_id_from_cell(el_id);
            }

            return mesh_nodes.node_id_from_face(index.face);
        }

        int node_id_from_edge_index(const Mesh3D &mesh, MeshNodes &mesh_nodes, const Navigation3D::Index &index)
        {
            Navigation3D::Index new_index = mesh.switch_element(index);
            int el_id = new_index.element;

            if(el_id < 0 || mesh.is_polytope(el_id))
            {
                new_index = mesh.switch_element(mesh.switch_face(index));
                el_id = new_index.element;

                if(el_id < 0 || mesh.is_polytope(el_id))
                {
                    return mesh_nodes.node_id_from_edge(index.edge);
                }

                return node_id_from_face_index(mesh, mesh_nodes, mesh.switch_face(new_index));
            }

            return node_id_from_face_index(mesh, mesh_nodes, mesh.switch_face(new_index));
        }

        int node_id_from_vertex_index_explore(const Mesh3D &mesh, const MeshNodes &mesh_nodes, const Navigation3D::Index &index, int &node_id)
        {
            Navigation3D::Index new_index = mesh.switch_element(index);

            int id = new_index.element;

            if(id < 0 || mesh.is_polytope(id))
            {
                // id = vertex_node_id(index.vertex);
                // node = node_from_vertex(index.vertex);
                node_id = mesh_nodes.primitive_from_vertex(index.vertex);
                return 3;
            }

            new_index = mesh.switch_element(mesh.switch_face(new_index));
            id = new_index.element;

            if(id < 0 || mesh.is_polytope(id))
            {
                // id = edge_node_id(switch_edge(new_index).edge);
                // node = node_from_edge(switch_edge(new_index).edge);
                node_id = mesh_nodes.primitive_from_edge(mesh.switch_edge(new_index).edge);
                return 2;
            }

            new_index = mesh.switch_element(mesh.switch_face(mesh.switch_edge(new_index)));
            id = new_index.element;

            if(id < 0 || mesh.is_polytope(id))
            {
                // id = face_node_id(new_index.face);
                // node = node_from_face(new_index.face);
                node_id = mesh_nodes.primitive_from_face(new_index.face);
                return 1;
            }

            // node = node_from_element(id);
            node_id = mesh_nodes.primitive_from_cell(id);
            return 0;
        }

        int node_id_from_vertex_index(const Mesh3D &mesh, MeshNodes &mesh_nodes, const Navigation3D::Index &index)
        {
            std::array<int, 6> path;
            std::array<int, 6> primitive_ids;

            path[0] = node_id_from_vertex_index_explore(mesh, mesh_nodes, index, primitive_ids[0]);
            path[1] = node_id_from_vertex_index_explore(mesh, mesh_nodes, mesh.switch_face(index), primitive_ids[1]);

            path[2] = node_id_from_vertex_index_explore(mesh, mesh_nodes, mesh.switch_edge(index), primitive_ids[2]);
            path[3] = node_id_from_vertex_index_explore(mesh, mesh_nodes, mesh.switch_face(mesh.switch_edge(index)), primitive_ids[3]);

            path[4] = node_id_from_vertex_index_explore(mesh, mesh_nodes, mesh.switch_edge(mesh.switch_face(index)), primitive_ids[4]);
            path[5] = node_id_from_vertex_index_explore(mesh, mesh_nodes, mesh.switch_face(mesh.switch_edge(mesh.switch_face(index))), primitive_ids[5]);

            const int min_path = *std::min_element(path.begin(), path.end());

            int primitive_id = 0;
            for(int i = 0 ; i < 6; ++i)
            {
                if(path[i]==min_path)
                {
                    primitive_id = primitive_ids[i];
                    break;
                }
            }

            return mesh_nodes.node_id_from_primitive(primitive_id);
        }

        void get_edge_elements_neighs(const Mesh3D &mesh, MeshNodes &mesh_nodes, const int element_id, const int edge_id, int dir, std::vector<int> &ids)
        {
            std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 12> to_edge;
            mesh.to_edge_functions(to_edge);

            Navigation3D::Index index;
            for(int i = 0; i < 12; ++i)
            {
                index = to_edge[i](mesh.get_index_from_element(element_id));

                if(index.edge == edge_id)
                    break;
            }

            assert(index.edge == edge_id);

            if(dir == 1)
            {
                int id;
                do
                {
                    ids.push_back(mesh_nodes.node_id_from_cell(index.element));
                    index = mesh.next_around_edge(index);
                }
                while(index.element != element_id);

                return;
            }

            if(dir == 0)
            {
                int id;
                do
                {
                    const Navigation3D::Index f_index = mesh.switch_face(mesh.switch_edge(index));
                    ids.push_back(node_id_from_face_index(mesh, mesh_nodes, f_index));

                    index = mesh.next_around_edge(index);
                }
                while(index.element != element_id);

                return;
            }

            if(dir == 2)
            {
                int id;
                do
                {
                    const Navigation3D::Index f_index = mesh.switch_face(mesh.switch_edge(mesh.switch_vertex(index)));
                    ids.push_back(node_id_from_face_index(mesh, mesh_nodes, f_index));

                    index = mesh.next_around_edge(index);
                }
                while(index.element != element_id);

                return;
            }

            assert(false);
        }

        void add_edge_id_for_poly(const Navigation3D::Index &index, const Mesh3D &mesh, MeshNodes &mesh_nodes, const int global_index, std::map<int, InterfaceData> &poly_face_to_data)
        {
           const int f1 = index.face;
           const int f2 = mesh.switch_face(index).face;

           const int id1 = mesh_nodes.primitive_from_face(f1);
           const int id2 = mesh_nodes.primitive_from_face(f2);

           if(mesh_nodes.is_primitive_interface(id1))
           {
            InterfaceData &data = poly_face_to_data[f1];
            data.local_indices.push_back(global_index);
        }

        if(mesh_nodes.is_primitive_interface(id2))
        {
            InterfaceData &data = poly_face_to_data[f2];
            data.local_indices.push_back(global_index);
        }
    }

    void add_vertex_id_for_poly(const Navigation3D::Index &index, const Mesh3D &mesh, MeshNodes &mesh_nodes, const int global_index, std::map<int, InterfaceData> &poly_face_to_data)
    {
       const int f1 = index.face;
       const int f2 = mesh.switch_face(index).face;
       const int f3 = mesh.switch_face(mesh.switch_edge(index)).face;

       const int id1 = mesh_nodes.primitive_from_face(f1);
       const int id2 = mesh_nodes.primitive_from_face(f2);
       const int id3 = mesh_nodes.primitive_from_face(f3);

       if(mesh_nodes.is_primitive_interface(id1))
       {
        InterfaceData &data = poly_face_to_data[f1];
        data.local_indices.push_back(global_index);
    }

    if(mesh_nodes.is_primitive_interface(id2))
    {
        InterfaceData &data = poly_face_to_data[f2];
        data.local_indices.push_back(global_index);
    }

    if(mesh_nodes.is_primitive_interface(id3))
    {
        InterfaceData &data = poly_face_to_data[f3];
        data.local_indices.push_back(global_index);
    }
}

void explore_edge(const Navigation3D::Index &index, const Mesh3D &mesh, MeshNodes &mesh_nodes, const int x, const int y, const int z, SpaceMatrix &space, LocalBoundary &local_boundary, std::map<int, InterfaceData> &poly_face_to_data)
{
    int node_id = node_id_from_edge_index(mesh, mesh_nodes, index);
    space(x, y, z) = node_id;
            // node(x, y, z) = mesh_nodes.node_position(node_id);

    if(is_edge_singular(index, mesh))
    {
        std::vector<int> ids;
        mesh.get_edge_elements_neighs(index.edge, ids);
                //irregular edge

        assert(!space.is_k_regular);
        space.is_k_regular = true;
        space.x = x; space.y = y; space.z = z;
        space.edge_id = index.edge;
    }

    // if(mesh_nodes.is_boundary(node_id))
        // bounday_nodes.push_back(node_id);

    add_edge_id_for_poly(index, mesh, mesh_nodes, 9*z + 3*y + x, poly_face_to_data);
}

void explore_vertex(const Navigation3D::Index &index, const Mesh3D &mesh, MeshNodes &mesh_nodes, const int x, const int y, const int z, SpaceMatrix &space, LocalBoundary &local_boundary, std::map<int, InterfaceData> &poly_face_to_data)
{
    int node_id = node_id_from_vertex_index(mesh, mesh_nodes, index);
    space(x, y, z) = node_id;
            // node(x, y, z) = mesh_nodes.node_position(node_id);

    // if(mesh_nodes.is_boundary(node_id))
        // bounday_nodes.push_back(node_id);

    add_vertex_id_for_poly(index, mesh, mesh_nodes, 9*z + 3*y + x, poly_face_to_data);
}

void explore_face(const Navigation3D::Index &index, const Mesh3D &mesh, MeshNodes &mesh_nodes, const int x, const int y, const int z,  SpaceMatrix &space, LocalBoundary &local_boundary, std::map<int, InterfaceData> &poly_face_to_data)
{
    int node_id = node_id_from_face_index(mesh, mesh_nodes, index);
    space(x, y, z) = node_id;
            // node(x, y, z) = mesh_nodes.node_position(node_id);

    if(mesh_nodes.is_boundary(node_id))
    {
        // local_boundary.add_boundary_primitive(index.face, FEBasis3d::quadr_hex_face_local_nodes(mesh, index)[8]-20);
        local_boundary.add_boundary_primitive(index.face, FEBasis3d::hex_face_local_nodes(false, 2, mesh, index)[8]-20);
        // bounday_nodes.push_back(node_id);
    }
    else if(mesh_nodes.is_interface(node_id))
    {
        InterfaceData &data = poly_face_to_data[index.face];
        data.local_indices.push_back(9*z + 3*y + x);
                // igl::viewer::Viewer &viewer = UIState::ui_state().viewer;
                // viewer.data.add_points(mesh_nodes.node_position(node_id), Eigen::MatrixXd::Constant(1, 3, 0));
    }
}

void build_local_space(const Mesh3D &mesh,  MeshNodes &mesh_nodes, const int el_index,  SpaceMatrix &space, std::vector<LocalBoundary> &local_boundary, std::map<int, InterfaceData> &poly_face_to_data)
{
    assert(mesh.is_volume());

    Navigation3D::Index start_index = mesh.get_index_from_element(el_index);
    Navigation3D::Index index;

    std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 6> to_face;
    mesh.to_face_functions(to_face);

    std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 12> to_edge;
    mesh.to_edge_functions(to_edge);

    std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 8> to_vertex;
    mesh.to_vertex_functions(to_vertex);

    const int node_id = mesh_nodes.node_id_from_cell(el_index);
    space(1, 1, 1) = node_id;
            // node(1, 1, 1) = mesh_nodes.node_position(node_id);

    LocalBoundary lb(el_index, BoundaryType::Quad);

            ///////////////////////
    index = to_face[1](start_index);
    explore_face(index, mesh, mesh_nodes, 1, 1, 0, space, lb, poly_face_to_data);

    index = to_face[0](start_index);
    explore_face(index, mesh, mesh_nodes, 1, 1, 2, space, lb, poly_face_to_data);

    index = to_face[3](start_index);
    explore_face(index, mesh, mesh_nodes, 0, 1, 1, space, lb, poly_face_to_data);

    index = to_face[2](start_index);
    explore_face(index, mesh, mesh_nodes, 2, 1, 1, space, lb, poly_face_to_data);

    index = to_face[5](start_index);
    explore_face(index, mesh, mesh_nodes, 1, 0, 1, space, lb, poly_face_to_data);

    index = to_face[4](start_index);
    explore_face(index, mesh, mesh_nodes, 1, 2, 1, space, lb, poly_face_to_data);

            ///////////////////////
    index = to_edge[0](start_index);
    explore_edge(index, mesh, mesh_nodes, 1, 0, 0, space, lb, poly_face_to_data);

    index = to_edge[1](start_index);
    explore_edge(index, mesh, mesh_nodes, 2, 1, 0, space, lb, poly_face_to_data);

    index = to_edge[2](start_index);
    explore_edge(index, mesh, mesh_nodes, 1, 2, 0, space, lb, poly_face_to_data);

    index = to_edge[3](start_index);
    explore_edge(index, mesh, mesh_nodes, 0, 1, 0, space, lb, poly_face_to_data);

    index = to_edge[4](start_index);
    explore_edge(index, mesh, mesh_nodes, 0, 0, 1, space, lb, poly_face_to_data);

    index = to_edge[5](start_index);
    explore_edge(index, mesh, mesh_nodes, 2, 0, 1, space, lb, poly_face_to_data);

    index = to_edge[6](start_index);
    explore_edge(index, mesh, mesh_nodes, 2, 2, 1, space, lb, poly_face_to_data);

    index = to_edge[7](start_index);
    explore_edge(index, mesh, mesh_nodes, 0, 2, 1, space, lb, poly_face_to_data);

    index = to_edge[8](start_index);
    explore_edge(index, mesh, mesh_nodes, 1, 0, 2, space, lb, poly_face_to_data);

    index = to_edge[9](start_index);
    explore_edge(index, mesh, mesh_nodes, 2, 1, 2, space, lb, poly_face_to_data);

    index = to_edge[10](start_index);
    explore_edge(index, mesh, mesh_nodes, 1, 2, 2, space, lb, poly_face_to_data);

    index = to_edge[11](start_index);
    explore_edge(index, mesh, mesh_nodes, 0, 1, 2, space, lb, poly_face_to_data);

            ////////////////////////////////////////////////////////////////////////
    index = to_vertex[0](start_index);
    explore_vertex(index, mesh, mesh_nodes, 0, 0, 0, space, lb, poly_face_to_data);

    index = to_vertex[1](start_index);
    explore_vertex(index, mesh, mesh_nodes, 2, 0, 0, space, lb, poly_face_to_data);

    index = to_vertex[2](start_index);
    explore_vertex(index, mesh, mesh_nodes, 2, 2, 0, space, lb, poly_face_to_data);

    index = to_vertex[3](start_index);
    explore_vertex(index, mesh, mesh_nodes, 0, 2, 0, space, lb, poly_face_to_data);

    index = to_vertex[4](start_index);
    explore_vertex(index, mesh, mesh_nodes, 0, 0, 2, space, lb, poly_face_to_data);

    index = to_vertex[5](start_index);
    explore_vertex(index, mesh, mesh_nodes, 2, 0, 2, space, lb, poly_face_to_data);

    index = to_vertex[6](start_index);
    explore_vertex(index, mesh, mesh_nodes, 2, 2, 2, space, lb, poly_face_to_data);

    index = to_vertex[7](start_index);
    explore_vertex(index, mesh, mesh_nodes, 0, 2, 2, space, lb, poly_face_to_data);

    if(!lb.empty())
        local_boundary.emplace_back(lb);
}

void setup_knots_vectors(MeshNodes &mesh_nodes, const SpaceMatrix &space, std::array<std::array<double, 4>, 3> &h_knots, std::array<std::array<double, 4>, 3> &v_knots, std::array<std::array<double, 4>, 3> &w_knots)
{
            //left and right neigh are absent
    if(mesh_nodes.is_boundary_or_interface(space(0, 1, 1)) && mesh_nodes.is_boundary_or_interface(space(2, 1, 1)))
    {
        h_knots[0] = {{0, 0, 0, 1}};
        h_knots[1] = {{0, 0, 1, 1}};
        h_knots[2] = {{0, 1, 1, 1}};
    }
             //left neigh is absent
    else if(mesh_nodes.is_boundary_or_interface(space(0, 1, 1)))
    {
        h_knots[0] = {{0, 0, 0, 1}};
        h_knots[1] = {{0, 0, 1, 2}};
        h_knots[2] = {{0, 1, 2, 3}};
    }
            //right neigh is absent
    else if(mesh_nodes.is_boundary_or_interface(space(2,1,1)))
    {
        h_knots[0] = {{-2, -1, 0, 1}};
        h_knots[1] = {{-1, 0, 1, 1}};
        h_knots[2] = {{0, 1, 1, 1}};
    }
    else
    {
        h_knots[0] = {{-2, -1, 0, 1}};
        h_knots[1] = {{-1, 0, 1, 2}};
        h_knots[2] = {{0, 1, 2, 3}};
    }

            //top and bottom neigh are absent
    if(mesh_nodes.is_boundary_or_interface(space(1,0,1)) && mesh_nodes.is_boundary_or_interface(space(1,2,1)))
    {
        v_knots[0] = {{0, 0, 0, 1}};
        v_knots[1] = {{0, 0, 1, 1}};
        v_knots[2] = {{0, 1, 1, 1}};
    }
            //bottom neigh is absent
    else if(mesh_nodes.is_boundary_or_interface(space(1,0,1)))
    {
        v_knots[0] = {{0, 0, 0, 1}};
        v_knots[1] = {{0, 0, 1, 2}};
        v_knots[2] = {{0, 1, 2, 3}};
    }
            //top neigh is absent
    else if(mesh_nodes.is_boundary_or_interface(space(1,2,1)))
    {
        v_knots[0] = {{-2, -1, 0, 1}};
        v_knots[1] = {{-1, 0, 1, 1}};
        v_knots[2] = {{0, 1, 1, 1}};
    }
    else
    {
        v_knots[0] = {{-2, -1, 0, 1}};
        v_knots[1] = {{-1, 0, 1, 2}};
        v_knots[2] = {{0, 1, 2, 3}};
    }

            //front and back neigh are absent
    if(mesh_nodes.is_boundary_or_interface(space(1,1,0)) && mesh_nodes.is_boundary_or_interface(space(1,1,2)))
    {
        w_knots[0] = {{0, 0, 0, 1}};
        w_knots[1] = {{0, 0, 1, 1}};
        w_knots[2] = {{0, 1, 1, 1}};
    }
            //back neigh is absent
    else if(mesh_nodes.is_boundary_or_interface(space(1,1,0)))
    {
        w_knots[0] = {{0, 0, 0, 1}};
        w_knots[1] = {{0, 0, 1, 2}};
        w_knots[2] = {{0, 1, 2, 3}};
    }
            //front neigh is absent
    else if(mesh_nodes.is_boundary_or_interface(space(1,1,2)))
    {
        w_knots[0] = {{-2, -1, 0, 1}};
        w_knots[1] = {{-1, 0, 1, 1}};
        w_knots[2] = {{0, 1, 1, 1}};
    }
    else
    {
        w_knots[0] = {{-2, -1, 0, 1}};
        w_knots[1] = {{-1, 0, 1, 2}};
        w_knots[2] = {{0, 1, 2, 3}};
    }
}

void basis_for_regular_hex(MeshNodes &mesh_nodes, const SpaceMatrix &space, const std::array<std::array<double, 4>, 3> &h_knots, const std::array<std::array<double, 4>, 3> &v_knots, const std::array<std::array<double, 4>, 3> &w_knots, ElementBases &b)
{
    for(int z = 0; z < 3; ++z)
    {
        for(int y = 0; y < 3; ++y)
        {
            for(int x = 0; x < 3; ++x)
            {
                        if(space.is_regular(x,y,z)) //space(1, y, z).size() <= 1 && space(x, 1, z).size() <= 1 && space(x, y, 1).size() <= 1)
                        {
                            const int local_index = 9*z + 3*y + x;
                            const int global_index = space(x, y, z);
                            const auto node = mesh_nodes.node_position(global_index);
                            // loc_nodes(x, y, z);

                            b.bases[local_index].init(2, global_index, local_index, node);

                            const QuadraticBSpline3d spline(h_knots[x], v_knots[y], w_knots[z]);

                            b.bases[local_index].set_basis([spline](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { spline.interpolate(uv, val); });
                            b.bases[local_index].set_grad( [spline](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { spline.derivative(uv, val); });
                        }
                    }
                }
            }
        }

        void basis_for_irregulard_hex(const int el_index, const Mesh3D &mesh, MeshNodes &mesh_nodes, const SpaceMatrix &space, const std::array<std::array<double, 4>, 3> &h_knots, const std::array<std::array<double, 4>, 3> &v_knots, const std::array<std::array<double, 4>, 3> &w_knots, ElementBases &b, std::map<int, InterfaceData> &poly_face_to_data)
        {
            for(int z = 0; z < 3; ++z)
            {
                for(int y = 0; y < 3; ++y)
                {
                    for(int x = 0; x < 3; ++x)
                    {
                        if(!space.is_regular(x,y,z)) //space(1, y, z).size() > 1 || space(x, 1, z).size() > 1 || space(x, y, 1).size() > 1)
                        {
                            const int local_index = 9*z + 3*y + x;

                            int mpx = -1;
                            int mpy = -1;
                            int mpz = -1;

                            int mmx = -1;
                            int mmy = -1;
                            int mmz = -1;

                            int xx = 1;
                            int yy = 1;
                            int zz = 1;

                            const int edge_id = space.edge_id;
                            int dir = -1;

                            if(space.x == x && space.y == y && space.z == 1)
                            {
                                mpx = 1;
                                mpy = y;
                                mpz = z;

                                mmx = x;
                                mmy = 1;
                                mmz = z;

                                zz = z;
                                dir = z;
                            }
                            else if(space.x == x && space.y == 1 && space.z == z)
                            {
                                mpx = 1;
                                mpy = y;
                                mpz = z;

                                mmx = x;
                                mmy = y;
                                mmz = 1;

                                yy = y;
                                dir = y;
                            }
                            else if(space.x == 1 && space.y == y && space.z == z)
                            {
                                mpx = x;
                                mpy = y;
                                mpz = 1;

                                mmx = x;
                                mmy = 1;
                                mmz = z;

                                xx = x;
                                dir = x;
                            }
                            else
                                assert(false);

                            const auto &center = b.bases[zz*9 + yy*3 + xx].global().front();

                            const auto &el1 = b.bases[mpz*9 + mpy*3 + mpx].global().front();
                            const auto &el2 = b.bases[mmz*9 + mmy*3 + mmx].global().front();

                            // std::cout<<"el_index "<<el_index<<std::endl;
                            // std::cout<<"center "<<center.index<<std::endl;
                            // std::cout<<"el1 "<<el1.index<<std::endl;
                            // std::cout<<"el2 "<<el2.index<<std::endl;

                            std::vector<int> ids;
                            get_edge_elements_neighs(mesh, mesh_nodes, el_index, edge_id, dir, ids);

                            if(ids.front() != center.index)
                            {
                                assert(dir != 1);
                                ids.clear();
                                get_edge_elements_neighs(mesh, mesh_nodes, el_index, edge_id, dir == 2 ? 0 : 2, ids);
                            }

                            assert(ids.front() == center.index);

                            std::vector<int> other_indices;
                            std::vector<Eigen::MatrixXd> other_nodes;
                            for(size_t i = 0; i < ids.size(); ++i)
                            {
                                const int node_id = ids[i];
                                if(node_id != center.index && node_id != el1.index && node_id != el2.index){
                                    other_indices.push_back(node_id);
                                }
                            }

                            // std::cout<<ids.size()<< " " << other_indices.size()<<std::endl;

                            auto &base = b.bases[local_index];

                            const int k = int(other_indices.size()) + 3;

                            // const bool is_interface = mesh_nodes.is_interface(center.index);
                            // const int face_id = is_interface ? mesh_nodes.face_from_node_id(center.index) : -1;

                            base.global().resize(k);

                            base.global()[0].index = center.index;
                            base.global()[0].val = (4. - k) / k;
                            base.global()[0].node = center.node;

                            base.global()[1].index = el1.index;
                            base.global()[1].val = (4. - k) / k;
                            base.global()[1].node = el1.node;

                            base.global()[2].index = el2.index;
                            base.global()[2].val = (4. - k) / k;
                            base.global()[2].node = el2.node;

                            // if(is_interface){
                                // poly_face_to_data[face_id].local_indices.push_back(local_index);
                            // }

                            for(std::size_t n = 0; n < other_indices.size(); ++n)
                            {
                                base.global()[3+n].index = other_indices[n];
                                base.global()[3+n].val = 4./k;
                                base.global()[3+n].node = mesh_nodes.node_position(other_indices[n]);
                            }

                            const QuadraticBSpline3d spline(h_knots[x], v_knots[y], w_knots[z]);

                            b.bases[local_index].set_basis([spline](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { spline.interpolate(uv, val); });
                            b.bases[local_index].set_grad( [spline](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { spline.derivative(uv, val); });
                        }
                    }
                }
            }
        }

        void create_q2_nodes(const Mesh3D &mesh, const int el_index, std::set<int> &vertex_id, std::set<int> &edge_id, std::set<int> &face_id, ElementBases &b, std::vector<LocalBoundary> &local_boundary, int &n_bases)
        {
            b.bases.resize(27);

            std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 6> to_face;
            mesh.to_face_functions(to_face);

            std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 12> to_edge;
            mesh.to_edge_functions(to_edge);

            std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 8> to_vertex;
            mesh.to_vertex_functions(to_vertex);

            LocalBoundary lb(el_index, BoundaryType::Quad);

            const Navigation3D::Index start_index = mesh.get_index_from_element(el_index);
            for (int j = 0; j < 8; ++j)
            {
                const Navigation3D::Index index = to_vertex[j](start_index);
                // const int loc_index = FEBasis3d::quadr_hex_face_local_nodes(mesh, index)[0];
                const int loc_index = FEBasis3d::hex_face_local_nodes(false, 2, mesh, index)[0];

                int current_vertex_node_id = -1;
                Eigen::MatrixXd current_vertex_node;

                //if the edge/vertex is boundary the it is a Q2 edge
                bool is_vertex_q2 = true;

                std::vector<int> vertex_neighs;
                mesh.get_vertex_elements_neighs(index.vertex, vertex_neighs);

                for(size_t i = 0; i < vertex_neighs.size(); ++i)
                {
                    if(mesh.is_spline_compatible(vertex_neighs[i]))
                    {
                        is_vertex_q2 = false;
                        break;
                    }
                }
                const bool is_vertex_boundary = mesh.is_boundary_vertex(index.vertex);

                if(is_vertex_q2)
                {
                    const bool is_new_vertex = vertex_id.insert(index.vertex).second;

                    if(is_new_vertex)
                    {
                        current_vertex_node_id = n_bases++;
                        current_vertex_node = mesh.point(index.vertex);

                        // if(is_vertex_boundary)//mesh.is_vertex_boundary(index.vertex))
                            // bounday_nodes.push_back(current_vertex_node_id);
                    }
                }

                //init new Q2 nodes
                if(current_vertex_node_id >= 0)
                    b.bases[loc_index].init(2, current_vertex_node_id, loc_index, current_vertex_node);

                b.bases[loc_index].set_basis([loc_index](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_basis_value_3d     (2, loc_index, uv, val); });
                b.bases[loc_index].set_grad( [loc_index](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_grad_basis_value_3d(2, loc_index, uv, val); });
            }

            for (int j = 0; j < 12; ++j)
            {
                Navigation3D::Index index = to_edge[j](start_index);

                int current_edge_node_id = -1;
                Eigen::Matrix<double, 1, 3> current_edge_node;
                // const int loc_index = FEBasis3d::quadr_hex_face_local_nodes(mesh, index)[1];
                const int loc_index = FEBasis3d::hex_face_local_nodes(false, 2, mesh, index)[4];

                bool is_edge_q2 = true;

                std::vector<int> edge_neighs;
                mesh.get_edge_elements_neighs(index.edge, edge_neighs);

                for(size_t i = 0; i < edge_neighs.size(); ++i)
                {
                    if(mesh.is_spline_compatible(edge_neighs[i]))
                    {
                        is_edge_q2 = false;
                        break;
                    }
                }
                const bool is_edge_boundary = mesh.is_boundary_edge(index.edge);

                if(is_edge_q2)
                {
                    const bool is_new_edge = edge_id.insert(index.edge).second;

                    if(is_new_edge)
                    {
                        current_edge_node_id = n_bases++;
                        current_edge_node = mesh.edge_barycenter(index.edge);

                        // if(is_edge_boundary)
                            // bounday_nodes.push_back(current_edge_node_id);
                    }
                }

                //init new Q2 nodes
                if(current_edge_node_id >= 0)
                    b.bases[loc_index].init(2, current_edge_node_id, loc_index, current_edge_node);

                b.bases[loc_index].set_basis([loc_index](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_basis_value_3d     (2, loc_index, uv, val); });
                b.bases[loc_index].set_grad( [loc_index](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_grad_basis_value_3d(2, loc_index, uv, val); });
            }

            for (int j = 0; j < 6; ++j)
            {
                Navigation3D::Index index = to_face[j](start_index);

                int current_face_node_id = -1;

                Eigen::Matrix<double, 1, 3> current_face_node;
                const int opposite_element = mesh.switch_element(index).element;
                const bool is_face_q2 = opposite_element < 0 || !mesh.is_spline_compatible(opposite_element);
                // const int loc_index = FEBasis3d::quadr_hex_face_local_nodes(mesh, index)[8];
                const int loc_index = FEBasis3d::hex_face_local_nodes(false, 2, mesh, index)[8];

                if (is_face_q2)
                {
                    const bool is_new_face = face_id.insert(index.face).second;

                    if(is_new_face)
                    {
                        current_face_node_id = n_bases++;
                        current_face_node = mesh.face_barycenter(index.face);

                        const int b_index = loc_index - 20;

                        if(opposite_element < 0) // && mesh.n_element_faces(opposite_element) == 6 && mesh.n_element_vertices(opposite_element) == 8)
                        {
                            // bounday_nodes.push_back(current_face_node_id);
                            lb.add_boundary_primitive(index.face, b_index);
                        }
                    }
                }

                //init new Q2 nodes
                if(current_face_node_id >= 0)
                    b.bases[loc_index].init(2, current_face_node_id, loc_index, current_face_node);

                b.bases[loc_index].set_basis([loc_index](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_basis_value_3d     (2, loc_index, uv, val); });
                b.bases[loc_index].set_grad( [loc_index](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_grad_basis_value_3d(2, loc_index, uv, val); });
            }

            // //central node always present
            b.bases[26].init(2, n_bases++, 26, mesh.cell_barycenter(el_index));
            b.bases[26].set_basis([](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_basis_value_3d     (2, 26, uv, val); });
            b.bases[26].set_grad( [](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { autogen::q_grad_basis_value_3d(2, 26, uv, val); });

            if(!lb.empty())
                local_boundary.emplace_back(lb);
        }

        void insert_into_global(const int el_index, const Local2Global &data, std::vector<Local2Global> &vec, const int size)
        {
            //ignore small weights
            if(fabs(data.val) <1e-10 )
                return;

            bool found = false;

            for(int i = 0; i < size; ++i)
            {
                if(vec[i].index == data.index)
                {
                    // if(fabs(vec[i].val - data.val) > 1e-10){
                    //     std::cout<<el_index <<" "<<vec[i].val <<" "<< data.val<<" "<<fabs(vec[i].val - data.val)<<std::endl;
                    //     // vec[i].val += data.val;
                    // }
                    // assert(fabs(vec[i].val - data.val) < 1e-10);
                    assert((vec[i].node - data.node).norm() < 1e-10);
                    found = true;
                    break;
                }
            }

            if(!found)
                vec.push_back(data);
        }

        void assign_q2_weights(const Mesh3D &mesh, const int el_index, std::vector< ElementBases > &bases)
        {
            // Eigen::MatrixXd eval_p;
            std::vector<AssemblyValues> eval_p;
            const Navigation3D::Index start_index = mesh.get_index_from_element(el_index);
            ElementBases &b = bases[el_index];

            std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 6> to_face;
            mesh.to_face_functions(to_face);
            for (int f = 0; f < 6; ++f)
            {
                const Navigation3D::Index index = to_face[f](start_index);
                const int opposite_element = mesh.switch_element(index).element;

                if(opposite_element < 0 || !mesh.is_cube(opposite_element))
                    continue;

                // const auto &param_p     = FEBasis3d::quadr_hex_face_local_nodes_coordinates(mesh, mesh.switch_element(index));
                Eigen::Matrix<double, 9, 3> param_p;
                {
                    Eigen::MatrixXd hex_loc_nodes; polyfem::autogen::q_nodes_3d(2, hex_loc_nodes);
                    const auto opposite_indices = FEBasis3d::hex_face_local_nodes(false, 2, mesh, mesh.switch_element(index));
                    for(int k = 0; k < 9; ++k)
                        param_p.row(k) = hex_loc_nodes.row(opposite_indices[k]);
                }
                const auto &other_bases = bases[opposite_element];

                // const auto &indices     = FEBasis3d::quadr_hex_face_local_nodes(mesh, index);
                const auto &indices     = FEBasis3d::hex_face_local_nodes(false, 2, mesh, index);

                std::array<int, 9> sizes;

                for(int l = 0; l < 9; ++l)
                    sizes[l] = b.bases[indices[l]].global().size();

                other_bases.evaluate_bases(param_p, eval_p);
                for(std::size_t i = 0; i < other_bases.bases.size(); ++i)
                {
                    const auto &other_b = other_bases.bases[i];

                    if(other_b.global().empty()) continue;

                    // other_b.basis(param_p, eval_p);
                    assert(eval_p[i].val.size() == 9);

                    //basis i of element opposite element is zero on this elements
                    if(eval_p[i].val.cwiseAbs().maxCoeff() <= 1e-10)
                        continue;

                    for(std::size_t k = 0; k < other_b.global().size(); ++k)
                    {
                        for(int l = 0; l < 9; ++l)
                        {
                            Local2Global glob = other_b.global()[k];
                            glob.val *= eval_p[i].val(l);

                            insert_into_global(el_index, glob, b.bases[indices[l]].global(), sizes[l]);
                        }
                    }
                }
            }
        }

        void setup_data_for_polygons(const Mesh3D &mesh, const int el_index, const ElementBases &b, std::map<int, InterfaceData> &poly_face_to_data)
        {
            const Navigation3D::Index start_index = mesh.get_index_from_element(el_index);
            std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 6> to_face;
            mesh.to_face_functions(to_face);
            for (int f = 0; f < 6; ++f)
            {
                const Navigation3D::Index index = to_face[f](start_index);

                const int opposite_element = mesh.switch_element(index).element;
                const bool is_neigh_poly = opposite_element >= 0 && mesh.is_polytope(opposite_element);

                if(is_neigh_poly)
                {
                    // auto e2l = FEBasis3d::quadr_hex_face_local_nodes(mesh, index);
                    auto e2l = FEBasis3d::hex_face_local_nodes(false, 2, mesh, index);

                    InterfaceData &data = poly_face_to_data[index.face];

                    for(int kk = 0; kk < e2l.size(); ++kk)
                    {
                        const auto idx = e2l(kk);
                        data.local_indices.push_back(idx);
                    }
                }
            }
        }
    }

    int SplineBasis3d::build_bases(const Mesh3D &mesh, const int quadrature_order, std::vector< ElementBases > &bases, std::vector< LocalBoundary > &local_boundary, std::map<int, InterfaceData> &poly_face_to_data)
    {
        using std::max;
        assert(mesh.is_volume());

        MeshNodes mesh_nodes(mesh, true, true, 1, 1, 1);

        const int n_els = mesh.n_elements();
        bases.resize(n_els);
        local_boundary.clear();

        // bounday_nodes.clear();

        // HexQuadrature hex_quadrature;

        std::array<std::array<double, 4>, 3> h_knots;
        std::array<std::array<double, 4>, 3> v_knots;
        std::array<std::array<double, 4>, 3> w_knots;

        for(int e = 0; e < n_els; ++e)
        {
            if(!mesh.is_spline_compatible(e))
                continue;

            SpaceMatrix space;

            build_local_space(mesh, mesh_nodes, e, space, local_boundary, poly_face_to_data);

            ElementBases &b=bases[e];
            b.set_quadrature([quadrature_order](Quadrature &quad){
                HexQuadrature hex_quadrature;
                hex_quadrature.get_quadrature(quadrature_order, quad);
            });
            // hex_quadrature.get_quadrature(quadrature_order, b.quadrature);
            b.bases.resize(27);

            b.set_local_node_from_primitive_func([e](const int primitive_id, const Mesh &mesh)
            {
                const auto &mesh3d = dynamic_cast<const Mesh3D &>(mesh);

                std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 6> to_face;
                mesh3d.to_face_functions(to_face);

                auto start_index = mesh3d.get_index_from_element(e);
                auto index = start_index;

                int lf;
                for(lf = 0; lf < mesh3d.n_cell_faces(e); ++lf)
                {
                    index = to_face[lf](start_index);
                    if(index.face == primitive_id)
                        break;
                }
                assert(index.face == primitive_id);

                static constexpr std::array<std::array<int, 9>, 6> face_to_index = {{
                    {{2*9+0*3+0, 2*9+1*3+0, 2*9+2*3+0, 2*9+0*3+1, 2*9+1*3+1, 2*9+2*3+1, 2*9+0*3+2, 2*9+1*3+2, 2*9+2*3+2}}, //0
                    {{0*9+0*3+0, 0*9+1*3+0, 0*9+2*3+0, 0*9+0*3+1, 0*9+1*3+1, 0*9+2*3+1, 0*9+0*3+2, 0*9+1*3+2, 0*9+2*3+2}}, //1

                    {{0*9+0*3+2, 0*9+1*3+2, 0*9+2*3+2, 1*9+0*3+2, 1*9+1*3+2, 1*9+2*3+2, 2*9+0*3+2, 2*9+1*3+2, 2*9+2*3+2}}, //2
                    {{0*9+0*3+0, 0*9+1*3+0, 0*9+2*3+0, 1*9+0*3+0, 1*9+1*3+0, 1*9+2*3+0, 2*9+0*3+0, 2*9+1*3+0, 2*9+2*3+0}}, //3

                    {{0*9+2*3+0, 0*9+2*3+1, 0*9+2*3+2, 1*9+2*3+0, 1*9+2*3+1, 1*9+2*3+2, 2*9+2*3+0, 2*9+2*3+1, 2*9+2*3+2}}, //4
                    {{0*9+0*3+0, 0*9+0*3+1, 0*9+0*3+2, 1*9+0*3+0, 1*9+0*3+1, 1*9+0*3+2, 2*9+0*3+0, 2*9+0*3+1, 2*9+0*3+2}}, //5
                }};

                Eigen::VectorXi res(9);

                for(int i = 0; i< 9; ++i)
                    res(i)=face_to_index[lf][i];

                return res;
            });

            setup_knots_vectors(mesh_nodes, space, h_knots, v_knots, w_knots);
            // print_local_space(space);

            basis_for_regular_hex(mesh_nodes, space, h_knots, v_knots, w_knots, b);
            basis_for_irregulard_hex(e, mesh, mesh_nodes, space, h_knots, v_knots, w_knots, b, poly_face_to_data);
        }

        int n_bases = mesh_nodes.n_nodes();

        std::set<int> face_id;
        std::set<int> edge_id;
        std::set<int> vertex_id;

        for(int e = 0; e < n_els; ++e)
        {
            if(mesh.is_polytope(e) || mesh.is_spline_compatible(e))
                continue;

            ElementBases &b=bases[e];
            // hex_quadrature.get_quadrature(quadrature_order, b.quadrature);
            b.set_quadrature([quadrature_order](Quadrature &quad){
                HexQuadrature hex_quadrature;
                hex_quadrature.get_quadrature(quadrature_order, quad);
            });

            b.set_local_node_from_primitive_func([e](const int primitive_id, const Mesh &mesh)
            {
                const auto &mesh3d = dynamic_cast<const Mesh3D &>(mesh);
                Navigation3D::Index index;

                for(int lf = 0; lf < 6; ++lf)
                {
                    index = mesh3d.get_index_from_element(e, lf, 0);
                    if(index.face == primitive_id)
                        break;
                }
                assert(index.face == primitive_id);

                // const auto indices = FEBasis3d::quadr_hex_face_local_nodes(mesh3d, index);
                const auto indices = FEBasis3d::hex_face_local_nodes(false, 2, mesh3d, index);
                Eigen::VectorXi res(indices.size());

                for(size_t i = 0; i< indices.size(); ++i)
                    res(i)=indices[i];

                return res;
            });

            create_q2_nodes(mesh, e, vertex_id, edge_id, face_id, b, local_boundary, n_bases);
        }

        bool missing_bases = false;
        do
        {
            missing_bases = false;
            for(int e = 0; e < n_els; ++e)
            {
                if(mesh.is_polytope(e) || mesh.is_spline_compatible(e))
                    continue;

                auto &b=bases[e];
                if(b.is_complete())
                    continue;

                assign_q2_weights(mesh, e, bases);

                missing_bases = missing_bases || b.is_complete();
            }
        }
        while(missing_bases);

        for(int e = 0; e < n_els; ++e)
        {
            if(mesh.is_polytope(e) || mesh.is_spline_compatible(e))
                continue;

            const ElementBases &b=bases[e];
            setup_data_for_polygons(mesh, e, b, poly_face_to_data);
        }

        // for(int e = 0; e < n_els; ++e)
        // {
        //     if(!mesh.is_polytope(e))
        //         continue;

        //     for (int lf = 0; lf < mesh.n_cell_faces(e); ++lf)
        //     {
        //         auto index = mesh.get_index_from_element(e, lf, 0);
        //         auto index2 = mesh.switch_element(index);
        //         if (index2.element >= 0) {
        //             auto &array = poly_face_to_data[index.face].local_indices;
        //             auto &b = bases[index2.element];
        //             array.resize(b.bases.size());
        //             std::iota(array.begin(), array.end(), 0);
        //         }
        //     }
        // }

        for(auto &k : poly_face_to_data)
        {
            auto &array = k.second.local_indices;
            std::sort(array.begin(), array.end());
            auto it = std::unique(array.begin(), array.end());
            array.resize(std::distance(array.begin(), it));
        }

        return n_bases;
    }

    void SplineBasis3d::fit_nodes(const Mesh3D &mesh, const int n_bases, std::vector< ElementBases > &gbases)
    {
        assert(false);
        // const int dim = 3;
        // const int n_constraints =  27;
        // const int n_elements = mesh.n_elements();

        // std::vector< Eigen::Triplet<double> > entries, entries_t;

        // MeshNodes nodes(mesh, true, true, 1, 1, 1);
        // // Eigen::MatrixXd tmp;
        // std::vector<AssemblyValues> tmp_val;

        // Eigen::MatrixXd node_rhs(n_constraints*n_elements, dim);
        // Eigen::MatrixXd samples(n_constraints, dim);

        // for(int i = 0; i < n_constraints; ++i)
        //     samples.row(i) = FEBasis3d::quadr_hex_local_node_coordinates(i);

        // for(int i = 0; i < n_elements; ++i)
        // {
        //     auto &base = gbases[i];

        //     if(!mesh.is_cube(i))
        //         continue;

        //     auto global_ids = FEBasis3d::quadr_hex_local_to_global(mesh, i);
        //     assert(global_ids.size() == n_constraints);

        //     for(int j = 0; j < n_constraints; ++j)
        //     {
        //         auto n_id = nodes.node_id_from_primitive(global_ids[j]);
        //         auto n = nodes.node_position(n_id);
        //         for(int d = 0; d < dim; ++d)
        //             node_rhs(n_constraints*i + j, d) = n(d);
        //     }

        //     base.evaluate_bases(samples, tmp_val);
        //     const auto &lbs = base.bases;

        //     const int n_local_bases = int(lbs.size());
        //     for(int j = 0; j < n_local_bases; ++j)
        //     {
        //         const Basis &b = lbs[j];
        //         const auto &tmp = tmp_val[j].val;

        //         for(std::size_t ii = 0; ii < b.global().size(); ++ii)
        //         {
        //             for (long k = 0; k < tmp.size(); ++k)
        //             {
        //                 entries.emplace_back(n_constraints*i + k, b.global()[ii].index, tmp(k)*b.global()[ii].val);
        //                 entries_t.emplace_back(b.global()[ii].index, n_constraints*i + k, tmp(k)*b.global()[ii].val);
        //             }
        //         }
        //     }
        // }

        // Eigen::MatrixXd new_nodes(n_bases, dim);
        // {
        //     StiffnessMatrix mat(n_constraints*n_elements, n_bases);
        //     StiffnessMatrix mat_t(n_bases, n_constraints*n_elements);

        //     mat.setFromTriplets(entries.begin(), entries.end());
        //     mat_t.setFromTriplets(entries_t.begin(), entries_t.end());

        //     StiffnessMatrix A = mat_t * mat;
        //     Eigen::MatrixXd b = mat_t * node_rhs;

        //     json params = {
        //         {"mtype", -2}, // matrix type for Pardiso (2 = SPD)
        //         // {"max_iter", 0}, // for iterative solvers
        //         // {"tolerance", 1e-9}, // for iterative solvers
        //     };
        //     auto solver = LinearSolver::create("", "");
        //     solver->setParameters(params);
        //     solver->analyzePattern(A);
        //     solver->factorize(A);

        //     for(int d = 0; d < dim; ++d)
        //         solver->solve(b.col(d), new_nodes.col(d));
        // }

        // for(int i = 0; i < n_elements; ++i)
        // {
        //     auto &base = gbases[i];

        //     if(!mesh.is_cube(i))
        //         continue;

        //     auto &lbs = base.bases;
        //     const int n_local_bases = int(lbs.size());
        //     for(int j = 0; j < n_local_bases; ++j)
        //     {
        //         Basis &b = lbs[j];

        //         for(std::size_t ii = 0; ii < b.global().size(); ++ii)
        //         {
        //             // if(nodes.is_primitive_boundary(b.global()[ii].index))
        //                 // continue;

        //             for(int d = 0; d < dim; ++d)
        //             {
        //                 b.global()[ii].node(d) = new_nodes(b.global()[ii].index, d);
        //             }
        //         }
        //     }
        // }
    }

}


#ifndef LSC_PLANNER_TRAJ_OPTIMIZER_HPP
#define LSC_PLANNER_TRAJ_OPTIMIZER_HPP

#include <sp_const.hpp>
#include <polynomial.hpp>
#include <param.hpp>
#include <mission.hpp>
#include <collision_constraints.hpp>

// Eigen
#include <Eigen/Dense>
#include <Eigen/Geometry>

// CPLEX
#include <ilcplex/ilocplex.h>

namespace DynamicPlanning {
    class TrajOptimizer {
    public:
        TrajOptimizer(const Param& param, const Mission& mission, const Eigen::MatrixXd& B);

        void solve(const Agent& agent, const CollisionConstraints& constraints);

        void updateParam(const Param& param);

        traj_t getTrajectory();

        double getQPcost() const;

    private:
        Param param;
        Mission mission;
        Eigen::MatrixXd Q_base, Aeq_base, deq, B;

        // trajectory optimization results
        traj_t trajectory;
        double current_qp_cost;

        // Frequently used constants
        int M, n, phi, dim;

        void buildConstraintMatrices(const Agent& agent);

        // Cost matrix Q
        void buildQBase();

        // Constraint matrix A_eq x > d_eq
        void buildAeqBase();

        void buildDeq(const Agent& agent);

        void populatebyrow(IloModel model, IloNumVarArray x, IloRangeArray c,
                           const Agent& agent, const CollisionConstraints& constraints);

        int getTerminalSegments(const Agent& agent) const;
    };
}

#endif //LSC_PLANNER_TRAJ_OPTIMIZER_HPP


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE PTranformd bench
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/timer/timer.hpp>
// The inclusion of boost chrono was commented in timer.hpp for boost >= 1.60.
// Because of this, the auto-link feature does not incude the chrono library
// anymore, what causes a link error.
// (see also https://svn.boost.org/trac/boost/ticket/11862)
// We add manually the line.
// Possible alternative: include only for specific version of boost and
// auto-link capable compiler
#include <boost/chrono/chrono.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;

BOOST_AUTO_TEST_CASE(PTransfromd_PTransformd)
{
  using namespace sva;

  const std::size_t size = 10000000;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<PTransformd> pt2(size, PTransformd::Identity());
  std::vector<PTransformd> ptRes(size);

  std::cout << "PTransform vs PTransform" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      ptRes[i] = pt1[i] * pt2[i];
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_MotionVec)
{
  using namespace sva;

  const std::size_t size = 10000000;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<MotionVecd> mv(size, MotionVecd(Eigen::Vector6d::Random()));
  std::vector<MotionVecd> mvRes(size);

  std::cout << "PTransform vs MotionVec" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      mvRes[i] = pt1[i] * mv[i];
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_MotionEigen)
{
  using namespace sva;

  const std::size_t size = 10000000;
  const std::size_t cols = 3;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<Matrix6Xd> mv(size, Matrix6Xd::Random(6, cols));
  std::vector<Matrix6Xd> mvRes(size, Matrix6Xd(6, cols));

  std::cout << "PTransform vs MotionEigen" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      pt1[i].mul(mv[i], mvRes[i]);
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_as_matrix_MotionEigen)
{
  using namespace sva;

  const std::size_t size = 10000000;
  const std::size_t cols = 3;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<Matrix6Xd> mv(size, Matrix6Xd::Random(6, cols));
  std::vector<Matrix6Xd> mvRes(size, Matrix6Xd(6, cols));

  std::cout << "PTransform as matrix vs MotionEigen" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      mvRes[i].noalias() = pt1[i].matrix() * mv[i];
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_MotionEigen_as_motion)
{
  using namespace sva;

  const std::size_t size = 10000000;
  const std::size_t cols = 3;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<Matrix6Xd> mv(size, Matrix6Xd::Random(6, cols));
  std::vector<Matrix6Xd> mvRes(size, Matrix6Xd(6, cols));

  std::cout << "PTransform vs MotionEigen as MotionVec" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      for(std::size_t j = 0; j < cols; ++j)
      {
        mvRes[i].col(j).noalias() = (pt1[i] * MotionVecd(mv[i].col(j))).vector();
      }
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_inv_MotionVec)
{
  using namespace sva;

  const std::size_t size = 10000000;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<MotionVecd> mv(size, MotionVecd(Eigen::Vector6d::Random()));
  std::vector<MotionVecd> mvRes(size);

  std::cout << "PTransform_inv vs MotionVec" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      mvRes[i] = pt1[i].inv() * mv[i];
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_invMul_MotionVec)
{
  using namespace sva;

  const std::size_t size = 10000000;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<MotionVecd> mv(size, MotionVecd(Eigen::Vector6d::Random()));
  std::vector<MotionVecd> mvRes(size);

  std::cout << "PTransform_invMul vs MotionVec" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      mvRes[i] = pt1[i].invMul(mv[i]);
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_dual_ForceVec)
{
  using namespace sva;

  const std::size_t size = 10000000;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<ForceVecd> mv(size, ForceVecd(Eigen::Vector6d::Random()));
  std::vector<ForceVecd> mvRes(size);

  std::cout << "PTransform dual vs ForceVec" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      mvRes[i] = pt1[i].dualMul(mv[i]);
    }
  }
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(PTransfromd_trans_ForceVec)
{
  using namespace sva;

  const std::size_t size = 10000000;
  std::vector<PTransformd> pt1(size, PTransformd::Identity());
  std::vector<ForceVecd> mv(size, ForceVecd(Eigen::Vector6d::Random()));
  std::vector<ForceVecd> mvRes(size);

  std::cout << "PTransform trans vs ForceVec" << std::endl;
  {
    boost::timer::auto_cpu_timer t;
    for(std::size_t i = 0; i < size; ++i)
    {
      mvRes[i] = pt1[i].transMul(mv[i]);
    }
  }
  std::cout << std::endl;
}


///////////////////////////////////////////////////////////////////////////////
// as_quantifier.hpp
//
//  Copyright 2008 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_XPRESSIVE_DETAIL_STATIC_TRANSFORMS_AS_QUANTIFIER_HPP_EAN_04_01_2007
#define BOOST_XPRESSIVE_DETAIL_STATIC_TRANSFORMS_AS_QUANTIFIER_HPP_EAN_04_01_2007

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/xpressive/detail/detail_fwd.hpp>
#include <boost/xpressive/detail/static/static.hpp>
#include <boost/xpressive/proto/proto.hpp>

namespace boost { namespace xpressive { namespace detail
{
    ///////////////////////////////////////////////////////////////////////////////
    // generic_quant_tag
    template<uint_t Min, uint_t Max>
    struct generic_quant_tag
    {
        typedef mpl::integral_c<uint_t, Min> min_type;
        typedef mpl::integral_c<uint_t, Max> max_type;
    };
}}}

namespace boost { namespace xpressive { namespace grammar_detail
{
    using detail::uint_t;

    ///////////////////////////////////////////////////////////////////////////////
    // min_type / max_type
    template<typename Tag>
    struct min_type : Tag::min_type {};

    template<>
    struct min_type<proto::tag::posit> : mpl::integral_c<uint_t, 1> {};

    template<>
    struct min_type<proto::tag::dereference> : mpl::integral_c<uint_t, 0> {};

    template<>
    struct min_type<proto::tag::logical_not> : mpl::integral_c<uint_t, 0> {};

    template<typename Tag>
    struct max_type : Tag::max_type {};

    template<>
    struct max_type<proto::tag::posit> : mpl::integral_c<uint_t, UINT_MAX-1> {};

    template<>
    struct max_type<proto::tag::dereference> : mpl::integral_c<uint_t, UINT_MAX-1> {};

    template<>
    struct max_type<proto::tag::logical_not> : mpl::integral_c<uint_t, 1> {};

    ///////////////////////////////////////////////////////////////////////////////
    // as_simple_quantifier
    template<typename Grammar, typename Greedy>
    struct as_simple_quantifier : proto::callable
    {
        template<typename Sig> struct result {};

        template<typename This, typename Expr, typename State, typename Visitor>
        struct result<This(Expr, State, Visitor)>
        {
            typedef typename proto::result_of::arg<Expr>::type arg_type;
            typedef typename Grammar::template result<void(arg_type, detail::true_xpression, Visitor)>::type xpr_type;
            typedef detail::simple_repeat_matcher<xpr_type, Greedy> matcher_type;
            typedef typename proto::terminal<matcher_type>::type type;
        };

        template<typename Expr, typename State, typename Visitor>
        typename result<void(Expr, State, Visitor)>::type
        operator ()(Expr const &expr, State const &, Visitor &visitor) const
        {
            typedef result<void(Expr, State, Visitor)> result_;
            typedef typename result_::arg_type arg_type;
            typedef typename result_::xpr_type xpr_type;
            typedef typename result_::matcher_type matcher_type;
            typedef typename Expr::proto_tag tag;

            xpr_type const &xpr = Grammar()(proto::arg(expr), detail::true_xpression(), visitor);
            matcher_type matcher(xpr, (uint_t)min_type<tag>(), (uint_t)max_type<tag>(), xpr.get_width().value());
            return proto::terminal<matcher_type>::type::make(matcher);
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // add_hidden_mark
    struct add_hidden_mark : proto::callable
    {
        template<typename Sig> struct result {};

        template<typename This, typename Expr, typename State, typename Visitor>
        struct result<This(Expr, State, Visitor)>
        {
            typedef
                typename shift_right<
                    terminal<detail::mark_begin_matcher>::type
                  , typename shift_right<
                        Expr
                      , terminal<detail::mark_end_matcher>::type
                    >::type
                >::type
            type;
        };

        template<typename Expr, typename State, typename Visitor>
        typename result<void(Expr, State, Visitor)>::type
        operator ()(Expr const &expr, State const &, Visitor &visitor) const
        {
            // we're inserting a hidden mark ... so grab the next hidden mark number.
            int mark_nbr = visitor.get_hidden_mark();
            detail::mark_begin_matcher begin(mark_nbr);
            detail::mark_end_matcher end(mark_nbr);

            typename result<void(Expr, State, Visitor)>::type that
                = {{begin}, {expr, {end}}};
            return that;
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // InsertMark
    struct InsertMark
      : or_<
            when<proto::assign<detail::basic_mark_tag, _>, _>
          , otherwise<add_hidden_mark>
        >
    {};

    ///////////////////////////////////////////////////////////////////////////////
    // as_default_quantifier_impl
    template<typename Greedy, uint_t Min, uint_t Max>
    struct as_default_quantifier_impl : proto::callable
    {
        template<typename Sig> struct result {};

        template<typename This, typename Expr, typename State, typename Visitor>
        struct result<This(Expr, State, Visitor)>
        {
            typedef
                typename InsertMark::template result<void(typename proto::result_of::arg<Expr>::type, State, Visitor)>::type
            marked_sub_type;

            typedef
                typename shift_right<
                    terminal<detail::repeat_begin_matcher>::type
                  , typename shift_right<
                        marked_sub_type
                      , typename terminal<detail::repeat_end_matcher<Greedy> >::type
                    >::type
                >::type
            type;
        };

        template<typename Expr, typename State, typename Visitor>
        typename result<void(Expr, State, Visitor)>::type
        operator ()(Expr const &expr, State const &state, Visitor &visitor) const
        {
            // Ensure this sub-expression is book-ended with mark matchers
            typename result<void(Expr, State, Visitor)>::marked_sub_type const &
                marked_sub = InsertMark()(proto::arg(expr), state, visitor);

            // Get the mark_number from the begin_mark_matcher
            int mark_number = proto::arg(proto::left(marked_sub)).mark_number_;
            BOOST_ASSERT(0 != mark_number);

            uint_t min_ = (uint_t)min_type<typename Expr::proto_tag>();
            uint_t max_ = (uint_t)max_type<typename Expr::proto_tag>();

            detail::repeat_begin_matcher begin(mark_number);
            detail::repeat_end_matcher<Greedy> end(mark_number, min_, max_);

            typename result<void(Expr, State, Visitor)>::type that
                = {{begin}, {marked_sub, {end}}};
            return that;
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // optional_tag
    template<typename Greedy>
    struct optional_tag
    {};

    ///////////////////////////////////////////////////////////////////////////////
    // as_default_optional
    template<typename Grammar, typename Greedy>
    struct as_default_optional : proto::callable
    {
        template<typename Sig> struct result {};

        template<typename This, typename Expr, typename State, typename Visitor>
        struct result<This(Expr, State, Visitor)>
        {
            typedef detail::optional_matcher<
                typename Grammar::template result<void(Expr, detail::alternate_end_xpression, Visitor)>::type
              , Greedy
            > type;
        };

        template<typename Expr, typename State, typename Visitor>
        typename result<void(Expr, State, Visitor)>::type
        operator ()(Expr const &expr, State const &, Visitor &visitor) const
        {
            return typename result<void(Expr, State, Visitor)>::type(
                Grammar()(expr, detail::alternate_end_xpression(), visitor)
            );
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // as_mark_optional
    template<typename Grammar, typename Greedy>
    struct as_mark_optional : proto::callable
    {
        template<typename Sig> struct result {};

        template<typename This, typename Expr, typename State, typename Visitor>
        struct result<This(Expr, State, Visitor)>
        {
            typedef detail::optional_mark_matcher<
                typename Grammar::template result<void(Expr, detail::alternate_end_xpression, Visitor)>::type
              , Greedy
            > type;
        };

        template<typename Expr, typename State, typename Visitor>
        typename result<void(Expr, State, Visitor)>::type
        operator ()(Expr const &expr, State const &, Visitor &visitor) const
        {
            int mark_number = proto::arg(proto::left(expr)).mark_number_;
            return typename result<void(Expr, State, Visitor)>::type(
                Grammar()(expr, detail::alternate_end_xpression(), visitor)
              , mark_number
            );
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // IsMarkerOrRepeater
    struct IsMarkerOrRepeater
      : or_<
            shift_right<terminal<detail::repeat_begin_matcher>, _>
          , assign<terminal<detail::mark_placeholder>, _>
        >
    {};

    ///////////////////////////////////////////////////////////////////////////////
    // as_optional
    template<typename Grammar, typename Greedy>
    struct as_optional
      : or_<
            when<IsMarkerOrRepeater, as_mark_optional<Grammar, Greedy> >
          , otherwise<as_default_optional<Grammar, Greedy> >
        >
    {};

    ///////////////////////////////////////////////////////////////////////////////
    // make_optional_
    template<typename Greedy>
    struct make_optional_ : proto::callable
    {
        template<typename Sig> struct result {};

        template<typename This, typename Expr, typename State, typename Visitor>
        struct result<This(Expr, State, Visitor)>
        {
            typedef typename unary_expr<optional_tag<Greedy>, Expr>::type type;
        };

        template<typename Expr, typename State, typename Visitor>
        typename unary_expr<optional_tag<Greedy>, Expr>::type
        operator ()(Expr const &expr, State const &, Visitor &) const
        {
            typename unary_expr<optional_tag<Greedy>, Expr>::type that = {expr};
            return that;
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // as_default_quantifier_impl
    template<typename Greedy, uint_t Max>
    struct as_default_quantifier_impl<Greedy, 0, Max>
      : call<make_optional_<Greedy>(as_default_quantifier_impl<Greedy, 1, Max>)>
    {};

    ///////////////////////////////////////////////////////////////////////////////
    // as_default_quantifier_impl
    template<typename Greedy>
    struct as_default_quantifier_impl<Greedy, 0, 1>
      : call<make_optional_<Greedy>(_arg)>
    {};

    ///////////////////////////////////////////////////////////////////////////////
    // as_default_quantifier
    template<typename Greedy>
    struct as_default_quantifier : proto::callable
    {
        template<typename Sig> struct result {};

        template<typename This, typename Expr, typename State, typename Visitor>
        struct result<This(Expr, State, Visitor)>
        {
            typedef
                as_default_quantifier_impl<
                    Greedy
                  , min_type<typename Expr::proto_tag>::value
                  , max_type<typename Expr::proto_tag>::value
                >
            impl;

            typedef typename impl::template result<void(Expr, State, Visitor)>::type type;
        };

        template<typename Expr, typename State, typename Visitor>
        typename result<void(Expr, State, Visitor)>::type
        operator ()(Expr const &expr, State const &state, Visitor &visitor) const
        {
            return as_default_quantifier_impl<
                Greedy
              , min_type<typename Expr::proto_tag>::value
              , max_type<typename Expr::proto_tag>::value
            >()(expr, state, visitor);
        }
    };

}}}

#endif


#include <boost/algorithm/hex.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION >= 106600
#include <boost/uuid/detail/sha1.hpp>
#else
#include <boost/uuid/sha1.hpp>
#endif
#include <cstring>
#include <util/crypto_hash.h>

class crypto_hash_private
{
public:
  boost::uuids::detail::sha1 s;
};

bool crypto_hash::operator<(const crypto_hash h2) const
{
  if(memcmp(hash, h2.hash, CRYPTO_HASH_SIZE) < 0)
    return true;

  return false;
}

std::string crypto_hash::to_string() const
{
  std::ostringstream buf;
  for(unsigned int i : hash)
    buf << std::hex << std::setfill('0') << std::setw(8) << i;

  return buf.str();
}

crypto_hash::crypto_hash()
  : p_crypto(std::make_shared<crypto_hash_private>()), hash{0}
{
}

void crypto_hash::ingest(void const *data, unsigned int size)
{
  p_crypto->s.process_bytes(data, size);
}

void crypto_hash::fin()
{
  p_crypto->s.get_digest(hash);
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_POINT_TO_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_POINT_TO_GEOMETRY_HPP

#include <iterator>
#include <type_traits>

#include <boost/core/ignore_unused.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/size.hpp>
#include <boost/range/value_type.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/detail/closest_feature/geometry_to_range.hpp>
#include <boost/geometry/algorithms/detail/closest_feature/point_to_range.hpp>
#include <boost/geometry/algorithms/detail/distance/is_comparable.hpp>
#include <boost/geometry/algorithms/detail/distance/iterator_selector.hpp>
#include <boost/geometry/algorithms/detail/distance/strategy_utils.hpp>
#include <boost/geometry/algorithms/detail/within/point_in_geometry.hpp>
#include <boost/geometry/algorithms/dispatch/distance.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/relate/services.hpp>
#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace closest_points
{

struct point_to_point
{
    template <typename P1, typename P2, typename Segment, typename Strategies>
    static inline void apply(P1 const& p1, P2 const& p2,
                             Segment& shortest_seg, Strategies const&)
    {
        set_segment_from_points::apply(p1, p2, shortest_seg);
    }
};

struct point_to_segment
{
    template <typename Point, typename Segment, typename OutputSegment, typename Strategies>
    static inline void apply(Point const& point, Segment const& segment,
                             OutputSegment& shortest_seg, Strategies const& strategies)
    {
        typename point_type<Segment>::type p[2];
        geometry::detail::assign_point_from_index<0>(segment, p[0]);
        geometry::detail::assign_point_from_index<1>(segment, p[1]);

        boost::ignore_unused(strategies);

        auto closest_point = strategies.closest_points(point, segment)
            .apply(point, p[0], p[1]);

        set_segment_from_points::apply(point, closest_point, shortest_seg);
    }
};

/*
struct point_to_box
{
    template<typename Point, typename Box, typename Strategies>
static inline auto apply(Point const& point, Box const& box,
                             Strategies const& strategies)
    {
        boost::ignore_unused(strategies);
        return strategies.closest_points(point, box).apply(point, box);
    }
};
*/

template <closure_selector Closure>
class point_to_range
{
public:

    template <typename Point, typename Range, typename Segment, typename Strategies>
    static inline void apply(Point const& point, Range const& range,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        using point_to_point_range = detail::closest_feature::point_to_point_range
            <
                Point, Range, Closure
            >;

        if (boost::size(range) == 0)
        {
            set_segment_from_points::apply(point, point, shortest_seg);
            return;
        }

        closest_points::creturn_t<Point, Range, Strategies> cd_min;

        auto comparable_distance = strategy::distance::services::get_comparable
            <
                decltype(strategies.distance(point, range))
            >::apply(strategies.distance(point, range));

        auto closest_segment = point_to_point_range::apply(point,
                                                           boost::begin(range),
                                                           boost::end(range),
                                                           comparable_distance,
                                                           cd_min);

        auto closest_point = strategies.closest_points(point, range)
            .apply(point, *closest_segment.first, *closest_segment.second);

        set_segment_from_points::apply(point, closest_point, shortest_seg);
    }
};

template<closure_selector Closure>
struct point_to_ring
{
    template <typename Point, typename Ring, typename Segment, typename Strategies>
    static inline auto apply(Point const& point,
                             Ring const& ring,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        if (within::within_point_geometry(point, ring, strategies))
        {
            set_segment_from_points::apply(point, point, shortest_seg);
        }
        else
        {
            point_to_range
            <
                closure<Ring>::value
            >::apply(point, ring, shortest_seg, strategies);
        }

    }
};

template <closure_selector Closure>
class point_to_polygon
{
    template <typename Polygon>
    struct distance_to_interior_rings
    {
        template
        <
            typename Point,
            typename InteriorRingIterator,
            typename Segment,
            typename Strategies
        >
        static inline void apply(Point const& point,
                                 InteriorRingIterator first,
                                 InteriorRingIterator last,
                                 Segment& shortest_seg,
                                 Strategies const& strategies)
        {
            using per_ring = point_to_range<Closure>;

            for (InteriorRingIterator it = first; it != last; ++it)
            {
                if (within::within_point_geometry(point, *it, strategies))
                {
                    // the point is inside a polygon hole, so its distance
                    // to the polygon is its distance to the polygon's
                    // hole boundary
                    per_ring::apply(point, *it, shortest_seg, strategies);
                    return;
                }
            }
            set_segment_from_points::apply(point, point, shortest_seg);
        }

        template
        <
            typename Point,
            typename InteriorRings,
            typename Segment,
            typename Strategies
        >
        static inline void apply(Point const& point, InteriorRings const& interior_rings,
                                 Segment& shortest_seg, Strategies const& strategies)
        {
            apply(point,
                  boost::begin(interior_rings),
                  boost::end(interior_rings),
                  shortest_seg,
                  strategies);
        }
    };

public:
    template
    <
        typename Point,
        typename Polygon,
        typename Segment,
        typename Strategies
    >
    static inline void apply(Point const& point,
                             Polygon const& polygon,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        using per_ring = point_to_range<Closure>;

        if (! within::covered_by_point_geometry(point, exterior_ring(polygon),
                                                strategies))
        {
            // the point is outside the exterior ring, so its distance
            // to the polygon is its distance to the polygon's exterior ring
            per_ring::apply(point, exterior_ring(polygon), shortest_seg, strategies);
            return;
        }

        // Check interior rings
        distance_to_interior_rings<Polygon>::apply(point,
                                                   interior_rings(polygon),
                                                   shortest_seg,
                                                   strategies);
    }
};

template
<
    typename MultiGeometry,
    bool CheckCoveredBy = std::is_same
        <
            typename tag<MultiGeometry>::type, multi_polygon_tag
        >::value
>
class point_to_multigeometry
{
private:
    using geometry_to_range = detail::closest_feature::geometry_to_range;

public:

    template
    <
        typename Point,
        typename Segment,
        typename Strategies
    >
    static inline void apply(Point const& point,
                             MultiGeometry const& multigeometry,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        using selector_type = distance::iterator_selector<MultiGeometry const>;

        closest_points::creturn_t<Point, MultiGeometry, Strategies> cd;

        auto comparable_distance = strategy::distance::services::get_comparable
            <
                decltype(strategies.distance(point, multigeometry))
            >::apply(strategies.distance(point, multigeometry));

        typename selector_type::iterator_type it_min
            = geometry_to_range::apply(point,
                                       selector_type::begin(multigeometry),
                                       selector_type::end(multigeometry),
                                       comparable_distance,
                                       cd);

        dispatch::closest_points
            <
                Point,
                typename std::iterator_traits
                    <
                        typename selector_type::iterator_type
                    >::value_type
            >::apply(point, *it_min, shortest_seg, strategies);
    }
};

// this is called only for multipolygons, hence the change in the
// template parameter name MultiGeometry to MultiPolygon
template <typename MultiPolygon>
struct point_to_multigeometry<MultiPolygon, true>
{
    template
    <
        typename Point,
        typename Segment,
        typename Strategies
    >
    static inline void apply(Point const& point,
                             MultiPolygon const& multipolygon,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        if (within::covered_by_point_geometry(point, multipolygon, strategies))
        {
            set_segment_from_points::apply(point, point, shortest_seg);
            return;
        }

        return point_to_multigeometry
            <
                MultiPolygon, false
            >::apply(point, multipolygon, shortest_seg, strategies);
    }
};

}} // namespace detail::closest_points
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename P1, typename P2>
struct closest_points
    <
        P1, P2, point_tag, point_tag, false
    > : detail::closest_points::point_to_point
{};

template <typename Point, typename Linestring>
struct closest_points
    <
        Point, Linestring, point_tag, linestring_tag, false
    > : detail::closest_points::point_to_range<closed>
{};

template <typename Point, typename Ring>
struct closest_points
    <
        Point, Ring, point_tag, ring_tag, false
    > : detail::closest_points::point_to_ring
        <
            closure<Ring>::value
        >
{};

template <typename Point, typename Polygon>
struct closest_points
    <
        Point, Polygon, point_tag, polygon_tag, false
    > : detail::closest_points::point_to_polygon
        <
            closure<Polygon>::value
        >
{};

template <typename Point, typename Segment>
struct closest_points
    <
        Point, Segment, point_tag, segment_tag, false
    > : detail::closest_points::point_to_segment
{};

/*
template <typename Point, typename Box>
struct closest_points
    <
         Point, Box, point_tag, box_tag,
         strategy_tag_distance_point_box, false
    > : detail::closest_points::point_to_box<Point, Box>
{};
*/

template<typename Point, typename MultiPoint>
struct closest_points
    <
        Point, MultiPoint, point_tag, multi_point_tag, false
    > : detail::closest_points::point_to_multigeometry<MultiPoint>
{};

template<typename Point, typename MultiLinestring>
struct closest_points
    <
        Point, MultiLinestring, point_tag, multi_linestring_tag, false
    > : detail::closest_points::point_to_multigeometry<MultiLinestring>
{};

template<typename Point, typename MultiPolygon>
struct closest_points
    <
        Point, MultiPolygon, point_tag, multi_polygon_tag, false
    > : detail::closest_points::point_to_multigeometry<MultiPolygon>
{};

template <typename Point, typename Linear>
struct closest_points
    <
         Point, Linear, point_tag, linear_tag, false
    > : closest_points
        <
            Point, Linear,
            point_tag, typename tag<Linear>::type, false
        >
{};

template <typename Point, typename Areal>
struct closest_points
    <
         Point, Areal, point_tag, areal_tag, false
    > : closest_points
        <
            Point, Areal,
            point_tag, typename tag<Areal>::type, false
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_POINT_TO_GEOMETRY_HPP


#include "FighterEntity.h"
#include "../Config/PlayerMovement.h"
#include "../Config/GameRules.h"
#include <boost/math/constants/constants.hpp>

#include <DescentEngine/src/EntityEngine/EntityEngine.h>
#include <DescentEngine/src/Pathfinding/Pathfinding.h>
#include <DescentEngine/src/Pathfinding/Node.h>
#include <DescentEngine/src/Timing.h>
#include <bullet/btBulletCollisionCommon.h>

FighterEntity::FighterEntity(Vector2 intialPosition, Rectangle2 boundingBox) :
		MultiVisualEntity(intialPosition, boundingBox), m_dead(false), m_life(GameRules::FighterLife), m_attackTime(
				EnemyBehaviour::AttacDelay), m_lastHit(-1.0f) {
	m_doesCollisionStick = true;
	setCollisionGroup(GameCollisionGroups::Characters);
	setCollisionMask(GameCollisionGroups::Characters_CollidesWith);
}

int FighterEntity::getCurrentSekktor() const {
	auto dir = getDirection();

	float cosPhi = acos(dir.y());
	// handle left sektor
	if (dir.x() < 0.0f) {
		cosPhi = boost::math::constants::pi<float>() + boost::math::constants::pi<float>() - cosPhi;
	}
	float sektor = (cosPhi / (2.0f * boost::math::constants::pi<float>())) * 8.0f;
	// rotate, so the up / down position will be in the middle of a sekktor
	// and not on the border
	sektor += 0.5f;
	// handle wrap around
	if (sektor < 0.0f) {
		sektor = std::fabs(0.5f + sektor);
	}
	if (sektor >= 8.0f) {
		sektor = 0.0f;
	}

	return std::floor(sektor);
}

void FighterEntity::updateVisual(Engines & eg) {
	const int sekktor = getCurrentSekktor();

	if (isDead()) {
		changeActiveVisual(eg, DescentTextureIds::Dead_0 + sekktor);
	} else if (getJumpAction().isActive()) {
		changeActiveVisual(eg, DescentTextureIds::Jump_0 + sekktor);
	} else if (getKickAction().isActive()) {
		changeActiveVisual(eg, DescentTextureIds::Kick_0 + sekktor);
	} else if (getHitAction().isActive()) {
		changeActiveVisual(eg, DescentTextureIds::Hit_0 + sekktor);
		// todo: use movement here so only walk when we actually move
	} else if (wasMoved()) {
		changeActiveVisual(eg, DescentTextureIds::Walk_0 + sekktor);
	} else {
		// idle animation
		// todo: call only when needed
		changeActiveVisual(eg, DescentTextureIds::Stand_0 + sekktor);
	}
}

void FighterEntity::validateActions(float deltaT, float hitRegenTime) {
	// todo: not so nice, make this more generic
	getJumpAction().validate(deltaT, PlayerMovement::JumpRegenerationTime);
	getKickAction().validate(deltaT, PlayerMovement::JumpRegenerationTime);
	getHitAction().validate(deltaT, hitRegenTime);
}

void FighterEntity::die() {
	m_dead = true;
	setCollisionGroup(GameCollisionGroups::CharactersDead);
	setCollisionMask(GameCollisionGroups::CharactersDead_CollidesWith);
	logging::Debug<>() << "A Fighter died !";
}

float FighterEntity::runTo(Vector2 location, float maxSpeed, float deltaT, EntityEngine & entNG,
		float closeThreshold) {

	constexpr float minimumMoveSq(util::sq(0.1f));

	// we are almost there already ...
	if (util::withinDelta(getPosition(), location, minimumMoveSq)) {
		return 0.0f;
	}

	bool runPathfinding = true;

	if (m_nextNode.isValid())
		if (!util::withinDelta(getPosition(), m_nextNode.getValue(), minimumMoveSq)) {
			// just move further to the last node found
			// not close enough to this node, yet
			runPathfinding = false;
		}

	if (runPathfinding) {
		Pathfinding & finding = entNG.getPathfinding();

		Node * startNode = entNG.findClosestNode(this->getPosition());
		Node * endNode = entNG.findClosestNode(location);

		assert(startNode);
		assert(endNode);

		//Timing tm;
		finding.reset();
		finding.resetNodes(entNG.getPathfindingNodes());
		const auto path = finding.pathToNode(startNode, endNode);
		//float dt = tm.end();
		//logging::Info() << "Pathfinding took " << dt << " seconds";

		// move to the next node:
		const float distToTarget = startNode->distanceTo(endNode);
		if (path.size() > 1) {
			const Vector2 nextLocation = path[1]->Location;
			m_nextNode.setValue(nextLocation);
		} else {
			m_nextNode.invalidate();
			return 0.0f;
		}
	}
	const Vector2 conVector(m_nextNode.getValue() - getPosition());
	const float dist = conVector.mag();
	const Vector2 dir = conVector.normalizedCopy();

	if (dir.nonZero()) {
		const float maxSpeedPerTimeStep = maxSpeed * deltaT;

		const Vector2 maxMove = dir * maxSpeedPerTimeStep;
		setDirection(dir);
		setMoveDelta(maxMove);
	}

	const float distToTarget = (getPosition() - location).mag();

	return distToTarget;
}