#include <polyfem/SplineBasis2d.hpp>

#include <polyfem/QuadraticBSpline2d.hpp>
#include <polyfem/QuadQuadrature.hpp>
#include <polyfem/MeshNodes.hpp>

#include <polysolve/LinearSolver.hpp>
#include <polyfem/FEBasis2d.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>

//TODO carefull with simplices

namespace polyfem
{
    using namespace polysolve;
    using namespace Eigen;

    namespace
    {
        typedef Matrix<std::vector<int>, 3, 3> SpaceMatrix;
        typedef Matrix<RowVectorNd, 3, 3> NodeMatrix;

        void print_local_space(const SpaceMatrix &space)
        {
            std::cout<<std::endl;
            for(int j=2; j >=0; --j)
            {
                for(int i=0; i < 3; ++i)
                {
                    if(space(i, j).size() > 0){
                        for(std::size_t l = 0; l < space(i, j).size(); ++l)
                            std::cout<<space(i, j)[l]<<",";

                        std::cout<<"\t";
                    }
                    else
                        std::cout<<"x\t";
                }
                std::cout<<std::endl;
            }
        }

        int node_id_from_edge_index(const Mesh2D &mesh, MeshNodes &mesh_nodes, const Navigation::Index &index)
        {
            const int face_id = mesh.switch_face(index).face;
            if(face_id >= 0 && mesh.is_cube(face_id))
                return mesh_nodes.node_id_from_face(face_id);

            return mesh_nodes.node_id_from_edge(index.edge);
        }

        void explore_direction(const Navigation::Index &index, const Mesh2D &mesh, MeshNodes &mesh_nodes, const int x, const int y, const bool is_x, const bool invert, LocalBoundary &lb, SpaceMatrix &space, NodeMatrix &node, std::map<int, InterfaceData> &poly_edge_to_data)
        {
            int node_id = node_id_from_edge_index(mesh, mesh_nodes, index);
            // bool real_boundary = mesh.node_id_from_edge_index(index, node_id);

            assert(std::find(space(x, y).begin(), space(x, y).end(), node_id) == space(x, y).end());
            space(x, y).push_back(node_id);
            node(x, y) = mesh_nodes.node_position(node_id);
            assert(node(x, y).size() == 2);

            const int x1 =  is_x ? x : (invert? 2 : 0);
            const int y1 = !is_x ? y : (invert? 2 : 0);

            const int x2 =  is_x ? x : (invert? 0 : 2);
            const int y2 = !is_x ? y : (invert? 0 : 2);

            const bool is_boundary = mesh_nodes.is_boundary(node_id);
            const bool is_interface = mesh_nodes.is_interface(node_id);

            if(is_boundary)
            {
                lb.add_boundary_primitive(index.edge, FEBasis2d::quad_edge_local_nodes(2, mesh, index)[1]-4);
                // lb.add_boundary_primitive(index.edge, FEBasis2d::quadr_quad_edge_local_nodes(mesh, index)[1]-4);
                // bounday_nodes.push_back(node_id);
            }
            else if(is_interface)
            {
                InterfaceData &data = poly_edge_to_data[index.edge];
                data.local_indices.push_back(y * 3 + x);
            }
            else
            {
                assert(!is_boundary && !is_interface);

                Navigation::Index start_index = mesh.switch_face(index);
                assert(start_index.vertex == index.vertex);
                assert(start_index.face >= 0);

                Navigation::Index edge1 = mesh.switch_edge(start_index);
                node_id = node_id_from_edge_index(mesh, mesh_nodes, edge1);
                // if(mesh_nodes.is_boundary(node_id))
                    // bounday_nodes.push_back(node_id);

                if(std::find(space(x1, y1).begin(), space(x1, y1).end(), node_id) == space(x1, y1).end())
                {
                    space(x1, y1).push_back(node_id);
                    node(x1, y1) = mesh_nodes.node_position(node_id);
                }

                Navigation::Index edge2 = mesh.switch_edge(mesh.switch_vertex(start_index));
                node_id = node_id_from_edge_index(mesh, mesh_nodes, edge2);
                // if(mesh_nodes.is_boundary(node_id))
                    // bounday_nodes.push_back(node_id);
                if(std::find(space(x2, y2).begin(), space(x2, y2).end(), node_id) == space(x2, y2).end())
                {
                    space(x2, y2).push_back(node_id);
                    node(x2, y2) = mesh_nodes.node_position(node_id);
                    // node(x2, y2).push_back(mesh.node_from_edge_index(edge2));
                }
            }
        }

        void add_id_for_poly(const Navigation::Index &index, const int x1, const int y1, const int x2, const int y2, const SpaceMatrix &space, std::map<int, InterfaceData> &poly_edge_to_data)
        {
            auto it = poly_edge_to_data.find(index.edge);
            if(it != poly_edge_to_data.end())
            {
                InterfaceData &data = it->second;

                assert(space(x1, y1).size() == 1);
                data.local_indices.push_back(y1 * 3 + x1);

                assert(space(x2, y2).size() == 1);
                data.local_indices.push_back(y2 * 3 + x2);
            }
        }

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

            Navigation::Index index;
            // space.setConstant(-1);

            const int el_node_id = mesh_nodes.node_id_from_face(el_index);
            space(1, 1).push_back(el_node_id);
            node(1, 1) = mesh_nodes.node_position(el_node_id);
            // (mesh.node_from_face(el_index));

            LocalBoundary lb(el_index, BoundaryType::QuadLine);

            //////////////////////////////////////////
            index = mesh.get_index_from_face(el_index);
            explore_direction(index, mesh, mesh_nodes, 1, 0, false, false, lb, space, node, poly_edge_to_data);

            //////////////////////////////////////////
            index = mesh.next_around_face(index);
            explore_direction(index, mesh, mesh_nodes, 2, 1, true, false, lb, space, node, poly_edge_to_data);

            //////////////////////////////////////////
            index = mesh.next_around_face(index);
            explore_direction(index, mesh, mesh_nodes, 1, 2, false, true, lb, space, node, poly_edge_to_data);

            //////////////////////////////////////////
            index = mesh.next_around_face(index);
            explore_direction(index, mesh, mesh_nodes, 0, 1, true, true, lb, space, node, poly_edge_to_data);

            //////////////////////////////////////////
            if(mesh_nodes.is_boundary_or_interface(space(1, 2).front()) && mesh_nodes.is_boundary_or_interface(space(2, 1).front()))
            {
                assert(space(2,2).empty());

                Navigation::Index start_index = mesh.get_index_from_face(el_index);
                start_index = mesh.next_around_face(start_index);
                start_index = mesh.next_around_face(start_index);

                const int node_id = mesh_nodes.node_id_from_vertex(start_index.vertex);
                // mesh.vertex_node_id(start_index.vertex);
                space(2,2).push_back(node_id);
                node(2,2) = mesh_nodes.node_position(node_id);
                // node(2,2).push_back(mesh.node_from_vertex(start_index.vertex));

                // bounday_nodes.push_back(node_id);
            }

            if(mesh_nodes.is_boundary_or_interface(space(1, 0).front()) && mesh_nodes.is_boundary_or_interface(space(2, 1).front()))
            {
                assert(space(2,0).empty());

                Navigation::Index start_index = mesh.get_index_from_face(el_index);
                start_index = mesh.next_around_face(start_index);

                const int node_id = mesh_nodes.node_id_from_vertex(start_index.vertex);
                // mesh.vertex_node_id(start_index.vertex);
                space(2,0).push_back(node_id);
                node(2,0) = mesh_nodes.node_position(node_id);
                // .push_back(mesh.node_from_vertex(start_index.vertex));

                // bounday_nodes.push_back(node_id);
            }

            if(mesh_nodes.is_boundary_or_interface(space(1, 2).front()) && mesh_nodes.is_boundary_or_interface(space(0, 1).front()))
            {
                assert(space(0,2).empty());

                Navigation::Index start_index = mesh.get_index_from_face(el_index);
                start_index = mesh.next_around_face(start_index);
                start_index = mesh.next_around_face(start_index);
                start_index = mesh.next_around_face(start_index);

                // const int node_id = mesh.vertex_node_id(start_index.vertex);
                const int node_id = mesh_nodes.node_id_from_vertex(start_index.vertex);
                space(0,2).push_back(node_id);
                node(0,2) = mesh_nodes.node_position(node_id);
                // .push_back(mesh.node_from_vertex(start_index.vertex));

                // bounday_nodes.push_back(node_id);
            }

            if(mesh_nodes.is_boundary_or_interface(space(1, 0).front()) && mesh_nodes.is_boundary_or_interface(space(0, 1).front()))
            {
                Navigation::Index start_index = mesh.get_index_from_face(el_index);

                // const int node_id = mesh.vertex_node_id(start_index.vertex);
                const int node_id = mesh_nodes.node_id_from_vertex(start_index.vertex);
                space(0,0).push_back(node_id);
                node(0,0) = mesh_nodes.node_position(node_id);
                //.push_back(mesh.node_from_vertex(start_index.vertex));

                // bounday_nodes.push_back(node_id);
            }

            // std::cout<<std::endl;
            // print_local_space(space);

            ////////////////////////////////////////////////////////////////////////
            index = mesh.get_index_from_face(el_index);
            add_id_for_poly(index, 0, 0, 2, 0, space, poly_edge_to_data);

            index = mesh.next_around_face(index);
            add_id_for_poly(index, 2, 0, 2, 2, space, poly_edge_to_data);

            index = mesh.next_around_face(index);
            add_id_for_poly(index, 2, 2, 0, 2, space, poly_edge_to_data);

            index = mesh.next_around_face(index);
            add_id_for_poly(index, 0, 2, 0, 0, space, poly_edge_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)
        {
            //left and right neigh are absent
            if(mesh_nodes.is_boundary_or_interface(space(0,1).front()) && mesh_nodes.is_boundary_or_interface(space(2,1).front()))
            {
                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).front()))
            {
                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).front()))
            {
                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).front()) && mesh_nodes.is_boundary_or_interface(space(1,2).front()))
            {
                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).front()))
            {
                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).front()))
            {
                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}};
            }
        }

        void basis_for_regular_quad(const SpaceMatrix &space, const NodeMatrix &loc_nodes, const std::array<std::array<double, 4>, 3> &h_knots, const std::array<std::array<double, 4>, 3> &v_knots, ElementBases &b)
        {
            for(int y = 0; y < 3; ++y)
            {
                for(int x = 0; x < 3; ++x)
                {
                    if(space(x,y).size() == 1)
                    {
                        const int global_index = space(x, y).front();
                        const Eigen::MatrixXd &node = loc_nodes(x,y);
                        assert(node.size() == 2);

                        const int local_index = y*3 + x;
                        b.bases[local_index].init(2, global_index, local_index, node);

                        const QuadraticBSpline2d spline(h_knots[x], v_knots[y]);
                        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_quad(const int el_id, const Mesh2D &mesh, MeshNodes &mesh_nodes, const SpaceMatrix &space, const NodeMatrix &loc_nodes, const std::array<std::array<double, 4>, 3> &h_knots, const std::array<std::array<double, 4>, 3> &v_knots, ElementBases &b)
        {
            for(int y = 0; y < 3; ++y)
            {
                for(int x = 0; x < 3; ++x)
                {
                    if(space(x,y).size() > 1)
                    {
                        const int mpx = 1;
                        const int mpy = y;

                        const int mmx = x;
                        const int mmy = 1;

                        std::vector<int> other_indices;
                        const auto &center = b.bases[1*3 + 1].global().front();

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

                        Navigation::Index start_index = mesh.get_index_from_face(el_id);
                        bool found = false;
                        for(int i =0; i < 4; ++i)
                        {
                            other_indices.clear();
                            int n_neighs = 0;
                            Navigation::Index index = start_index;
                            do
                            {
                                const int f_index = mesh_nodes.node_id_from_face(index.face);
                                if(f_index != el1.index && f_index != el2.index && f_index != center.index)
                                    other_indices.push_back(f_index);

                                ++n_neighs;
                                index = mesh.next_around_vertex(index);
                            }
                            while(index.face != start_index.face);
                            if(n_neighs != 4){
                                found = true;
                                break;
                            }

                            start_index = mesh.next_around_face(start_index);
                        }
                        assert(found);

                        const int local_index = y*3 + x;
                        auto &base = b.bases[local_index];

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

                        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;

                        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 QuadraticBSpline2d spline(h_knots[x], v_knots[y]);
                        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 Mesh2D &mesh, const int el_index, std::set<int> &vertex_id, std::set<int> &edge_id, ElementBases &b, std::vector<LocalBoundary> &local_boundary, int &n_bases)
        {
            b.bases.resize(9);

            LocalBoundary lb(el_index, BoundaryType::QuadLine);

            Navigation::Index index = mesh.get_index_from_face(el_index);
            for (int j = 0; j < 4; ++j)
            {
                int current_vertex_node_id = -1;
                int current_edge_node_id = -1;
                Eigen::Matrix<double, 1, 2> current_edge_node;
                Eigen::MatrixXd current_vertex_node;

                // auto e2l = FEBasis2d::quadr_quad_edge_local_nodes(mesh, index);
                auto e2l = FEBasis2d::quad_edge_local_nodes(2, mesh, index);

                int vertex_basis_id = e2l[0];
                int edge_basis_id = e2l[1];

                const int opposite_face = mesh.switch_face(index).face;

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

                Navigation::Index vindex=index;

                do
                {
                    if(vindex.face < 0)
                    {
                        is_vertex_boundary = true;
                        break;
                    }
                    if(mesh.is_spline_compatible(vindex.face))
                    {
                        is_vertex_q2 = false;
                        break;
                    }
                    vindex = mesh.next_around_vertex(vindex);
                }
                while(vindex.edge != index.edge);

                if(is_vertex_q2)
                {
                    vindex = mesh.switch_face(index);
                    do
                    {
                        if(vindex.face < 0)
                        {
                            is_vertex_boundary = true;
                            break;
                        }

                        if(mesh.is_spline_compatible(vindex.face))
                        {
                            is_vertex_q2 = false;
                            break;
                        }
                        vindex = mesh.next_around_vertex(vindex);
                    }
                    while(vindex.edge != index.edge);
                }

                const bool is_edge_q2 = opposite_face < 0 || !mesh.is_spline_compatible(opposite_face);

                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(opposite_face < 0)
                        {
                            // bounday_nodes.push_back(current_edge_node_id);
                            lb.add_boundary_primitive(index.edge, edge_basis_id-4);
                        }
                    }
                }

                if(is_vertex_q2)
                {
                    assert(is_edge_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[vertex_basis_id].init(2, current_vertex_node_id, vertex_basis_id, current_vertex_node);

                if(current_edge_node_id >= 0)
                    b.bases[edge_basis_id].init(2, current_edge_node_id, edge_basis_id, current_edge_node);

                //set the basis functions
                b.bases[vertex_basis_id].set_basis([vertex_basis_id](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { polyfem::autogen::q_basis_value_2d     (2, vertex_basis_id, uv, val); });
                b.bases[vertex_basis_id].set_grad( [vertex_basis_id](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { polyfem::autogen::q_grad_basis_value_2d(2, vertex_basis_id, uv, val); });

                b.bases[edge_basis_id].set_basis([edge_basis_id](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { polyfem::autogen::q_basis_value_2d     (2, edge_basis_id, uv, val); });
                b.bases[edge_basis_id].set_grad( [edge_basis_id](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { polyfem::autogen::q_grad_basis_value_2d(2, edge_basis_id, uv, val); });

                index = mesh.next_around_face(index);
            }

            //central node always present
            const int face_basis_id = 8;
            b.bases[face_basis_id].init(2, n_bases++, face_basis_id, mesh.face_barycenter(el_index));
            b.bases[face_basis_id].set_basis([face_basis_id](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { polyfem::autogen::q_basis_value_2d     (2, face_basis_id, uv, val); });
            b.bases[face_basis_id].set_grad( [face_basis_id](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { polyfem::autogen::q_grad_basis_value_2d(2, face_basis_id, uv, val); });

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

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

            bool found = false;

            for(std::size_t i = 0; i < vec.size(); ++i)
            {
                if(vec[i].index == data.index)
                {
                    // std::cout<<vec[i].val <<" "<< data.val<<" "<<fabs(vec[i].val - data.val)<<std::endl;
                    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 Mesh2D &mesh, const int el_index, std::vector< ElementBases > &bases)
        {
            // Eigen::MatrixXd eval_p;
            std::vector<AssemblyValues> eval_p;
            Navigation::Index index = mesh.get_index_from_face(el_index);
            ElementBases &b = bases[el_index];

            for (int j = 0; j < 4; ++j)
            {
                const int opposite_face = mesh.switch_face(index).face;

                if(opposite_face < 0 || !mesh.is_cube(opposite_face))
                {
                    index = mesh.next_around_face(index);
                    continue;
                }

                // const auto param_p = FEBasis2d::quadr_quad_edge_local_nodes_coordinates(mesh, mesh.switch_face(index));
                // const auto indices = FEBasis2d::quadr_quad_edge_local_nodes(mesh, index);

                const auto indices = FEBasis2d::quad_edge_local_nodes(2, mesh, index);
                Eigen::Matrix<double, 3, 2> param_p;

                {
                    Eigen::MatrixXd quad_loc_nodes; polyfem::autogen::q_nodes_2d(2, quad_loc_nodes);
                    const auto opposite_indices = FEBasis2d::quad_edge_local_nodes(2, mesh, mesh.switch_face(index));
                    for(int k = 0; k < 3; ++k)
                        param_p.row(k) = quad_loc_nodes.row(opposite_indices[k]);
                }

                // std::cout<<param_p<<"\n---------\n"<<std::endl;

                const int i0 = indices[0];
                const int i1 = indices[1];
                const int i2 = indices[2];

                const auto &other_bases = bases[opposite_face];
                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;

                    assert(eval_p[i].val.size() == 3);

                    //basis i of element opposite face 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)
                    {
                        // auto glob0 = other_b.global()[k]; glob0.val *= eval_p(0,i);
                        // auto glob1 = other_b.global()[k]; glob1.val *= eval_p(1,i);
                        // auto glob2 = other_b.global()[k]; glob2.val *= eval_p(2,i);

                        auto glob0 = other_b.global()[k]; glob0.val *= eval_p[i].val(0);
                        auto glob1 = other_b.global()[k]; glob1.val *= eval_p[i].val(1);
                        auto glob2 = other_b.global()[k]; glob2.val *= eval_p[i].val(2);

                        insert_into_global(glob0, b.bases[i0].global());
                        insert_into_global(glob1, b.bases[i1].global());
                        insert_into_global(glob2, b.bases[i2].global());
                    }
                }

                index = mesh.next_around_face(index);
            }
        }

        void setup_data_for_polygons(const Mesh2D &mesh, const int el_index, const ElementBases &b, std::map<int, InterfaceData> &poly_edge_to_data)
        {
            Navigation::Index index = mesh.get_index_from_face(el_index);
            for (int j = 0; j < 4; ++j)
            {
                const int opposite_face = mesh.switch_face(index).face;
                const bool is_neigh_poly = opposite_face >= 0 && mesh.is_polytope(opposite_face);

                if(is_neigh_poly)
                {
                    // auto e2l = FEBasis2d::quadr_quad_edge_local_nodes(mesh, index);
                    auto e2l = FEBasis2d::quad_edge_local_nodes(2, mesh, index);
                    const int vertex_basis_id = e2l[0];
                    const int edge_basis_id = e2l[1];
                    const int vertex_basis_id2 = e2l[2];

                    InterfaceData &data = poly_edge_to_data[index.edge];

                    data.local_indices.push_back(edge_basis_id);
                    data.local_indices.push_back(vertex_basis_id);
                    data.local_indices.push_back(vertex_basis_id2);
                }

                index = mesh.next_around_face(index);
            }
        }
    }

    int SplineBasis2d::build_bases(const Mesh2D &mesh, const int quadrature_order, std::vector< ElementBases > &bases, std::vector< LocalBoundary > &local_boundary, std::map<int, InterfaceData> &poly_edge_to_data)
    {
        using std::max;
        assert(!mesh.is_volume());

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

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

        local_boundary.clear();

        // QuadQuadrature quad_quadrature;

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

            SpaceMatrix space;
            NodeMatrix loc_nodes;

            // const int max_local_base =
            build_local_space(mesh, mesh_nodes, e, space, loc_nodes, local_boundary, poly_edge_to_data);
            // n_bases = max(n_bases, max_local_base);

            ElementBases &b=bases[e];
            // quad_quadrature.get_quadrature(quadrature_order, b.quadrature);
            b.set_quadrature([quadrature_order](Quadrature &quad){
                QuadQuadrature quad_quadrature;
                quad_quadrature.get_quadrature(quadrature_order, quad);
            });
            b.bases.resize(9);

            b.set_local_node_from_primitive_func([e](const int primitive_id, const Mesh &mesh)
            {
                Eigen::VectorXi res(3);
                const auto &mesh2d = dynamic_cast<const Mesh2D &>(mesh);
                auto index = mesh2d.get_index_from_face(e);
                int le;
                for(le = 0; le < mesh2d.n_face_vertices(e); ++le)
                {
                    if(index.edge == primitive_id)
                        break;
                    index = mesh2d.next_around_face(index);
                }
                assert(index.edge == primitive_id);

                switch(le)
                {
                    case 3: res << (3*0 + 0), (3*1 + 0), (3*2 + 0); break;
                    case 0: res << (3*0 + 0), (3*0 + 1), (3*0 + 2); break;
                    case 1: res << (3*0 + 2), (3*1 + 2), (3*2 + 2); break;
                    case 2: res << (3*2 + 0), (3*2 + 1), (3*2 + 2); break;
                    default: assert(false);
                }

                return res;
            });

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

            setup_knots_vectors(mesh_nodes, space, h_knots, v_knots);

            // print_local_space(space);

            basis_for_regular_quad(space, loc_nodes, h_knots, v_knots, b);
            basis_for_irregulard_quad(e, mesh, mesh_nodes, space, loc_nodes, h_knots, v_knots, b);
        }

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

        int n_bases = mesh_nodes.n_nodes();

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

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

            b.set_local_node_from_primitive_func([e](const int primitive_id, const Mesh &mesh)
            {
                const auto &mesh2d = dynamic_cast<const Mesh2D &>(mesh);
                auto index = mesh2d.get_index_from_face(e);

                for(int le = 0; le < mesh2d.n_face_vertices(e); ++le)
                {
                    if(index.edge == primitive_id)
                        break;
                    index = mesh2d.next_around_face(index);
                }
                assert(index.edge == primitive_id);

                // const auto indices = FEBasis2d::quadr_quad_edge_local_nodes(mesh2d, index);
                const auto indices = FEBasis2d::quad_edge_local_nodes(2, mesh2d, 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, 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_edge_to_data);
        }

        return n_bases;
    }

    void SplineBasis2d::fit_nodes(const Mesh2D &mesh, const int n_bases, std::vector< ElementBases > &gbases)
    {
        assert(false);
        // const int dim = 2;
        // const int n_constraints =  9;
        // const int n_elements = mesh.n_elements();

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

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

        // Eigen::MatrixXd node_rhs(n_constraints*n_elements, dim);
        // Eigen::MatrixXd samples(n_constraints, dim);
        // polyfem::autogen::q_nodes_2d(2, samples);
        // // for(int i = 0; i < n_constraints; ++i)
        // //     samples.row(i) = FEBasis2d::quadr_quad_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 = FEBasis2d::quadr_quad_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);
        //             }
        //         }
        //     }
        // }
    }

}


//
// $Id$
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef QTOFPEAKPICKERFILTER_H
#define QTOFPEAKPICKERFILTER_H

#include <iostream>
#include <iterator>

#include "pwiz/data/msdata/MSDataFile.hpp"
#include "pwiz/data/msdata/SpectrumIterator.hpp"
#include "pwiz/data/msdata/SpectrumInfo.hpp"
#include "pwiz/data/msdata/SpectrumListWrapper.hpp"
#include "pwiz/utility/findmf/base/ms/peakpickerqtof.hpp"
#include <boost/cstdint.hpp>

namespace ralab{
typedef boost::uint32_t uint32_t;
  /** This is used to write the filtered spectrum list*/
  struct QTOFPeakPickerFilter : pwiz::msdata::SpectrumListWrapper{
    double resolution_;
    double smoothwidth_;
    uint32_t integrationWidth_;
    double intensityThreshold_;
    bool area_; // should area or intensities be determined
	uint32_t maxnumberofpeaks_;

    QTOFPeakPickerFilter(
        const pwiz::msdata::SpectrumListPtr & inner, //!< spectrum list
        double resolution, //!< instrument resolution
        double smoothwidth = 2., //!< smoothwidth
        double integrationWidth = 4, //! integration width
        double intensityThreshold = 10.,
        bool area = true, //!< do you want to store are or intensity
	uint32_t maxnumberofpeaks = 0 //!< max number of peaks returned by picker
        ):SpectrumListWrapper(inner),
      resolution_(resolution),
      smoothwidth_(smoothwidth),
      integrationWidth_(integrationWidth),
      intensityThreshold_(intensityThreshold),
      area_(area),maxnumberofpeaks_(maxnumberofpeaks)
    {
      // add processing methods to the copy of the inner SpectrumList's data processing
      pwiz::msdata::ProcessingMethod method;
      method.order = dp_->processingMethods.size();
      method.userParams.push_back(pwiz::msdata::UserParam("qtof peak picker"));
      if (!dp_->processingMethods.empty())
        method.softwarePtr = dp_->processingMethods[0].softwarePtr;
      dp_->processingMethods.push_back(method);
    }

    pwiz::msdata::SpectrumPtr spectrum( std::size_t index, bool getBinaryData ) const
    {
      pwiz::msdata::SpectrumPtr specptr = inner_->spectrum(index, true);
      //if (!pwiz::msdata::msLevelsToPeakPick_.contains(s->cvParam(MS_ms_level).valueAs<int>()))
      //  return specptr;

      std::vector<pwiz::msdata::CVParam>& cvParams = specptr->cvParams;
      std::vector<pwiz::msdata::CVParam>::iterator itr = std::find(cvParams.begin(), cvParams.end(), pwiz::msdata::MS_profile_spectrum);

      // return non-profile spectra as-is
      // (could have been acquired as centroid, or vendor may have done the centroiding)
      if (itr == cvParams.end())
        return specptr;

      // make sure the spectrum has binary data
      if (!specptr->getMZArray().get() || !specptr->getIntensityArray().get())
        specptr = inner_->spectrum(index, true);

      // replace profile term with centroid term
      std::vector<pwiz::msdata::CVParam>& cvParams2 = specptr->cvParams;
      *(std::find(cvParams2.begin(), cvParams2.end(), pwiz::msdata::MS_profile_spectrum)) = pwiz::msdata::MS_centroid_spectrum;

      try
      {
        pwiz::util::BinaryData<double>& mzs = specptr->getMZArray()->data;
        pwiz::util::BinaryData<double>& intensities = specptr->getIntensityArray()->data;

        /// if empty spectrum return withouth processing
        if(mzs.size() == 0 ){
          return specptr;
        }
        std::pair<double, double> range = std::make_pair(mzs.front(),mzs.back());

        //construct peak picker
        ralab::base::ms::PeakPicker<double, ralab::base::ms::SimplePeakArea > pp(
              resolution_,
              range,
              smoothwidth_,
              integrationWidth_,
              intensityThreshold_,
              area_,
	 maxnumberofpeaks_
              );

        pp( mzs.begin(), mzs.end(), intensities.begin());

        mzs.assign(pp.getPeakMass().begin(),pp.getPeakMass().end());
        intensities.assign(pp.getPeakArea().begin(),
                           pp.getPeakArea().end());
        specptr->defaultArrayLength = mzs.size();

      }
      catch(std::exception& e)
      {
        throw std::runtime_error(std::string("[QTOFPeakPickerFilter] Error filtering intensity data: ") + e.what());
      }
      specptr->dataProcessingPtr = dp_;
      return specptr;
    }
  };
}

#endif // QTOFPEAKPICKERFILTER_H


//==================================================================================================
/*!
  @file

  @copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ALGORITHM_COPY_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_COPY_HPP_INCLUDED

#include <boost/simd/algorithm/transform.hpp>
#include <boost/simd/pack.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Copies the elements in the range, defined by [first, last)
    to another range beginning at out.

    @param first  Beginning of the range of elements
    @param last   End of the range of elements
    @param out    Beginning of the destination range

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

      - return a pointer to the element past the last element copied.

    @par Example:

      @snippet copy.cpp copy

    @par Possible output:

      @snippet copy.txt copy

  **/
  template<typename T> T* copy(T const* first, T const* last, T* out)
  {
    struct local
    {
           T  operator()(T const& x)        const { return x; }
      pack<T> operator()(pack<T> const& x)  const { return x; }
    };

    return boost::simd::transform(first, last, out, local{});
  }
} }

#endif


/**
* Copyright (C) 2020 Xilinx, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License"). You may
* not use this file except in compliance with the License. A copy of the
* License is located at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*/
#ifndef XF_BLAS_MATRIX_HPP
#define XF_BLAS_MATRIX_HPP

#include <iostream>
#include <fstream>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/filter/gzip.hpp>
//#include <boost/iostreams/filter/bzip2.hpp>

class MtxRow {
   private:
    unsigned int m_Row, m_Col;
    double m_Val;

   public:
    MtxRow() : m_Row(0), m_Col(0), m_Val(0) {}
    MtxRow(double p_Val, unsigned int p_Row, unsigned int p_Col) : m_Row(p_Row), m_Col(p_Col), m_Val(p_Val) {}
    unsigned int getRow() { return m_Row; }
    unsigned int getCol() { return m_Col; }
    double getVal() { return m_Val; }
    void scan(std::istream& p_Is) {
        p_Is >> m_Row >> m_Col >> m_Val;
        if ((m_Row <= 0) || (m_Col <= 0)) {
            std::cerr << "  Error: invalid MTX file line row=" << m_Row << " col=" << m_Col << " val=" << m_Val << "\n";
            assert(0);
        }
        // Indices start from 1 in MTX; 0 locally
        m_Row--;
        m_Col--;
    }
    friend bool operator<(MtxRow& a, MtxRow& b) {
        if (a.getRow() < b.getRow()) {
            return (true);
        } else if (a.getRow() == b.getRow()) {
            if (a.getCol() < b.getCol()) {
                return (true);
            }
        }
        return (false);
    }
    struct compareCol {
        bool operator()(MtxRow& a, MtxRow& b) {
            if (a.getCol() < b.getCol()) {
                return (true);
            } else if (a.getCol() == b.getCol()) {
                if (a.getRow() < b.getRow()) {
                    return (true);
                }
            }
            return (false);
        }
    };

    void print(std::ostream& os) {
        os << std::setw(BLAS_FLOAT_WIDTH) << int(getRow()) << " " << std::setw(BLAS_FLOAT_WIDTH) << int(getCol())
           << "    " << std::setw(BLAS_FLOAT_WIDTH) << getVal();
    }
};
inline std::ostream& operator<<(std::ostream& os, MtxRow& p_Val) {
    p_Val.print(os);
    return (os);
}

// Regula Matrix descriptor with data itself stored in caller's space
template <typename T>
class DenseMat {
   private:
    unsigned int m_Rows, m_Cols, m_Ld;
    T* m_Addr;

   public:
    DenseMat() {}
    DenseMat(unsigned int p_Rows, unsigned int p_Cols, unsigned int p_Ld, T* p_Addr)
        : m_Rows(p_Rows), m_Cols(p_Cols), m_Ld(p_Ld), m_Addr(p_Addr) {}
    DenseMat& operator=(const DenseMat& p_Src) {
        assert(p_Src.rows() == rows());
        assert(p_Src.cols() == cols());
        for (unsigned int row = 0; row < m_Rows; ++row) {
            for (unsigned int col = 0; col < m_Cols; ++col) {
                m_Addr[row][col] = p_Src.getVal(row, col);
            }
        }
        return *this;
    }
    inline T& getVal(unsigned int p_Row, unsigned int p_Col) { return m_Addr[p_Row * ld() + p_Col]; }
    inline unsigned int rows() { return m_Rows; }
    inline unsigned int cols() { return m_Cols; }
    inline unsigned int ld() { return m_Ld; }
    void init(unsigned int p_Rows, unsigned int p_Cols, unsigned int p_Ld, T* p_Addr) {
        m_Rows = p_Rows;
        m_Cols = p_Cols;
        m_Ld = p_Ld;
        m_Addr = p_Addr;
    }
    void fillMod(T p_Max, T p_First = 0, bool p_small = false) {
        T l_val = p_First;
        for (unsigned int row = 0; row < m_Rows; ++row) {
            for (unsigned int col = 0; col < ld(); ++col) {
                getVal(row, col) = l_val;
                l_val++;
                l_val %= p_Max;
            }
        }
    }

    void fillModFromFile(std::string l_fs) {
        std::ifstream inputFile(l_fs);
        T l_val;
        if (inputFile.good()) {
            for (unsigned int row = 0; row < m_Rows; ++row) {
                for (unsigned int col = 0; col < ld(); ++col) {
                    inputFile >> l_val;
                    getVal(row, col) = l_val;
                }
            }
        } else {
            std::cout << "no file no loading!\n";
        }
    }

    void fillModFromBin(std::string l_fs) {
        std::ifstream inFile;
        inFile.open(l_fs);
        if (inFile.is_open()) {
            inFile.read((char*)m_Addr, sizeof(T) * m_Rows * ld());
            inFile.close();
        } else {
            std::cout << "no file no loading!\n";
        }
    }

    void fillFromFile(std::istream& p_Is) {
        T l_val;
        for (unsigned int row = 0; row < m_Rows; ++row) {
            for (unsigned int col = 0; col < ld(); ++col) {
                p_Is >> l_val;
                getVal(row, col) = l_val;
            }
        }
    }
#if BLAS_runTransp == 1
    // Golden comparsion functions for transp engine
    void transpose(DenseMat& p_A) {
        for (unsigned int row = 0; row < rows(); ++row) {
            for (unsigned int col = 0; col < cols(); ++col) {
                getVal(row, col) = p_A.getVal(col, row);
            }
        }
        std::swap(m_Rows, m_Cols);
    }
    void transposeGva(DenseMat& p_A, unsigned int p_rowEdgeWidth, unsigned int p_colEdgeWidth) {
        unsigned int l_pos = 0;
        for (unsigned int rowBlock = 0; rowBlock < p_A.rows() / p_rowEdgeWidth; ++rowBlock) {
            for (unsigned int colBlock = 0; colBlock < p_A.cols() / p_colEdgeWidth; ++colBlock) {
                for (unsigned int col = 0; col < p_colEdgeWidth; ++col) {
                    for (unsigned int row = 0; row < p_rowEdgeWidth; ++row) {
                        getVal(l_pos / cols(), l_pos % cols()) =
                            p_A.getVal(row + rowBlock * p_rowEdgeWidth, col + colBlock * p_colEdgeWidth);
                        l_pos++;
                    }
                    // std::cout << "DEBUG transposeGva step " << *this << "\n";
                }
            }
        }
        std::swap(m_Rows, m_Cols);
    }
#endif
    void print(std::ostream& os) {
        os << m_Rows << "x" << m_Cols << " Ld=" << m_Ld << "\n";
        unsigned int l_cols = cols(); // normal matrix
                                      // ld();; // parent matrix (within Ld
        for (unsigned int row = 0; row < rows(); ++row) {
            for (unsigned int col = 0; col < l_cols; ++col) {
                os << std::setw(BLAS_FLOAT_WIDTH) << int(getVal(row, col)) << " ";
            }
            os << "\n";
        }
    }
    bool cmp(float p_TolRel, float p_TolAbs, DenseMat& p_Ref) {
        bool ok = true;
        unsigned int l_numExactMatches = 0, l_numMismatches = 0;
        for (unsigned int row = 0; row < rows(); ++row) {
            for (unsigned int col = 0; col < cols(); ++col) {
                std::string l_Prefix = "      row " + std::to_string(row) + " col " + std::to_string(col);
                T v = getVal(row, col);
                T vRef = p_Ref.getVal(row, col);
                bool l_exactMatch = false;
                bool l_ok = xf::blas::cmpVal<T>(p_TolRel, p_TolAbs, vRef, v, l_Prefix, l_exactMatch, 1);
                ok = ok && l_ok;
                if (l_exactMatch) {
                    l_numExactMatches++;
                }
                if (!l_ok) {
                    l_numMismatches++;
                }
            }
        }
        unsigned int l_total = rows() * cols();
        unsigned int l_withinTolerance = l_total - l_numExactMatches - l_numMismatches;
        std::cout << "  Compared " << l_total << " values:"
                  << "  exact match " << l_numExactMatches << "  within tolerance " << l_withinTolerance
                  << "  mismatch " << l_numMismatches << "\n";
        return (ok);
    }
};
template <typename T1>
std::ostream& operator<<(std::ostream& os, DenseMat<T1>& p_Val) {
    p_Val.print(os);
    return (os);
}

/*Type define and float specialization*/

typedef DenseMat<float> MatType_ForFloat;

typedef DenseMat<BLAS_dataType> MatType;

template <>
void MatType_ForFloat::fillMod(float p_Max, float p_First, bool p_small) {
    if (p_small) {
        // for easier debug of matrices
        float l_val = p_First;
        for (unsigned int row = 0; row < m_Rows; ++row) {
            for (unsigned int col = 0; col < ld(); ++col) {
                getVal(row, col) = l_val;
                l_val += 0.01;
                if (l_val > p_Max) {
                    l_val -= p_Max;
                }
            }
        }
    } else {
        // for better float robustness of large matrices
        float l_val = 1.0;
        float l_step = 0.3;
        float l_drift = 0.00001;
        float l_sign = 1;
        for (unsigned int row = 0; row < m_Rows; ++row) {
            for (unsigned int col = 0; col < ld(); ++col) {
                getVal(row, col) = l_val;
                l_val += l_sign * l_step;
                l_step += l_drift;
                l_sign = -l_sign;
                if (l_val > p_Max) {
                    l_val -= p_Max;
                }
            }
        }
    }
}
template <>
void MatType_ForFloat::print(std::ostream& os) {
    os << m_Rows << "x" << m_Cols << " Ld=" << m_Ld << "\n";
    unsigned int l_cols = cols(); // normal matrix
                                  // ld();; // parent matrix (within Ld
    for (unsigned int row = 0; row < rows(); ++row) {
        for (unsigned int col = 0; col < l_cols; ++col) {
            os << std::setw(BLAS_FLOAT_WIDTH) << std::fixed << std::setprecision(3) << getVal(row, col) << " ";
        }
        os << "\n";
    }
}

#endif


/*
  [auto_generated]
  boost/numeric/odeint/iterator/detail/const_step_iterator_impl.hpp

  [begin_description]
  tba.
  [end_description]

  Copyright 2013 Karsten Ahnert
  Copyright 2013 Mario Mulansky

  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_NUMERIC_ODEINT_ITERATOR_DETAIL_CONST_STEP_ITERATOR_IMPL_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_ITERATOR_DETAIL_CONST_STEP_ITERATOR_IMPL_HPP_DEFINED

#include <boost/numeric/odeint/iterator/detail/ode_iterator_base.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>

namespace boost {
namespace numeric {
namespace odeint {

    template< class Iterator , class Stepper , class System , class State , typename Tag , class StepperTag >
    class const_step_iterator_impl;

    /*
     * Specilization for steppers and error steppers
     */
    template< class Iterator , class Stepper , class System , class State , typename Tag >
    class const_step_iterator_impl< Iterator , Stepper , System , State , Tag , stepper_tag >
        : public detail::ode_iterator_base< Iterator , Stepper , System , State , Tag >
    {
    private:

        typedef Stepper stepper_type;
        typedef System system_type;
        typedef typename boost::numeric::odeint::unwrap_reference< stepper_type >::type unwrapped_stepper_type;
        typedef State state_type;
        typedef typename traits::time_type< stepper_type >::type time_type;
        typedef typename traits::value_type< stepper_type >::type ode_value_type;
        #ifndef DOXYGEN_SKIP
        typedef detail::ode_iterator_base< Iterator , Stepper , System , State , Tag > base_type;
        #endif

    public:

        /**
         * \brief Constructs a const_step_iterator. This constructor should be used to construct the begin iterator.
         *
         * \param stepper The stepper to use during the iteration.
         * \param sys The system function (ODE) to solve.
         * \param s The initial state. const_step_iterator stores a reference of s and changes its value during the iteration.
         * \param t The initial time.
         * \param t_end The end time, at which the iteration should stop.
         * \param dt The initial time step.
         */
        const_step_iterator_impl( stepper_type stepper , system_type sys , state_type &s , time_type t , time_type t_end , time_type dt )
            : base_type( stepper , sys , t , dt ) , m_t_start( t ) , m_t_end( t_end ) , m_state( &s ) , m_step( 0 )
        {
            if( detail::less_with_sign( this->m_t_end , this->m_t , this->m_dt ) )
                this->m_at_end = true;
        }

        /**
         * \brief Constructs a const_step_iterator. This constructor should be used to construct the end iterator.
         *
         * \param stepper The stepper to use during the iteration.
         * \param sys The system function (ODE) to solve.
         * \param s The initial state. const_step_iterator stores a reference of s and changes its value during the iteration.
         */
        const_step_iterator_impl( stepper_type stepper , system_type sys , state_type &s )
            : base_type( stepper , sys ) { }

    protected:

        friend class boost::iterator_core_access;

        void increment()
        {
            if( detail::less_eq_with_sign( static_cast<time_type>(this->m_t+this->m_dt) ,
                                           this->m_t_end , this->m_dt ) )
            {
                unwrapped_stepper_type &stepper = this->m_stepper;
                stepper.do_step( this->m_system , *this->m_state , this->m_t , this->m_dt );
                // use integer to compute current time to reduce roundoff errors
                this->m_step++;
                this->m_t = this->m_t_start + static_cast< typename unit_value_type<time_type>::type >(this->m_step)*this->m_dt;
            } else {
                this->m_at_end = true;
            }
        }

    public:
        const state_type& get_state() const
        {
            return *m_state;
        }

    private:
        time_type m_t_start;
        time_type m_t_end;
        state_type* m_state;
        size_t m_step;

    };

    /*
     * Specilization for dense output stepper
     */
    /**
     * \brief ODE Iterator with constant step size. The value type of this iterator is the state type of the stepper.
     *
     * Implements an ODE iterator solving the ODE with constant steps. Uses dense-output steppers.
     * const_step_iterator is a model of single-pass iterator.
     *
     * The value type of this iterator is the state type of the stepper. Hence one can only access the state and not the current time.
     *
     * \tparam Stepper The stepper type which should be used during the iteration.
     * \tparam System The type of the system function (ODE) which should be solved.
     */
    template< class Iterator , class Stepper , class System , class State , typename Tag >
    class const_step_iterator_impl< Iterator , Stepper , System , State , Tag , dense_output_stepper_tag >
        : public detail::ode_iterator_base< Iterator , Stepper , System , State , Tag >
    {
    private:

        typedef Stepper stepper_type;
        typedef System system_type;
        typedef typename boost::numeric::odeint::unwrap_reference< stepper_type >::type unwrapped_stepper_type;
        typedef State state_type;
        typedef typename traits::time_type< stepper_type >::type time_type;
        typedef typename traits::value_type< stepper_type >::type ode_value_type;
        #ifndef DOXYGEN_SKIP
        typedef detail::ode_iterator_base< Iterator , Stepper , System , State , Tag > base_type;
        #endif

    public:

        /**
         * \brief Constructs a const_step_iterator. This constructor should be used to construct the begin iterator.
         *
         * \param stepper The stepper to use during the iteration.
         * \param sys The system function (ODE) to solve.
         * \param s The initial state. const_step_iterator stores a reference of s and changes its value during the iteration.
         * \param t The initial time.
         * \param t_end The end time, at which the iteration should stop.
         * \param dt The initial time step.
         */
        const_step_iterator_impl( stepper_type stepper , system_type sys , state_type &s , time_type t , time_type t_end , time_type dt )
            : base_type( stepper , sys , t , dt ) , m_t_start( t ) , m_t_end( t_end ) , m_state( &s ) , m_step( 0 )
        {
            if( detail::less_eq_with_sign( this->m_t , this->m_t_end , this->m_dt ) )
            {
                unwrapped_stepper_type &st = this->m_stepper;
                st.initialize( * ( this->m_state ) , this->m_t , this->m_dt );
            } else {
                this->m_at_end = true;
            }
        }

        /**
         * \brief Constructs a const_step_iterator. This constructor should be used to construct the end iterator.
         *
         * \param stepper The stepper to use during the iteration.
         * \param sys The system function (ODE) to solve.
         * \param s The initial state. const_step_iterator stores a reference of s and changes its value during the iteration.
         */
        const_step_iterator_impl( stepper_type stepper , system_type sys , state_type &s )
            : base_type( stepper , sys ) , m_state( &s )
        {
        }

    protected:

        friend class boost::iterator_core_access;

        void increment( void )
        {
            if( detail::less_eq_with_sign( static_cast<time_type>(this->m_t+this->m_dt) ,
                                           this->m_t_end , this->m_dt ) )
            {
                unwrapped_stepper_type &stepper = this->m_stepper;
                // use integer to compute current time to reduce roundoff errors
                this->m_step++;
                this->m_t = this->m_t_start + static_cast< typename unit_value_type<time_type>::type >(this->m_step)*this->m_dt;
                while( detail::less_with_sign( stepper.current_time() , this->m_t ,
                       stepper.current_time_step() ) )
                {
                    stepper.do_step( this->m_system );
                }
                stepper.calc_state( this->m_t , *( this->m_state ) );
            } else {
                this->m_at_end = true;
            }
        }

    public:
        const state_type& get_state() const
        {
            return *m_state;
        }

    private:
        time_type m_t_start;
        time_type m_t_end;
        state_type* m_state;
        size_t m_step;
    };

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_ITERATOR_DETAIL_CONST_STEP_ITERATOR_IMPL_HPP_DEFINED


#pragma once

#include <armadillo>

#include "heattransferstate.hpp"

/*!
 * \brief The HeatTransferBase class is an abstract class, the base class for all
 * heat transfer implementations. It mostly just defines a stencil for the
 * evaluate() function and the HeatTransferState class.
 */
class HeatTransferBase
{
public:
    //! Have to declare virtual destructor to avoid compiler warnings.
    //! Only declared here, to avoid the inline compiler-generated default destructor.
    virtual ~HeatTransferBase();

    // pure virtual method
    // returns q [W/m2] = U*(gasTemperature - ambientTemperature)
    /*!
     * \brief Evaluate heat transfer.
     * \param current Current heat transfer state.
     * \param timeStep Time step [s]
     * \param ambientTemperature Ambient temperature [K]
     * \param gasPressure Gas pressure [Pa]
     * \param gasTemperature Gas temperature [K]
     * \param gasReynoldsNumber Reynolds number of gas [-]
     * \param gasHeatCapacityConstantPressure Heat capacity (\f$c_p\f$) [J/kg K]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return HeatTransferState instance with heat flux and updated temperatures.
     */
    virtual HeatTransferState evaluate(
            const HeatTransferState& current,
            const double timeStep,
            const double ambientTemperature,
            const double gasPressure,
            const double gasTemperature,
            const double gasReynoldsNumber,
            const double gasHeatCapacityConstantPressure,
            const double gasViscosity) const = 0;

    /*!
     * \brief Make instance of HeatTransferState from heat flux.
     *
     * This is meant to be overriden in more advanced heat transfer
     * implementations that that use the optional temperature property.
     *
     * \param heatFlux Heat flux [W/m2]
     * \return HeatTransferState instance.
     */
    virtual HeatTransferState makeState(const double heatFlux) const;

    /*!
     * \brief Make instance of HeatTransferState from heat flux. Overload.
     *
     * This version just calls HeatTransferBase::makeState(const double), but more
     * advanced implementations should initialize the temperature property from
     * the two temperature arguments.
     *
     * \param heatFlux Heat flux [W/m2]
     * \param ambientTemperature Ambient temperature [K]
     * \param gasTemperature Gas temperature [K]
     * \return HeatTransferState instance with temperature.
     */
    virtual HeatTransferState makeState(
            const double heatFlux,
            const double gasTemperature,
            const double ambientTemperature) const;
};


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Copyright (c) 2010-2015 Illumina, Inc.
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.

// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

/**
 *  \brief Test cases for popcount / bit ops
 *
 *
 * \file test_popcount.cpp
 * \author Peter Krusche
 * \email pkrusche@illumina.com
 *
 */

#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>

#include <boost/filesystem.hpp>
#include <boost/filesystem/path.hpp>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <bitset>

#include "helpers/Popcount.hh"

BOOST_AUTO_TEST_CASE(testPopcount)
{
    BOOST_CHECK_EQUAL(popcount64(3), 2);
    BOOST_CHECK_EQUAL(popcount64(3 << 10), 2);
    BOOST_CHECK_EQUAL(popcount64(255l << 37), 8);
    BOOST_CHECK_EQUAL(popcount64(-1), 64);
}

BOOST_AUTO_TEST_CASE(testPopcountPermutations)
{
    uint64_t threebits = 0x70;
    typedef std::bitset<64> bin;

//    std::cerr << std::hex << threebits << " / " << bin(threebits) << std::endl;
    threebits = next_permutation(threebits);
    BOOST_CHECK_EQUAL(threebits, (uint64_t)0x83);
//    std::cerr << std::hex << threebits << " / " << bin(threebits) << std::endl;
    threebits = next_permutation(threebits);
    BOOST_CHECK_EQUAL(threebits, (uint64_t)0x85);
//    std::cerr << std::hex << threebits << " / " << bin(threebits) << std::endl;
    threebits = next_permutation(threebits);
    BOOST_CHECK_EQUAL(threebits, (uint64_t)0x86);
//    std::cerr << std::hex << threebits << " / " << bin(threebits) << std::endl;

    threebits = 0x07;
    // 0111 ->
    // 1011 = 8+2+1 = 11
    // 1101 = 8+4+1 = 13
    // 1110 = 8+4+2 = 14
    threebits = next_permutation(threebits);
    BOOST_CHECK_EQUAL(threebits, (uint64_t)0xb);
    threebits = next_permutation(threebits);
    BOOST_CHECK_EQUAL(threebits, (uint64_t)0xd);
    threebits = next_permutation(threebits);
    BOOST_CHECK_EQUAL(threebits, (uint64_t)0xe);
}

BOOST_AUTO_TEST_CASE(testPopcountPermutationsRandom)
{
    typedef std::bitset<64> bin;
    for(int n_test = 0; n_test < 1000; ++n_test)
    {
        uint64_t value = rand64();
        int initial_popcount = popcount64(value);

//        std::cerr << "Test: " << n_test << " : " << std::hex << value << " / " << bin(value) << std::endl;

        uint64_t value_copy = value;
        int popcount_test = 0;
        while(value_copy != 0)
        {
            if(value_copy & 1)
            {
                ++popcount_test;
            }
            value_copy >>= 1;
        }

        BOOST_CHECK_EQUAL(popcount_test, initial_popcount);

        for(int n_perm = 0; n_perm < 1000; ++n_perm)
        {
            value = next_permutation(value);
            BOOST_CHECK_EQUAL(popcount64(value), initial_popcount);
//            std::cerr << "Test: " << n_test << " : " << std::hex << value << " / " << bin(value) << std::endl;
        }
    }
}


#ifndef ATM_BRANCHING_SUBSTRINGS_HPP
#define ATM_BRANCHING_SUBSTRINGS_HPP

#include <cmath>
#include <map>
#include <vector>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/iterator.hpp>
#include <boost/range/sub_range.hpp>
#include <boost/range/value_type.hpp>
#include "sast/sast.hpp"

namespace atm {

template <class RandomAccessRange, class Index>
struct branching_substrings {
    using range_type = RandomAccessRange;
    using char_type = typename boost::range_value<RandomAccessRange>::type;
    using index_type = Index;

protected:
    using sast_type = sast::sast<RandomAccessRange, index_type>;

public:
    struct substr {
        using iterator       = typename boost::range_iterator<RandomAccessRange>::type;
        using const_iterator = typename boost::range_const_iterator<RandomAccessRange>::type;

        index_type pos()           const { return i_->pos(); }
        boost::sub_range<const std::vector<index_type>> allpos() const { return i_->allpos(); }
        index_type length()        const { return i_->length(); }
        index_type frequency()     const { return i_->frequency(); }
        double     spurity()       const { return parent_->strict_purity(i_); }
        double     lpurity()       const { return parent_->loose_purity(i_); }
        double     luniversality() const { return parent_->left_universality(i_); }
        double     runiversality() const { return parent_->right_universality(i_); }

        iterator begin() { return boost::begin(parent_->input_) + pos(); }
        iterator end()   { return boost::begin(parent_->input_) + pos() + length(); }
        const_iterator begin() const { return boost::const_begin(parent_->input_) + pos(); }
        const_iterator end()   const { return boost::const_begin(parent_->input_) + pos() + length(); }

        substr(const branching_substrings* parent, typename sast_type::const_iterator i)
            : parent_(parent), i_(i)
        {}

    private:
        const branching_substrings* parent_;
        typename sast_type::const_iterator i_;
    };

private:
    template <class> struct substring_iterator;

public:
    using iterator       = substring_iterator<substr>;
    using const_iterator = substring_iterator<const substr>;

    branching_substrings(const sast_type& sast)
        : sast_(sast),
          input_(sast_.input()),
          count_(sast_.size(), 0), // initialize the count table with an "undefined" value.
          recip_(sast_.size(), 0)  // 正数は計算結果、それ以外は未計算を表わす。
    {}

    iterator begin() { return iterator(this, sast_.begin()); }
    iterator end()   { return iterator(this, sast_.end()); }
    const_iterator begin() const { return const_iterator(this, sast_.begin()); }
    const_iterator end()   const { return const_iterator(this, sast_.end()); }

private:
    template <class Value>
    struct substring_iterator
        : public boost::iterator_facade<
            substring_iterator<Value>,
            Value,
            boost::random_access_traversal_tag,
            Value,
            int>
    {
        substring_iterator()
            : parent_(0), i_()
        {}

        substring_iterator(const branching_substrings* parent, typename sast_type::const_iterator i)
            : parent_(parent), i_(i)
        {}

        template <class OtherValue>
        substring_iterator(substring_iterator<OtherValue> const& other)
            : parent_(other.parent_), i_(other.i_)
        {}

    private:
        friend class boost::iterator_core_access;
        template <class> friend struct substring_iterator;

        void increment() { ++i_; }

        void decrement() { --i_; }

        void advance(int n) { i_ += n; }

        int distance_to(const substring_iterator<Value>& other) const { return other.i_ - this->i_; }

        template <class OtherValue>
        bool equal(const substring_iterator<OtherValue>& other) const {
            return this->parent_ == other.parent_ && this->i_ == other.i_;
        }

        Value dereference() const {
            return substr(parent_, i_);
        }

        const branching_substrings* parent_;
        typename sast_type::const_iterator i_;
    };

protected:
    uint64_t get_count(typename sast_type::const_iterator n) const {
        const int i = n - sast_.begin();

        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
        // substrと同じ出現回数のsub-substrの数count[i]を数える。
        if (count_[i] > 0) {
            return count_[i];
        }
        else {
            const auto freq_substr = n->frequency();

            // substrと同じ出現回数のsub-substrを数える。
            uint64_t count = 0;
            {
                // substrの末尾を0文字以上削って得られるsub-substrについて考える。
                // ノードiに対応する部分文字列をsubstr[i]とすると、substr[i]の末尾
                // を削って得られる部分文字列の内で、substr[i]と同じ頻度をもつもの
                // はsuffix tree上ではノードiにまとめられている
                // （分岐が無い <=> 頻度が同じ）。

                // ノードiの親ノードjを見つける。
                const auto p = n.parent();

                // substrの末尾を0文字以上削って得られるsub-substrの内で、出現
                // 回数がsubstrと同じものの数はd[i] - d[j]である。
                count += n->length() - p->length();;
            }
            {
                // substrの先頭を1文字以上削ったsub-substrを考える。
                const auto m = n.suffix();
                const auto freq_subsubstr = m->frequency();
                if (freq_subsubstr == freq_substr) {
                    count += get_count(m);
                }
            }

            // memoize
            count_[i] = count;
            return count_[i];
        }
    }

    double strict_purity(typename sast_type::const_iterator n) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
        const auto len_substr  = n->length();

        // substrと同じ出現回数のsub-substrを数える。
        const uint64_t count = get_count(n);

        // strict purity of substr
        const uint64_t num_subsubstrs = static_cast<uint64_t>(1 + len_substr) * len_substr / 2;
        const double spurity = static_cast<double>(count) / num_subsubstrs;

        return spurity;
    }

    double get_reciprocal(typename sast_type::const_iterator n) const {
        const int i = n - sast_.begin();

        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
        // substrの全部分文字列の頻度の逆数の総和recip[i]を求める。
        if (recip_[i] > 0) {
            return recip_[i];
        }
        else {
            double recip = 0;
            {
                // substrの末尾を0文字以上削って得られるsub-substrについて考える。
                for (auto m = n, p = n.parent(); m != sast_.end(); m = p, p = p.parent()) {
                    const auto num_subsubstrs_of_same_frequency = m->length() - p->length();
                    const auto freq_subsubstr = m->frequency();
                    const double r = 1.0 / freq_subsubstr;
                    recip += num_subsubstrs_of_same_frequency * r;
                }
            }
            {
                // substrの先頭を1文字以上削ったsub-substrを考える。
                const auto m = n.suffix();
                if (m != sast_.end()) {
                    recip += get_reciprocal(m);
                }
            }

            // memoize
            recip_[i] = recip;
            return recip_[i];
        }
    }

    double loose_purity(typename sast_type::const_iterator n) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
        const auto freq_substr = n->frequency();
        const auto len_substr  = n->length();

        const double rel = freq_substr * get_reciprocal(n);

        // loose purity of substr
        const uint64_t num_subsubstrs = static_cast<uint64_t>(1 + len_substr) * len_substr / 2;
        const double lpurity = rel / num_subsubstrs;

        return lpurity;
    }

    std::map<char_type, int> left_extensions(typename sast_type::const_iterator n) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。

        std::map<char_type, int> char_dist;
        for (auto pos : n->allpos()) {
            const auto& c = boost::const_begin(input_)[pos - 1];
            char_dist[c] += 1;
        }

        return char_dist;
    }

    std::map<char_type, int> right_extensions(typename sast_type::const_iterator n) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
        const auto len_substr = n->length();

        std::map<char_type, int> char_dist;
        for (auto pos : n->allpos()) {
            const auto& c = boost::const_begin(input_)[pos + len_substr];
            char_dist[c] += 1;
        }

        return char_dist;
    }

    double left_universality(typename sast_type::const_iterator n) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
        const auto freq_substr = n->frequency();

        std::map<char_type, int> char_dist = left_extensions(n);

        double e = 0;
        for (const auto& kv : char_dist) {
            const double p = static_cast<double>(kv.second) / freq_substr;
            e += -p * std::log(p);
        }
        const double u = 1 - std::exp(-e);

        return u;
    }

    double right_universality(typename sast_type::const_iterator n) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
        const auto freq_substr = n->frequency();

        std::map<char_type, int> char_dist = right_extensions(n);

        double e = 0;
        for (const auto& kv : char_dist) {
            const double p = static_cast<double>(kv.second) / freq_substr;
            e += -p * std::log(p);
        }
        const double u = 1 - std::exp(-e);

        return u;
    }

    const sast_type& sast_;
    const RandomAccessRange& input_;
    mutable std::vector<uint64_t> count_;
    mutable std::vector<double>   recip_;
};

}  // namespace atm

#endif  /* ATM_BRANCHING_SUBSTRINGS_HPP */


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2018 Sylko Olzscher
 *
 */
#ifndef CYNG_NUMERIC_CAST_HPP
#define CYNG_NUMERIC_CAST_HPP

#include <type_traits>
#include <limits.h>
#include <cyng/value_cast.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace cyng
{
    template < typename T >
    T string_to_numeric(object const& obj, T const& def) noexcept
    {
        static_assert(std::is_arithmetic<T>::value, "only arithmetic types supported");
        try {
            const std::string defll = std::to_string(def);
            const std::string inp = cyng::value_cast<std::string>(obj, defll);
            return (std::is_unsigned<T>::value)
                ? std::stoull(inp)
                : std::stoll(inp)
                ;
        }
        catch (std::exception const&) {
        }
        return def;
    }

	/**
	 * If the hidden value is not of type T the function returns the default value.
	 *
	 * @tparam T cast object
	 * @param def default value
	 * @return a const reference const& of the value or the default value if
	 */
	template < typename T >
	T numeric_cast(object const& obj, T const& def) noexcept
	{
		static_assert(std::is_arithmetic<T>::value, "only arithmetic types supported");
		try {
			switch (obj.get_class().tag())	{

				case TC_NULL:	return static_cast<T>(0);

				case TC_BOOL:
					return value_cast(obj, false)
					? std::numeric_limits<T>::lowest()
					: std::numeric_limits<T>::max()
					;

				case TC_CHAR:	return boost::numeric_cast<T>(value_cast<char>(obj, 0));
				case TC_UINT8:	return boost::numeric_cast<T>(value_cast<std::uint8_t>(obj, 0));
				case TC_UINT16:	return boost::numeric_cast<T>(value_cast<std::uint16_t>(obj, 0));
				case TC_UINT32:	return boost::numeric_cast<T>(value_cast<std::uint32_t>(obj, 0));
				case TC_UINT64:	return boost::numeric_cast<T>(value_cast<std::uint64_t>(obj, 0));
				case TC_INT8:	return boost::numeric_cast<T>(value_cast<std::int8_t>(obj, 0));
				case TC_INT16:	return boost::numeric_cast<T>(value_cast<std::int16_t>(obj, 0));
				case TC_INT32:	return boost::numeric_cast<T>(value_cast<std::int32_t>(obj, 0));
				case TC_INT64:	return boost::numeric_cast<T>(value_cast<std::int64_t>(obj, 0));

				case TC_FLOAT:		return static_cast<T>(value_cast<float>(obj, 0.0));
				case TC_DOUBLE:		return static_cast<T>(value_cast<double>(obj, 0.0));
				case TC_FLOAT80:	return static_cast<T>(value_cast<long double>(obj, 0.0));
				case TC_STRING:		return string_to_numeric<T>(obj, def);
				default:
					break;
			}
		}
		catch (boost::numeric::positive_overflow const& ex)	{
			std::cerr
				<< "***Warning: "
				<< ex.what()
				<< std::endl
				;
		}
		catch (boost::numeric::negative_overflow const& ex)	{
			std::cerr
				<< "***Warning: "
				<< ex.what()
				<< std::endl
				;
		}
		catch (boost::numeric::bad_numeric_cast const& ex)	{
			std::cerr
				<< "***Warning: "
				<< ex.what()
				<< std::endl
				;
		}
		catch (std::invalid_argument const& ex)	{
			std::cerr
				<< "***Warning: "
				<< ex.what()
				<< std::endl
				;
		}
		return def;
	}
}

#endif //	CYNG_NUMERIC_CAST_HPP


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE PipelineQuadraticEpsilonTest1

#include <../random_util.hpp>
#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <map>
#include <tuple>
#include <vector>

#define protected public
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/ipucopy.hpp>
#include <popart/op/l1.hpp>
#include <popart/op/restore.hpp>
#include <popart/op/stash.hpp>
#include <popart/optimizer.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>
#undef protected

#include <popart/session.hpp>

// A test of quadratic convergence for continuous pipelining.
// Comparison between
// {exact, continuous without recompute, continuous with recompute}
// for a model which is a chain of 6 of these:
//
//  -+
//   |
//   +-In------+    W
//     |       |    |
//    sin      |    |
//      \      |    |
//       \     |    |
//         add      |
//           \      |
//            \     |
//             \    |
//               add
//                |
//                |
//               Out
//                |
//                +-+
//                  |
//
// 2 layers on each of 3 IPUs.

BOOST_AUTO_TEST_CASE(QuadraticEpsilonTest1) {

  // dump poplar execution traces to file
  bool dumpPoplarTrace = false;

  // Store the initial and final weights from training
  class WeightPair {
  public:
    using Wts = std::vector<std::vector<float>>;
    WeightPair(Wts s, Wts e) : start(s), end(e) {}
    Wts start;
    Wts end;
  };

  enum class RunType {
    SingleDevice = 0,    // exact SGD on a single device
    ContinuousPipe,      // Continuous (in-exact) pipelining
    ContinuousRecompPipe // Continuous pipelined
  };

  using namespace popart;

  // Return a vector of {initial weights, updated weights},
  // one for each {learnRate, nSteps}.
  // Note: This call only compiles one engine, so adding new learning
  // rates does not increase compile time too considerably
  auto getResults = [dumpPoplarTrace](RunType rt,
                                      const std::vector<float> &learnRates,
                                      const std::vector<int64_t> &nSteps_s,
                                      int accumulationFactor) {
    if (learnRates.size() != nSteps_s.size()) {
      throw error("learnRates and nSteps_s not of same size, invalid test");
    }

    int microBatchesPerStep = 20;
    if (microBatchesPerStep % accumulationFactor != 0) {
      throw error(
          "accumulation factor {} is not a factor of microBatchesPerStep {}",
          accumulationFactor,
          microBatchesPerStep);
    }

    // The number of weight updates per step
    int64_t batchesPerStep = microBatchesPerStep / accumulationFactor;

    int seed = 1011;
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdis(-1.f, +1.f);
    int64_t microBatchSize = 3;
    int64_t sampleHeight   = 8;
    std::vector<int64_t> sampleShape{sampleHeight, sampleHeight};
    std::vector<int64_t> weightShape = sampleShape;

    // The input "image" processed by the network has this shape
    std::vector<int64_t> microBatchShape{
        microBatchSize, sampleHeight, sampleHeight};

    TensorInfo sampleInfo{"FLOAT", sampleShape};
    TensorInfo weightInfo = sampleInfo;
    TensorInfo microBatchInfo{"FLOAT", microBatchShape};
    int64_t sampleElms{sampleHeight * sampleHeight};

    // number of "pixels" in a micro-batch
    int64_t microBatchElms = sampleElms * microBatchSize;

    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();
    auto input0      = builder->addInputTensor(microBatchInfo, "0tupni");

    // Storage of necessary weights, ids, and pointers; for all layers.
    std::vector<std::vector<float>> allWeights;
    std::vector<ConstVoidData> allWeightCvds;
    std::vector<TensorId> allWeightIds;
    std::vector<TensorId> actIds;
    std::vector<std::vector<float>> w_readbacks;
    WeightsIO weightsRead;

    // For the 2 pipeline cases, use multiple IPUs.
    int nIPUs = (rt != RunType::SingleDevice ? 3 : 1);

    auto addLayer = [&allWeights,
                     &allWeightCvds,
                     &allWeightIds,
                     &actIds,
                     sampleElms,
                     &weightsRead,
                     weightInfo,
                     sampleInfo,
                     &fdis,
                     &eng,
                     &builder,
                     &aiOnnx,
                     &w_readbacks](TensorId actInId, int vgid) {
      // The weights which will be readback for this layer are stored here
      w_readbacks.push_back(std::vector<float>(sampleElms, -99.0f));

      // The initializer weights for this layer are stored here
      allWeights.push_back(std::vector<float>(sampleElms, 0));

      // initialize weights for this layer
      for (auto &x : allWeights.back()) {
        x = 100 * fdis(eng);
      }

      // store pointers and IDs
      allWeightCvds.push_back({allWeights.back().data(), sampleInfo});
      allWeightIds.push_back(
          builder->addInitializedInputTensor(allWeightCvds.back()));

      //
      // add weights and apply small non-linear bifurcation:
      // x -> (x + sin(x) + w)
      //

      auto act0 = aiOnnx.sin({actInId});
      builder->virtualGraph(act0, vgid);

      act0 = aiOnnx.add({act0, actInId});
      builder->virtualGraph(act0, vgid);
      // To prevent the cos Ops (grad of sin) from running in the fwd pass, we
      // must prevent the adds from being inplace. This ensures the stashing we
      // need to test
      builder->setInplacePreferences(
          act0, {{"AddLhsInplace", -100.0f}, {"AddRhsInplace", -100.0f}});

      act0 = aiOnnx.add({act0, allWeightIds.back()});
      builder->virtualGraph(act0, vgid);

      actIds.push_back(act0);
      weightsRead.insert(allWeightIds.back(),
                         {w_readbacks.back().data(), weightInfo});
    };

    int nLayers = 6;
    addLayer(input0, 0);
    for (int i = 1; i < nLayers; ++i) {
      addLayer(actIds.back(), (i * nIPUs) / nLayers);
    }
    TensorId actFinal = actIds.back();

    SessionOptions userOptions;
    std::map<std::string, std::string> deviceOpts{
        {"numIPUs", std::to_string(nIPUs)}};

    userOptions.virtualGraphMode = VirtualGraphMode::Manual;

    if (dumpPoplarTrace) {
      userOptions.reportOptions.insert({"showExecutionSteps", "true"});
    }

    if (rt != RunType::SingleDevice) {
      userOptions.enablePipelining = true;
    }

    if (rt == RunType::ContinuousRecompPipe) {
      userOptions.autoRecomputation = RecomputationType::Standard;
    }

    if (accumulationFactor > 1) {
      userOptions.accumulationFactor         = accumulationFactor;
      userOptions.enableGradientAccumulation = true;
    } else {
      userOptions.enableGradientAccumulation = false;
    }

    float lambda = 0.1;
    auto l1      = builder->aiGraphcoreOpset1().l1loss(
        {actFinal}, 0.1, ReductionType::Sum);
    builder->virtualGraph(l1, nIPUs - 1);
    auto proto    = builder->getModelProto();
    auto dataFlow = DataFlow(batchesPerStep);

    // The learning rate will be adjusted to the correct value at runtime
    auto optimizer = SGD({{"defaultLearningRate", {10000., false}}});

    // number of "pixels" in a step
    int64_t stepDataElms = accumulationFactor * microBatchElms * batchesPerStep;

    auto device = createTestDevice(TEST_TARGET, nIPUs);

    auto session = popart::TrainingSession::createFromOnnxModel(
        proto,
        dataFlow,
        l1,
        optimizer,
        device,
        InputShapeInfo(),
        userOptions,
        popart::Patterns(PatternsLevel::Default));

    auto opSchedule =
        session->getIr().getOpSchedule({}, RequireOptimalSchedule::Yes);
    int nRecomp = 0;
    for (auto op : opSchedule) {
      if (op->settings.recomputeType == RecomputeType::Recompute) {
        ++nRecomp;
      }
    }

    // Note that having Ops annotated as recompute does not, for pipelining,
    // imply anthing will be recomputed. The true test for this will be that the
    // number of Tensors stashed is reduced for the recompute in pipelining
    // case.
    if (rt == RunType::ContinuousRecompPipe) {
      BOOST_CHECK(nRecomp > 0);
    } else {
      BOOST_CHECK(nRecomp == 0);
    }

    session->prepareDevice();

    // all the data for a single step
    std::vector<float> v_input_x(stepDataElms);

    std::map<popart::TensorId, popart::IArray &> anchors = {};

    std::vector<WeightPair> weightPairs;
    for (int iteration = 0; iteration < learnRates.size(); ++iteration) {

      // Fix the seed, to ensure the same sequence of sample generation for each
      // learning rate
      eng.seed(57);

      auto learnRate = learnRates.at(iteration);
      auto nSteps    = nSteps_s.at(iteration);

      // initialize weights, write weights to device
      session->resetHostWeights(proto);
      session->weightsFromHost();

      // initialize learnRate, write to device
      SGD newOptimizer({{"defaultLearningRate", {learnRate, false}}});
      session->updateOptimizerFromHost(&newOptimizer);

      int64_t samplesPerStep =
          batchesPerStep * accumulationFactor * microBatchSize;

      std::vector<int64_t> stepDataShape{batchesPerStep * accumulationFactor,
                                         microBatchSize,
                                         sampleHeight,
                                         sampleHeight};

      TensorInfo stepDataInfo{"FLOAT", stepDataShape};

      for (int i = 0; i < nSteps; ++i) {
        std::cout << "Step # " << i << std::endl;

        // generate new samples
        for (int i = 0; i < samplesPerStep; ++i) {
          for (int j = 0; j < sampleElms; ++j) {
            auto stepIndex       = i * sampleElms + j;
            v_input_x[stepIndex] = fdis(eng);
          }
        }
        popart::NDArrayWrapper<float> input_x_wrapper(v_input_x.data(),
                                                      stepDataInfo);
        std::map<popart::TensorId, popart::IArray &> inputs = {
            {input0, input_x_wrapper}};
        popart::StepIO stepio(inputs, anchors);

        // process the samples
        session->run(stepio);

        if (dumpPoplarTrace) {
          if (iteration == 0 && i == 0 && rt == RunType::ContinuousRecompPipe) {
            auto summaryReport = session->getSummaryReport();
            std::ofstream filly;
            filly.open("recompLog.log");
            filly << summaryReport;
            filly.close();
          }

          if (iteration == 0 && i == 0 && rt == RunType::ContinuousPipe) {
            auto summaryReport = session->getSummaryReport();
            std::ofstream filly;
            filly.open("pipelineLog.log");
            filly << summaryReport;
            filly.close();
          }
        }
      }

      // read final weights back
      session->weightsToHost();
      session->readWeights(weightsRead);
      weightPairs.push_back(WeightPair(allWeights, w_readbacks));
    }

    return weightPairs;
  };

  auto getDelta = [](const std::vector<std::vector<float>> &a,
                     const std::vector<std::vector<float>> &b) {
    std::vector<float> vDelta;
    for (int layer = 0; layer < a.size(); ++layer) {
      float delta = 0;
      for (int i = 0; i < a[layer].size(); ++i) {
        delta += std::abs(a[layer][i] - b[layer][i]);
      }
      vDelta.push_back(delta);
    }
    return vDelta;
  };

  auto print = [&getDelta](const WeightPair &exact,
                           const WeightPair &continuous) {
    auto delta_exact      = getDelta(exact.start, exact.end);
    auto delta_continuous = getDelta(continuous.start, continuous.end);
    auto delta_starts     = getDelta(continuous.start, exact.start);
    auto delta_ends       = getDelta(continuous.end, exact.end);

    std::cout << "| exact end - exact start |_1    "
              << "   | exact_end - continuous_end |_1 / "
              << " | exact_end - exact_start |_1 " << std::endl;

    for (int i = 0; i < delta_exact.size(); ++i) {
      std::cout << delta_exact[i] << "   " << delta_ends[i] / delta_exact[i]
                << std::endl;
    }
  };

  auto getMeanRelative = [&getDelta](const WeightPair &wp0,
                                     const WeightPair &wp1) {
    float sum       = 0.0f;
    auto delta_0    = getDelta(wp0.start, wp0.end);
    auto delta_ends = getDelta(wp0.end, wp1.end);
    for (auto i = 0; i < delta_0.size(); ++i) {
      sum += delta_ends[i] / delta_0[i];
    }
    return sum / delta_0.size();
  };

  std::vector<float> learnRates{
      0.128, 0.064, 0.032, 0.016, 0.008, 0.004, 0.002, 0.001, 0.0005, 0.128};

  std::vector<int64_t> nSteps_s{1, 2, 4, 8, 16, 32, 64, 128, 256, 1};

  int accumulationFactor = 1;

  std::cout << "\nGet results for Continuous without Recomputation\n"
            << std::endl;
  auto continuous = getResults(
      RunType::ContinuousPipe, learnRates, nSteps_s, accumulationFactor);

  std::cout << "\nGet results for Continuous with Recomputation\n" << std::endl;
  auto recomp = getResults(
      RunType::ContinuousRecompPipe, learnRates, nSteps_s, accumulationFactor);

  std::cout << "\nGet results for SingleDevice run\n" << std::endl;
  auto exact = getResults(
      RunType::SingleDevice, learnRates, nSteps_s, accumulationFactor);

  bool printLog = true;

  if (printLog) {
    for (int i = 0; i < exact.size() - 1; ++i) {
      std::cout << "\n@ learning rate " << learnRates[i] << std::endl;
      std::cout << "-----------------" << std::endl;
      std::cout << "For continuous (without recompute)" << std::endl;
      print(exact[i], continuous[i]);
      std::cout << "For continuous (with recompute)" << std::endl;
      print(exact[i], recomp[i]);
    }

    std::cout << "Reporting mean relative differences over all weights "
              << std::endl;
    for (int i = 0; i < exact.size() - 1; ++i) {
      std::cout << "\n@ learning rate " << learnRates[i] << std::endl;
      std::cout << "exact <-> continuous "
                << getMeanRelative(exact[i], continuous[i]) << std::endl;

      std::cout << "exact <-> recomp " << getMeanRelative(exact[i], recomp[i])
                << std::endl;

      std::cout << "continuous <-> recomp "
                << getMeanRelative(continuous[i], recomp[i]) << std::endl;
    }
  }

  // The final run should match the first run exactly (same learning rate)
  BOOST_CHECK(getMeanRelative(exact.back(), exact[0]) < 1e-8);

  // at 0.0005:
  auto small = getMeanRelative(exact[8], continuous[8]);

  // at 0.128:
  auto large = getMeanRelative(exact[0], continuous[0]);

  //  the experiment above give 0.11868 for this run
  BOOST_CHECK(large < 0.5);

  // the ratio should be about 2**8 using, using a margin of 2**2
  BOOST_CHECK(large / small > (2 << 6));
}

// very nice convergence (04 September 2019):

// 165: @ learning rate 0.128
// 165: exact <-> continuous 0.143023
// 165: exact <-> recomp 0.137421
// 165: continuous <-> recomp 0.0168884
// 165:
// 165: @ learning rate 0.064
// 165: exact <-> continuous 0.0598955
// 165: exact <-> recomp 0.0593508
// 165: continuous <-> recomp 0.00738907
// 165:
// 165: @ learning rate 0.032
// 165: exact <-> continuous 0.0276319
// 165: exact <-> recomp 0.0270495
// 165: continuous <-> recomp 0.00313846
// 165:
// 165: @ learning rate 0.016
// 165: exact <-> continuous 0.0132138
// 165: exact <-> recomp 0.012881
// 165: continuous <-> recomp 0.00172954
// 165:
// 165: @ learning rate 0.008
// 165: exact <-> continuous 0.00570888
// 165: exact <-> recomp 0.00564257
// 165: continuous <-> recomp 0.000833319
// 165:
// 165: @ learning rate 0.004
// 165: exact <-> continuous 0.00257001
// 165: exact <-> recomp 0.00253338
// 165: continuous <-> recomp 0.000424094
// 165:
// 165: @ learning rate 0.002
// 165: exact <-> continuous 0.00119873
// 165: exact <-> recomp 0.00115962
// 165: continuous <-> recomp 0.000205215
// 165:
// 165: @ learning rate 0.001
// 165: exact <-> continuous 0.000569798
// 165: exact <-> recomp 0.000562985
// 165: continuous <-> recomp 0.000106053
// 165:
// 165: @ learning rate 0.0005
// 165: exact <-> continuous 0.00030137
// 165: exact <-> recomp 0.000299084
// 165: continuous <-> recomp 5.77406e-05
//