#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);
        //             }
        //         }
        //     }
        // }
    }

}


#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;
};


#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 */


#include "LinearApproximation.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

LinearApproximation::LinearApproximation(const size_t t_numVars)
  : m_numVars(t_numVars)
{
}

LinearApproximation::~LinearApproximation()
{
}

double LinearApproximation::average() const
{
  double sum = 0;
  for (std::vector<std::vector<double> >::const_iterator lhsitr = m_values.begin();
      lhsitr != m_values.end();
      ++lhsitr)
  {
    sum += (*lhsitr)[m_numVars];
  }
  return sum / m_numVars;
}

LinearApproximation LinearApproximation::operator-(const LinearApproximation &t_rhs) const
{
  if (m_numVars != t_rhs.m_numVars)
  {
    throw std::runtime_error("Unable to subtract, different number of variables");
  }

  LinearApproximation retval(m_numVars);

  for (std::vector<std::vector<double> >::const_iterator lhsitr = m_values.begin();
      lhsitr != m_values.end();
      ++lhsitr)
  {
    for (std::vector<std::vector<double> >::const_iterator rhsitr = t_rhs.m_values.begin();
        rhsitr != t_rhs.m_values.end();
        ++rhsitr)
    {
      std::vector<double> vals(m_numVars, 0);

      bool match = true;
      for (size_t i = 0; i < m_numVars; ++i)
      {
        if ( (*lhsitr)[i] != (*rhsitr)[i] )
        {
          match = false;
          break;
        } else {
          vals[i] = (*lhsitr)[i];
        }
      }

      if (match)
      {
        double value = (*lhsitr)[m_numVars] - (*rhsitr)[m_numVars];
        retval.addVals(vals, value);
        break;
      }
    }
  }

  return retval;
}

void LinearApproximation::addVals(std::vector<double> t_vals, const double t_result)
{
  validateVariableSize(t_vals);

  t_vals.push_back(t_result);

  for (std::vector<std::vector<double> >::const_iterator lhsitr = m_values.begin();
      lhsitr != m_values.end();
      ++lhsitr)
  {
    bool match = true;
    for (size_t i = 0; i < m_numVars; ++i)
    {
      if ( (*lhsitr)[i] != t_vals[i] )
      {
        match = false;
        break;
      }
    }

    if (match)
    {
      if ((*lhsitr)[m_numVars] != t_result)
      {
        throw std::runtime_error("Data already exists with a different result value");
      } else {
        // it already exists
        return;
      }
    }
  }

  // it didn't already exist, add it now
  m_values.push_back(t_vals);

}

std::vector<std::vector<double> > LinearApproximation::findMinimalDifferences(
    size_t t_numDifferences,
    const std::vector<std::vector<double> > &t_vals,
    const std::vector<std::vector<double> > &t_data) const
{
  // t_vals includes the baseline requested value, so we want one more than that
  if (t_data.empty() || t_numDifferences + 2 == t_vals.size())
  {
    // this is as good as we're going to get / can use right now
    std::vector<std::vector<double> > retval = t_vals;

    if (!t_vals.empty())
    {
      retval.erase(retval.begin());
    }
    return retval;
  }

  std::vector<std::vector<double> > candidates;
  std::vector<std::vector<double> > remainder;

  for (size_t i = 0; i < t_data.size(); ++i)
  {
    size_t numDifferences = 0;

    for (size_t j = 0; j < m_numVars; ++j)
    {
      for (size_t i2 = 0; i2 < t_vals.size(); ++i2)
      {
        if (t_data[i][j] != t_vals[i2][j])
        {
          ++numDifferences;
          break; // don't check any further input rows
        }
      }

      if (numDifferences > t_numDifferences)
      {
        // don't continue with this data row
        break;
      }
    }

    if (numDifferences <= t_numDifferences)
    {
      candidates.push_back(t_data[i]);
    } else {
      remainder.push_back(t_data[i]);
    }
  }

  std::vector<std::vector<double> > keptresult = t_vals;
  keptresult.erase(keptresult.begin());

  for (size_t i = 0; i < candidates.size(); ++i)
  {
    std::vector<std::vector<double> > newvals(t_vals);
    newvals.push_back(candidates[i]);

    std::vector<std::vector<double> > newdata(t_data);
    newdata.erase(std::remove(newdata.begin(), newdata.end(), candidates[i]), newdata.end());

    std::vector<std::vector<double> > result = findMinimalDifferences(t_numDifferences, newvals, newdata);
    if (result.size() > keptresult.size())
    {
      keptresult = result;
    }
  }

  return keptresult;
}

std::vector<std::vector<double> > LinearApproximation::filterForProblemReduction(const std::vector<double> &t_vals,
    const std::vector<std::vector<double> > &t_data) const
{
  std::vector<std::vector<double> > testData;
  testData.push_back(t_vals);

  for (size_t i = 1; i < m_numVars; ++i)
  {
    std::vector<std::vector<double> > results = findMinimalDifferences(i, testData, t_data);
    if (results.size() > i)
    {
      return results;
    }
  }

  return t_data;
}

double LinearApproximation::approximate(const std::vector<double> &t_vals) const
{
  // Linear Approximation
  // A(x-x0) + B(y-y0) + C(z-z0) = 0
  validateVariableSize(t_vals);

//  print("Approximating: ", t_vals);

  std::vector<std::vector<double> > sortedPoints = sortByDistance(t_vals, m_values);

  if (!sortedPoints.empty() && distance(t_vals, sortedPoints[0]) == 0)
  {
    //return exact match
    return sortedPoints[0].back();
  }

  std::vector<std::vector<double> > optimalPoints = filterForProblemReduction(t_vals, sortedPoints);

//  print("Starting points: ", m_values);

//  print("Sorted Points: ", sortedPoints);
  std::vector<std::vector<double> > chosenPoints = filterForSimilarity(t_vals, optimalPoints);

//  print("Chosen points: ", chosenPoints);

  std::vector<std::vector<std::vector<double> > > coefficientMatrices = buildCoefficientMatrices(chosenPoints);

//  print("Coefficients: ", coefficientMatrices);

  std::vector<double> coefficients = solveDeterminates(coefficientMatrices);

//  print("Coefficients: ", coefficients);
  assert(coefficients.size() == m_numVars + 1);

  double approximation = 0;
  for (size_t i = 0; i < m_numVars; ++i)
  {
    // A(x-x0) + B(y-y0) + C(z-z0) ...
    approximation += coefficients[i] * (t_vals[i] - chosenPoints[0][i]);
  }

  // the last variable is the one we are solving for
  // z=(z0*C+(y0-y)*B+(x0-x)*A)/C
  if (coefficients[m_numVars] == 0)
  {
    throw std::runtime_error("Unabled to approximate, not enough data");
  }

  approximation = (approximation + chosenPoints[0][m_numVars] * coefficients[m_numVars]) / coefficients[m_numVars];

//  std::cout << "final approximation: " << approximation << std::endl;

  return approximation;
}

void LinearApproximation::validateVariableSize(const std::vector<double> &t_vals) const
{
  if (t_vals.size() != m_numVars)
  {
    throw std::range_error("Unexpected number of variables supplied");
  }
}

const std::vector<std::vector<double> > LinearApproximation::filterForSimilarity(const std::vector<double> &t_point, const std::vector<std::vector<double> > &t_vals) const
{
  std::vector<std::vector<double> > goodPoints = t_vals;
  int pointsNeeded = m_numVars + 1;

  for (size_t i = 0; i < m_numVars; ++i)
  {
    bool diversityFound = false;
    bool allTheSame = true;

    int differencePosition = -1;

    int rowCount = 0;
    for (std::vector<std::vector<double> >::const_iterator itr = t_vals.begin();
        itr != t_vals.end();
        ++itr, ++rowCount)
    {
      if (t_vals[0][i] != (*itr)[i])
      {
        diversityFound = true; // not all of the inputs match each other
      }

      if (t_point[i] != (*itr)[i])
      {
        allTheSame = false; // not all of the inputs for this position match the requested approximation
        differencePosition = rowCount;
      }
    }

    if (!diversityFound && !allTheSame)
    {
      // There is not enough data available to calculate the value for this position
      std::stringstream ss;
      ss << "Not enough data available for the " << i << " variable position";
      throw std::runtime_error(ss.str());
    }

    if (allTheSame || (differencePosition >= pointsNeeded))
    {
      if (pointsNeeded > 2)
      {
        --pointsNeeded;
        goodPoints.resize(std::min(pointsNeeded, static_cast<int>(goodPoints.size()))); // never expand it
      }
    }
  }

  goodPoints.resize(std::min(pointsNeeded, static_cast<int>(goodPoints.size())));
  return goodPoints;
}

std::vector<std::vector<std::vector<double> > > LinearApproximation::buildCoefficientMatrices(
    const std::vector<std::vector<double> > &t_points) const
{
  std::set<size_t> discardedColumns;

  for (size_t i = 0; i <= m_numVars; ++i)
  {
    bool allthesame = true;
    for (std::vector<std::vector<double> >::const_iterator itr = t_points.begin();
        itr != t_points.end();
        ++itr)
    {
      if ((*itr)[i] != t_points[0][i])
      {
        allthesame = false;
      }
    }

    if (allthesame)
    {
      discardedColumns.insert(i);
    }
  }

  std::vector<std::vector<std::vector<double> > > retvals;

  for (size_t i = 0; i <= m_numVars; ++i)
  {
    if (discardedColumns.count(i) == 1)
    {
      // this is a discarded column its value is 0
      std::vector<double> v(1,0); // a vector of one element of value 0
      std::vector<std::vector<double> > m;
      m.push_back(v);
      retvals.push_back(m);
    } else {
      std::vector<std::vector<double> > m;
      for (size_t curRow = 0; curRow < m_numVars - discardedColumns.size(); ++curRow)
      {
        std::vector<double> row;
        for (size_t j = 0; j <= m_numVars; ++j)
        {
          if (j != i && discardedColumns.count(j) == 0)
          {
            // This is a column we want to work with
            row.push_back(t_points[curRow+1][j] - t_points[0][j]);
          }
        }
        m.push_back(row);
      }
      retvals.push_back(m);
    }
  }

  return retvals;
}

std::vector<std::vector<double> > LinearApproximation::removeCol(const std::vector<std::vector<double> > &t_matrix, const size_t col) const
{
  std::vector<std::vector<double> > retval(t_matrix.size()-1, std::vector<double>(t_matrix.size()-1, 0));

  for (size_t i = 1; i < t_matrix.size(); ++i)
  {
    size_t colnum = 0;
    for (size_t j = 0; j < t_matrix[i].size(); ++j)
    {
      if (j != col)
      {
        retval[i-1][colnum] = t_matrix[i][j];
        ++colnum;
      }
    }
  }

  return retval;
}

double LinearApproximation::determinate(const std::vector<std::vector<double> > &t_matrix) const
{
/*
  bool docache = false;

  if (docache)
  {
    std::map<std::vector<std::vector<double> >, double>::const_iterator itr = m_cache.find(t_matrix);
    if (itr != m_cache.end())
    {
      std::cout << "Cache hit" << std::endl;
      return itr->second;
    }
  }

  if (t_matrix.size() == 1)
  {
    return t_matrix[0][0];
  } else if (t_matrix.size() == 2) {
    return t_matrix[0][0] * t_matrix[1][1] - t_matrix[0][1] * t_matrix[1][0];
  } else {
    double d = 0;

    for (size_t i = 0; i < t_matrix.size(); ++i)
    {
      //          d += pow(-1, i) * t_matrix[0][i] * determinate(removeCol(t_matrix, i));
      d += (i%2==1?-1:1) * t_matrix[0][i] * determinate(removeCol(t_matrix, i));
    }

    if (docache)
    {
      m_cache[t_matrix] = d;
    }
    return d;
  }
  */

  namespace bnu = boost::numeric::ublas;

  class Determinant
  {

    static int determinant_sign(const bnu::permutation_matrix<std::size_t>& pm)
    {
      int pm_sign=1;
      std::size_t size = pm.size();
      for (std::size_t i = 0; i < size; ++i)
      {
        if (i != pm(i)) {
          pm_sign *= -1; // swap_rows would swap a pair of rows here, so we change sign
        }
      }
      return pm_sign;
    }

    static double determinant( bnu::matrix<double>& m ) {
      bnu::permutation_matrix<std::size_t> pm(m.size1());
      double det = 1.0;
      if( bnu::lu_factorize(m,pm) ) {
        det = 0.0;
      } else {
        for(size_t i = 0; i < m.size1(); i++) {
          det *= m(i,i); // multiply by elements on diagonal
        }

        det = det * determinant_sign( pm );
      }
      return det;
    }

    public:
    static double determinant(const std::vector<std::vector<double> > &t_matrix)
    {
      bnu::matrix<double> matrix(t_matrix.size(), t_matrix.size());

      for (size_t i = 0; i < t_matrix.size(); ++i)
      {
        for (size_t j = 0; j < t_matrix[i].size(); ++j)
        {
          matrix(i, j) = t_matrix[i][j];
        }
      }

      return determinant(matrix);
    }
  };

  double retval = Determinant::determinant(t_matrix);

  /*
  if (docache)
  {
    m_cache[t_matrix] = retval;
  }
  */

  return retval;
}

std::vector<double> LinearApproximation::solveDeterminates(const std::vector<std::vector<std::vector<double> > > &t_matrices) const
{
  std::vector<double> retval;

  for (std::vector<std::vector<std::vector<double> > >::const_iterator itr = t_matrices.begin();
      itr != t_matrices.end();
      ++itr)
  {
    retval.push_back(determinate(*itr));
  }

  return retval;
}

std::vector<std::vector<double> > LinearApproximation::sortByDistance(const std::vector<double> &t_point,
    const std::vector<std::vector<double> > &t_values) const
{
  std::set<std::pair<double, std::vector<double> > > distanceSorted;

  for (std::vector<std::vector<double> >::const_iterator itr = t_values.begin();
      itr != t_values.end();
      ++itr)
  {
    distanceSorted.insert(std::make_pair(distance(t_point, *itr), *itr));
  }

  std::vector<std::vector<double> > retval;

  for (std::set<std::pair<double, std::vector<double> > >::const_iterator itr = distanceSorted.begin();
      itr != distanceSorted.end();
      ++itr)
  {
    retval.push_back(itr->second);
  }

  return retval;
}

double LinearApproximation::distance(const std::vector<double> &t_p1, const std::vector<double> &t_p2) const
{
  double result = 0;
  for (size_t i = 0; i < m_numVars; ++i)
  {
    double part = t_p1[i] - t_p2[i];
    part *= part;

    result += part;
  }

  return sqrt(result);
}

void LinearApproximation::print(const std::string &t_str, const std::vector<double> &t_vals)
{
  if (!t_str.empty())
  {
    std::cout << t_str << std::endl;
  }

  std::cout << "[ ";
  for (std::vector<double>::const_iterator itr = t_vals.begin();
      itr != t_vals.end();
      ++itr)
  {
    std::cout << std::setw(10) << *itr << " ";
  }
  std::cout << "]" << std::endl;
}

void LinearApproximation::print(const std::string &t_str, const std::vector<std::vector<double> > &t_vals)
{
  if (!t_str.empty())
  {
    std::cout << t_str << std::endl;
  }

  for (std::vector<std::vector<double> >::const_iterator itr = t_vals.begin();
      itr != t_vals.end();
      ++itr)
  {
    print("", *itr);
  }
}

std::pair<double, double> LinearApproximation::nearestFurthestNeighborDistances(const std::vector<double> &t_vals) const
{
  std::vector<std::vector<double> > sorted = sortByDistance(t_vals, m_values);

  if (sorted.size() < 1)
  {
    throw std::range_error("no neighbors");
  }

  return std::make_pair(distance(t_vals, sorted.front()), distance(t_vals, sorted.back()));
}

void LinearApproximation::print(const std::string &t_str, const std::vector<std::vector<std::vector<double> > > &t_vals)
{
  if (!t_str.empty())
  {
    std::cout << t_str << std::endl;
  }

  for (std::vector<std::vector<std::vector<double> > >::const_iterator itr = t_vals.begin();
      itr != t_vals.end();
      ++itr)
  {
    print("", *itr);
    std::cout << std::endl;
  }
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Eigen. http://eigen.tuxfamily.org/dox/TopicUnalignedArrayAssert.html, last accessed: 18th
 *          March, 2013.
 *
 *    Notes
 *      This implementation uses the KeplerianElements class, which is marked for deprecation. The
 *      code will be updated to use a simple "Vector6d" object from the Eigen library instead.
 *
 */

#include <stdexcept>

#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerStateExtractor.h"
#include "Tudat/InputOutput/parsedDataVectorUtilities.h"

namespace tudat
{
namespace ephemerides
{

//! Extract the Keplerian Elements.
std::shared_ptr< Eigen::Vector6d > KeplerStateExtractor::extract(
        ParsedDataLineMapPtr dataLineMap )
{
    // Short-hand notation.
    namespace parsed_data_vector_utilities = input_output::parsed_data_vector_utilities;
    using Eigen::Vector6d;

    // Create a new KeplerianElements object.
    std::shared_ptr< Vector6d > keplerianElements
            = std::allocate_shared< Vector6d >( Eigen::aligned_allocator< Vector6d >( ) );

    // Find and set semi-major axis.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::semiMajorAxis ) )
    {
        ( *keplerianElements )( orbital_element_conversions::semiMajorAxisIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::semiMajorAxis );
    }

    else
    {
        throw std::runtime_error( "No semi-major axis entry found." );
    }

    // Find and set eccentricity.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::eccentricity ) )
    {
        ( *keplerianElements )( orbital_element_conversions::eccentricityIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::eccentricity );
    }

    else
    {
        throw std::runtime_error( "No eccentricity entry found." );
    }

    // Find and set inclination.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::inclination ) )
    {
        ( *keplerianElements )( orbital_element_conversions::inclinationIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::inclination );
    }

    else
    {
        throw std::runtime_error( "No inclination entry found." );
    }

    // Find and set longitude of ascending node.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::longitudeOfAscendingNode ) )
    {
        ( *keplerianElements )( orbital_element_conversions::longitudeOfAscendingNodeIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::longitudeOfAscendingNode );
    }

    else
    {
        throw std::runtime_error( "No longitude of ascending node entry found." );
    }

    // Find and set argument of periapsis.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::argumentOfPeriapsis ) )
    {
        ( *keplerianElements )( orbital_element_conversions::argumentOfPeriapsisIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::argumentOfPeriapsis );
    }

    else
    {
        throw std::runtime_error( "No argument of periapsis entry found." );
    }

    // Find and set true anomaly.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::trueAnomaly ) )
    {
        ( *keplerianElements )( orbital_element_conversions::trueAnomalyIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::trueAnomaly );
    }

    // If the true anomaly is not present, check for mean anomaly.
    else if ( checkOptionalFieldType( dataLineMap, 1,
                                      input_output::field_types::state::meanAnomaly ) )
    {
        // Store mean anomaly.
        const double meanAnomaly = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::meanAnomaly );

        // Retrieve eccentricity.
        const double eccentricity
                = ( *keplerianElements )( orbital_element_conversions::eccentricityIndex );

        // Convert to eccentric anomaly.
        const double eccentricAnomaly = orbital_element_conversions::
                convertMeanAnomalyToEccentricAnomaly( eccentricity, meanAnomaly );

        // Convert eccentric anomaly to true anomaly and set the latter.
        ( *keplerianElements )( orbital_element_conversions::trueAnomalyIndex )
                = orbital_element_conversions::
                convertEccentricAnomalyToTrueAnomaly( eccentricAnomaly, eccentricity );
    }

    else
    {
        throw std::runtime_error( "No true anomaly or mean anomaly entries found." );
    }

    return keplerianElements;
}

} // namespace ephemerides
} // namespace tudat


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

/// \file rmax_adjust.cc
///

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/fstream.hpp"
namespace po = boost::program_options;
namespace fs = boost::filesystem;

#include <vw/Camera/CAHVORModel.h>
#include <vw/BundleAdjustment.h>
#include <vw/Math.h>
#include <vw/Math/LevenbergMarquardt.h>

using namespace vw;
using namespace vw::camera;
using namespace vw::ba;

#include <stdlib.h>
#include <iostream>

#include <asp/Sessions/RMAX/RMAX.h>

// This sifts out from a vector of strings, a listing of GCPs.  This
// should be useful for those programs who accept their data in a mass
// input vector.
std::vector<std::string>
sort_out_gcps( std::vector<std::string>& image_files ) {
  std::vector<std::string> gcp_files;
  std::vector<std::string>::iterator it = image_files.begin();
  while ( it != image_files.end() ) {
    if ( boost::iends_with(*it, ".gcp") ){
      gcp_files.push_back( *it );
      it = image_files.erase( it );
    } else
      it++;
  }

  return gcp_files;
}

static std::string prefix_from_filename(std::string const& filename) {
  std::string result = filename;
  int index = result.rfind(".");
  if (index != -1)
    result.erase(index, result.size());
  return result;
}

class HelicopterBundleAdjustmentModel : public ba::ModelBase<HelicopterBundleAdjustmentModel, 6, 3> {

  typedef Vector<double,6> camera_vector_t;
  typedef Vector<double,3> point_vector_t;

  std::vector<ImageInfo> m_image_infos;

  boost::shared_ptr<ControlNetwork> m_network;

  std::vector<camera_vector_t> a;
  std::vector<point_vector_t> b;
  std::vector<camera_vector_t> a_target;
  std::vector<point_vector_t> b_target;
  int m_num_pixel_observations;

public:

  HelicopterBundleAdjustmentModel(std::vector<ImageInfo> const& image_infos,
                                  boost::shared_ptr<ControlNetwork> network) :
    m_image_infos(image_infos), m_network(network),
    a(image_infos.size()), b(network->size()),
    a_target(image_infos.size()), b_target(network->size()) {

    // Compute the number of observations from the bundle.
    m_num_pixel_observations = 0;
    for (unsigned i = 0; i < network->size(); ++i)
      m_num_pixel_observations += (*m_network)[i].size();

    // Set up the a and b vectors, storing the initial values.
    for (unsigned j = 0; j < m_image_infos.size(); ++j) {
      a[j] = camera_vector_t();
      a_target[j] = a[j];
    }

    for (unsigned i = 0; i < m_network->size(); ++i) {
      b[i] = (*m_network)[i].position();
      b_target[i] = b[i];
    }

  }

  // Return a reference to the camera and point parameters.
  camera_vector_t A_parameters(int j) const { return a[j]; }
  point_vector_t B_parameters(int i) const { return b[i]; }
  void set_A_parameters(int j, camera_vector_t const& a_j) {
    a[j] = a_j;
  }
  void set_B_parameters(int i, point_vector_t const& b_i) {
    b[i] = b_i;
  }

  // Return the initial parameters
  camera_vector_t A_target(int j) const { return a_target[j]; }
  point_vector_t B_target(int i) const { return b_target[i]; }

  // Return general sizes
  unsigned num_cameras() const { return a.size(); }
  unsigned num_points() const { return b.size(); }
  unsigned num_pixel_observations() const { return m_num_pixel_observations; }

  // Return pixel observations
  unsigned num_observations_of_point(const int& i) const { return (*m_network)[i].size(); }
  unsigned corresponding_camera_for_measure(const int& i, const int& m){
    return (*m_network)[i][m].image_id();
  }
  Vector2 pixel_observation_of_point(const int& i, const int& m) const {
    //Finding out which camera accounts for the m_th observations of point i
    unsigned int camera_id = (*m_network)[i][m].image_id();

    Vector<double,6> a_j = a[camera_id];
    Vector3 position_correction = subvector(a_j,0,3);
    Vector3 pose_correction = subvector(a_j,3,3);
    camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[camera_id],position_correction,pose_correction);

    Vector4 point_estimation = B_parameters(i);
    point_estimation = point_estimation/point_estimation(3);
    return cam.point_to_pixel(subvector(point_estimation,0,3));
  }

  // Return the covariance of the camera parameters for camera j.
  inline Matrix<double,camera_params_n,camera_params_n> A_inverse_covariance ( unsigned /*j*/ ) {
    Matrix<double,camera_params_n,camera_params_n> result;
    result(0,0) = 1/1;  // Position sigma = 1 meter
    result(1,1) = 1/1;
    result(2,2) = 1/1;
    result(3,3) = 1;  // Pose sigma = 1 degrees
    result(4,4) = 1;
    result(5,5) = 1;
    return result;
  }

  // Return the covariance of the point parameters for point i.
  inline Matrix<double,point_params_n,point_params_n> B_inverse_covariance ( unsigned /*i*/ ) {
    Matrix<double,point_params_n,point_params_n> result;
    result(0,0) = 1/1000.0;  // Point sigma = 1000 meters ( we set this to be
    result(1,1) = 1/1000.0;  // so large that it essentially removes point position
    result(2,2) = 1/1000.0;  // constraints from the bundle adjustment entirely. )
    return result;
  }

  void write_adjustment(int j, std::string const& filename) {
    std::ofstream ostr(filename.c_str());
    ostr << a[j][0] << " " << a[j][1] << " " << a[j][2] << "\n";
    ostr << a[j][3] << " " << a[j][4] << " " << a[j][5] << "\n";
  }

  void write_adjusted_camera(int j, std::string const& filename) {
    Vector<double,6> a_j = a[j];
    Vector3 position_correction = subvector(a_j, 0, 3);
    Vector3 pose_correction = subvector(a_j, 3,3);
    camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[j], position_correction, pose_correction);
    cam.write(filename);
  }

  void write_adjusted_cameras_append(std::string const& filename) {
    std::ofstream ostr(filename.c_str(),std::ios::app);
    for (unsigned j=0; j < a.size();++j){
      Vector<double,6> a_j = a[j];
      Vector3 position_correction = subvector(a_j,0,3);
      Vector3 pose_correction = subvector(a_j,3,3);
      camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[j],position_correction,pose_correction);
      ostr << j << "\t" << cam.C(0) << "\t" << cam.C(1) << "\t" << cam.C(2) << "\n";
      ostr << j << "\t" << cam.A(0) << "\t" << cam.A(1) << "\t" << cam.A(2) << "\n";
      ostr << j << "\t" << cam.H(0) << "\t" << cam.H(1) << "\t" << cam.H(2) << "\n";
      ostr << j << "\t" << cam.V(0) << "\t" << cam.V(1) << "\t" << cam.V(2) << "\n";
      ostr << j << "\t" << cam.O(0) << "\t" << cam.O(1) << "\t" << cam.O(2) << "\n";
      ostr << j << "\t" << cam.R(0) << "\t" << cam.R(1) << "\t" << cam.R(2) << "\n";
    }
  }

  std::vector<boost::shared_ptr<camera::CameraModel> > adjusted_cameras() {
    std::vector<boost::shared_ptr<camera::CameraModel> > result(a.size());
    for (unsigned j = 0; j < result.size(); ++j) {
      Vector3 position_correction = subvector(a[j], 0, 3);
      Vector3 pose_correction = subvector(a[j], 3,3);

      camera::CAHVORModel *cahvor = new camera::CAHVORModel;
      *cahvor = rmax_image_camera_model(m_image_infos[j], position_correction, pose_correction);

      result[j] = boost::shared_ptr<camera::CameraModel>( cahvor );
    }
    return result;
  }

  // Given the 'a' vector (camera model parameters) for the j'th
  // image, and the 'b' vector (3D point location) for the i'th
  // point, return the location of b_i on imager j in pixel
  // coordinates.
  Vector2 operator() ( unsigned /*i*/, unsigned j, Vector<double,6> const& a_j, Vector<double,3> const& b_i ) const {
    Vector3 position_correction = subvector(a_j, 0, 3);
    Vector3 pose_correction = subvector(a_j, 3,3);
    camera::CAHVORModel cam = rmax_image_camera_model(m_image_infos[j], position_correction, pose_correction);
    return cam.point_to_pixel(b_i);
  }

  // Errors on the image plane
  void image_errors( std::vector<double>& pix_errors ) {
    pix_errors.clear();
    for (unsigned i = 0; i < m_network->size(); ++i)
      for(unsigned m = 0; m < (*m_network)[i].size(); ++m) {
        int camera_idx = (*m_network)[i][m].image_id();
        Vector2 pixel_error = (*m_network)[i][m].position() - (*this)(i, camera_idx,
                                                                   a[camera_idx],b[i]);
        pix_errors.push_back(norm_2(pixel_error));
      }
  }

  // Errors for camera position
  void camera_position_errors( std::vector<double>& camera_position_errors ) {
    camera_position_errors.clear();
    for (unsigned j=0; j < this->num_cameras(); ++j) {
      Vector3 position_initial, position_now;
      position_initial = subvector(a_target[j],0,3);
      position_now = subvector(a[j],0,3);

      camera_position_errors.push_back(norm_2(position_initial-position_now));
    }
  }

  // Errors for camera pose
  void camera_pose_errors( std::vector<double>& camera_pose_errors ) {
    camera_pose_errors.clear();
    for (unsigned j=0; j < this->num_cameras(); ++j) {
      Vector3 pose_initial, pose_now;
      pose_initial = subvector(a_target[j],3,3);
      pose_now = subvector(a[j],3,3);

      camera_pose_errors.push_back(norm_2(pose_initial-pose_now));
    }
  }

  // Errors for gcp errors
  void gcp_errors( std::vector<double>& gcp_errors ) {
    gcp_errors.clear();
    for (unsigned i=0; i < this->num_points(); ++i)
      if ((*m_network)[i].type() == ControlPoint::GroundControlPoint) {
        point_vector_t p1 = b_target[i]/b_target[i](3);
        point_vector_t p2 = b[i]/b[i](3);
        gcp_errors.push_back(norm_2(subvector(p1,0,3) - subvector(p2,0,3)));
      }
  }

  // Give access to the control network
  boost::shared_ptr<ControlNetwork> control_network() {
    return m_network;
  }
};

int main(int argc, char* argv[]) {

  std::vector<std::string> image_files;
  std::vector<std::string> gcp_files;
  std::string cnet_file;
  boost::shared_ptr<ControlNetwork> cnet( new ControlNetwork("My first control network"));
  double lambda;
  int min_matches;
  double robust_outlier_threshold;
  int max_iterations;
  int report_level;

  po::options_description general_options("Options");
  general_options.add_options()
    ("cnet,c", po::value<std::string>(&cnet_file), "Load a control network from a file")
    ("lambda,l", po::value<double>(&lambda), "Set the initial value of the LM parameter lambda")
    ("min-matches", po::value<int>(&min_matches)->default_value(5), "Set the mininmum number of matches between images that will be considered.")
    ("robust-threshold", po::value<double>(&robust_outlier_threshold)->default_value(10.0), "Set the threshold for robust cost functions.")
    ("max-iterations", po::value<int>(&max_iterations)->default_value(25), "Set the maximum number of iterations.")
    ("save-iteration-data,s", "Saves all camera information between iterations to iterCameraParam.txt, it also saves point locations for all iterations in iterPointsParam.txt. Warning: This is slow as pixel observations need to be calculated on each step.")
    ("report-level,r",po::value<int>(&report_level)->default_value(10),"Changes the detail of the Bundle Adjustment Report")
    ("run-match,m", "Run ipmatch to create .match files from overlapping images.")
    ("match-debug-images,d", "Create debug images when you run ipmatch.")
    ("help,h", "Display this help message");

  po::options_description hidden_options("");
  hidden_options.add_options()
    ("input-files", po::value<std::vector<std::string> >(&image_files));

  po::options_description options("Allowed Options");
  options.add(general_options).add(hidden_options);

  po::positional_options_description p;
  p.add("input-files", -1);

  po::variables_map vm;
  po::store( po::command_line_parser( argc, argv ).options(options).positional(p).run(), vm );
  po::notify( vm );

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " [options] <rmax image filenames>..." << std::endl << std::endl;
  usage << general_options << std::endl;

  if( vm.count("help") ) {
    std::cout << usage << std::endl;
    return 1;
  }

  if( vm.count("input-files") < 1 || image_files.size() < 2) {
    std::cout << "Error: Must specify at least two input files!" << std::endl << std::endl;
    std::cout << usage.str();
    return 1;
  }
  gcp_files = sort_out_gcps( image_files );

  // Read in the camera model and RMAX image info for the input
  // images.
  std::vector<std::string> camera_files(image_files.size());
  std::vector<ImageInfo> image_infos(image_files.size());
  std::vector<boost::shared_ptr<CameraModel> > camera_models(image_files.size());
  for (unsigned i = 0; i < image_files.size(); ++i) {
    read_image_info( image_files[i], image_infos[i] );
    CAHVORModel *cahvor = new CAHVORModel;
    *cahvor = rmax_image_camera_model(image_infos[i]);
    camera_models[i] = boost::shared_ptr<CameraModel>(cahvor);
  }

  if ( vm.count("cnet") ) {
    // Loading a Control Network

    vw_out() << "Loading control network from file: " << cnet_file << "\n";
    cnet->read_binary(cnet_file);

  } else {
    // Building a Control Network
    build_control_network( *cnet, camera_models,
                           image_files, min_matches );
    add_ground_control_points( *cnet, image_files,
                               gcp_files.begin(), gcp_files.end() );

    cnet->write_binary("rmax_adjust");
  }

  HelicopterBundleAdjustmentModel ba_model(image_infos, cnet);
  AdjustSparse<HelicopterBundleAdjustmentModel, L2Error> bundle_adjuster(ba_model, L2Error());

  if (vm.count("lambda"))
    bundle_adjuster.set_lambda(lambda);

  //Clearing the monitoring text files to be used for saving camera params
  if (vm.count("save-iteration-data")){
    std::ofstream ostr("iterCameraParam.txt",std::ios::out);
    ostr << "";
    ostr.close();
    ostr.open("iterPointsParam.txt",std::ios::out);
    ostr << "";
    ostr.close();

    //Now I'm going to save the initial starting position of the cameras
    ba_model.write_adjusted_cameras_append("iterCameraParam.txt");
    std::ofstream ostr_points("iterPointsParam.txt",std::ios::app);
    for (unsigned i = 0; i < ba_model.num_points(); ++i){
      Vector<double,3> current_point = ba_model.B_parameters(i);
      ostr_points << i << "\t" << current_point(0) << "\t" << current_point(1) << "\t" << current_point(2) << "\n";
    }
  }

  // Reporter
  BundleAdjustReport<AdjustSparse<HelicopterBundleAdjustmentModel, L2Error> >
    reporter( "RMAX Adjust", ba_model, bundle_adjuster, report_level );

  // Performing Bundle Adjustment
  double abs_tol = 1e10, rel_tol=1e10;
  while(bundle_adjuster.update(abs_tol, rel_tol)) {
    reporter.loop_tie_in();

    // Writing Current Camera Parameters to file for later reading in MATLAB
    if (vm.count("save-iteration-data")) {

      //Writing this iterations camera data
      ba_model.write_adjusted_cameras_append("iterCameraParam.txt");

      //Writing this iterations point data, also saving the pixel param data
      std::ofstream ostr_points("iterPointsParam.txt",std::ios::app);
      for (unsigned i = 0; i < ba_model.num_points(); ++i){

        Vector<double,3> current_point = ba_model.B_parameters(i);
        ostr_points << i << "\t" << current_point(0) << "\t" << current_point(1) << "\t" << current_point(2) << "\n";
      }
    }

    if (bundle_adjuster.iterations() > max_iterations || abs_tol < 0.01 || rel_tol < 1e-10)
      break;
  }
  reporter.end_tie_in();

  for (unsigned int i=0; i < ba_model.num_cameras(); ++i)
    ba_model.write_adjustment(i, prefix_from_filename(image_files[i])+".rmax_adjust");

  // Compute the post-adjustment residuals
  std::vector<boost::shared_ptr<CameraModel> > adjusted_cameras = ba_model.adjusted_cameras();
}


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <fstream>
#include <iostream>

#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/dipole_matrix.hpp"
#include "heom/dipole_pseudo_pathway.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/sites_to_states.hpp"
#include "heom/static_fluorescence_config.hpp"
#include "heom/thermal_state_search.hpp"
#include "heom/thermal_state_search_config.hpp"
#include "heom/thermal_state_search_solver.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using num::int_t;
using num::real_t;
using num::complex_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::static_fluorescence_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		using tss_solver_t = heom::thermal_state_search_solver<heom::ode, heom::thermal_state_search, heom::hierarchy_norm>;
		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// prepare output file (so we run into possible file errors before starting the computation)
		auto& observation_filename = heom_config.observations().get().front().get().second; // filename of first specified observation, guaranteed to exist by config checks
		std::ofstream observation_file(observation_filename);
		if (observation_file.fail())
			throw std::runtime_error("Error: could not open output file: " + observation_filename);

		// linear absorption pseudo pathway with ground state
		const auto& pathway_spec = heom::dipole_pseudo_pathway_wgs_spec;
		const auto sts_mode = pathway_spec.sites_to_states_mode();

		// compute thermal state
		heom::instance heom_instance_tss(heom_config, sts_mode, complete_graph);

		// OpenCL range
		ocl::nd_range range {
			{}, // offset
			{1, static_cast<std::uint64_t>(heom_instance_tss.matrices())}, // global size
			{1, 1} // local size
		};

		// create thermal state search solver (tss) from configuration and instance
		tss_solver_t tss_solver(ocl_config, range, heom_config, heom_instance_tss);

		std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
		tss_solver.write_ocl_runtime_config(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// determine thermal state until converged or max_steps is reached
		std::cout << "Computing thermal state..." << std::endl;
		int_t i = 0;
		for (; i < heom_config.thermal_state_search_max_steps(); ++i) {
			tss_solver.step_forward(1);
			// abort if convergence is reached
			if (tss_solver.test_convergence(heom_config.thermal_state_search_delta()))
				break;
		}
		if (i < heom_config.thermal_state_search_max_steps())
			std::cout << "... thermal state search converged after " << i << " steps." << std::endl;
		else
			std::cout << "... WARNING: thermal state search stopped after max_steps were reached (maybe increase max_steps, or delta)." << std::endl;

		// synchronise instance with tss solver results, i.e. transfer compute device to host memory
		tss_solver.update_instance();

		std::cout << "-------- Thermal State Search Solver Runtime Summary -------" << std::endl;
		tss_solver.write_runtime_summary(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// run the tss solver, no observations
		int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		int_t steps_per_iteration = heom_config.program_observe_steps();
		int_t total_steps = iterations * heom_config.program_observe_steps();

		// output data structure for traces
		const size_t num_dipole_matrices = heom_config.dipole_tensor_prefactors().size();
		const int_t num_observations = iterations + 1;
		std::vector<heom::matrix_trace_observation_view> observations(num_observations);

		// for progress estimate
		const size_t solver_runs = num_dipole_matrices;
		size_t solver_run = 0;

		// iterate over all dipoles and compute population dynamics
		for (size_t tensor_index = 0; tensor_index < num_dipole_matrices; ++tensor_index) {

			// copy thermal state instance
			heom::instance heom_instance { heom_instance_tss };
			// overwrite pseudo-hamiltonian of tss instance
			heom_instance.set_hamiltonian(state_hamiltonian(heom_config.system_hamiltonian(), sts_mode));

			// OpenCL range
			ocl::nd_range range {
				{}, // offset
				{1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
				{1, 1} // local size
			};

			// create a solver from configuration and instance
			solver_t solver(ocl_config, range, heom_config, heom_instance);
			std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
			solver.write_ocl_runtime_config(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;
			++solver_run; // for progress estimate

			// compute dipole matrices
			auto get_dipole_matrix = [&](size_t spec_index) {
				return heom::get_dipole_matrix_for_pathway(pathway_spec, spec_index, heom_config.system_sites(), heom_config, tensor_index);
			};
			auto dipole_matrix_minus = get_dipole_matrix(0);
			auto dipole_matrix_plus = get_dipole_matrix(1);
			dipole_matrix_minus.scale(heom::get_dipole_tensor_prefactor(pathway_spec, heom_config, tensor_index));

			// multiply D+ from right
			solver.hierarchy_mmult_right(dipole_matrix_plus.data());

			// setup observer with pre-scaled D-
			heom::matrix_trace_observer<solver_t> trace_observer(complete_graph, solver, dipole_matrix_minus.data()); // D- from left via observer

			// generate first observation with initial value
			observations[0] += trace_observer.observe_trace(0.0);

			// write header once
			if (tensor_index == 0)
				trace_observer.write_header(observation_file, true);

			for (int_t i = 0; i < iterations; ++i) {
				// propagate
				solver.step_forward(steps_per_iteration);

				// after propagation, we are at:
				const auto current_step = (i + 1) * steps_per_iteration;
				const real_t current_time = current_step * heom_config.solver_step_size();

				// observe
				observations[i + 1] += trace_observer.observe_trace(current_time);

				// update status
				heom::write_progress(current_step - 1, total_steps, solver_run, solver_runs, "Calculation static fluorescence: ", std::cout);
			}

			// write_complex_matrix(result_buffer_top, heom_instance.states(), std::cout);
			std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
			solver.write_runtime_summary(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;

		} // for tensor_index

		// write averaged traces
		for (auto& obs : observations) {
			obs.avg(num_dipole_matrices);
			observation_file << obs << '\n';
		}
		observation_file << std::flush; // tell the OS to write to disk

		std::cout << "-------------------- Memory Summary ------------------------" << std::endl;
		std::cout << "local hierarchy buffer size:            "
		          << boost::format("%11.2f MiB\n") % (heom_instance_tss.size_hierarchy_byte() / 1024.0 / 1024.0);
		std::cout << "local instance size:                    "
		          << boost::format("%11.2f MiB\n") % (heom_instance_tss.allocated_byte() / 1024.0 / 1024.0);
		std::cout << "max. heap via aligned::allocate(..):    "
		          << boost::format("%11.2f MiB") % (heom::memory::aligned::instance().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % heom::memory::aligned::instance().allocations_max()
		          << ')' << std::endl;
		std::cout << "max. OpenCL buffer allocations:         "
		          << boost::format("%11.2f MiB") % (tss_solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % tss_solver.ocl_helper().allocations_max()
		          << ')' << std::endl;
//		std::cout << "max. OpenCL buffer allocations:         "
//		          << boost::format("%11.2f MiB") % (solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
//		          << " ("
//		          << boost::format("count: %6i") % solver.ocl_helper().allocations_max()
//		          << ')' << std::endl;
		std::cout << "------------------------------------------------------------" << std::endl;

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "time in main():\t" << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}