#ifndef __CELL2UNIVARIATEQUADRATURE__CLASS__
#define __CELL2UNIVARIATEQUADRATURE__CLASS__

#include <iostream>
#include <iomanip>
#include "UnivariateQuadrature.hpp"

#include <Eigen/Dense>

// include Matlab headers
#include "mex.h"
#include "matrix.h"

/** \brief extends univariate quadrature and wraps univariate quadrature
*          formulas provided by matlab into a univariateQuadrature
*/
class Cell2univariateQuadrature : public UnivariateQuadrature {
 public:
  void initQuadrature(int maxLvl){
      // dummy routine to satisfy compiler, since virtual method has
      // to be instantiated
  };

  /** \brief transforms cell array into univariate quadrature some basic error
  *          checking is done. If an error occurs, Matlab takes care of it.
  */
  void initQuadrature(const mxArray *cell) {
    int n = 0;
    int m = 0;
    int ldCell = 0;
    const mxArray *pCell = NULL;
    const mwSize *dims = NULL;
    mwSize numDims = 0;

    numDims = mxGetNumberOfDimensions(cell);
    dims = mxGetDimensions(cell);
    ldCell = dims[0] < dims[1];

    if (numDims != 2 || dims[1 - ldCell] != 1)
      mexErrMsgIdAndTxt(
          "MATLAB:cell2univariateQuadrature",
          "cell array has to be of size (1,maxLvl+1) or (maxLvl+1,1).");

    // get memory for the quadrature vector
    _maxLvl = dims[ldCell] - 1;
    _Q.resize(_maxLvl + 1);
    for (mwSize i = 0; i <= (mwSize)_maxLvl; ++i) {
      // get pointer to cell entry and determine format
      pCell = mxGetCell(cell, i);
      m = mxGetM(pCell);
      n = mxGetN(pCell);
      if (m != 2)
        mexErrMsgIdAndTxt(
            "MATLAB:cell2univariateQuadrature",
            "each entry in the cell array has to be of the form [xi; "
            "w] where xi and w are row vectors.");
      // write points and weights to quadrature rule
      _Q[i].xi.resize(n);
      _Q[i].w.resize(n);
      double *Array = mxGetPr(pCell);
      for (int j = 0; j < n; ++j) {
        _Q[i].xi(j) = Array[2 * j];
        _Q[i].w(j) = Array[2 * j + 1];
      }
    }
  };

  /**    \brief checks if quadrature rules up to degree maxdeg exist
  *             if not, returns an error
  *      \param[in] maxLvl  maximum levl of the quadrature which is required
  */
  void resizeQuadrature(int maxLvl) {
    if (_maxLvl < maxLvl)
      mexErrMsgIdAndTxt("MATLAB:cell2univariateQuadrature",
                        "Too few univariate quadrature rules provided.");
  };

  /**    \brief testing routine for the Cell to univariate Quadrature.
  *      \param[in] maxLvl quadratures from lvl=0..maxLvl are printed
  *                 if maxLvl <= _maxLvl
  */
  void testQuadrature(int maxLvl) {
    maxLvl = _maxLvl < maxLvl ? _maxLvl : maxLvl;
    double sumw = 0;
    for (int i = 0; i <= maxLvl; ++i) {
      sumw = 0;
      for (int j = 0; j < _Q[i].xi.size(); ++j) {
        std::cout << std::setprecision(6) << "xi = " << std::setw(9)
                  << _Q[i].xi(j) << "\t"
                  << "w= " << std::setw(9) << _Q[i].w(j) << std::endl;
        sumw += _Q[i].w(j);
      }
      std::cout << "\nsum of weights: " << sumw << std::endl << std::endl;
      (void)maxLvl;
    };
  };
};
#endif


/*!
 * @file
 * Defines the @ref Monoid typeclass.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_MONOID_HPP
#define BOOST_MPL11_MONOID_HPP

#include <boost/mpl11/fwd/monoid.hpp>

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/core.hpp>
#include <boost/mpl11/detail/left_folds/variadic.hpp>

namespace boost { namespace mpl11 {
    template <typename Left, typename Right, typename>
    struct Monoid : false_ { };

    template <typename x1, typename x2, typename ...xn>
    struct plus
        : detail::left_folds::variadic<plus, x1, x2, xn...>
    { };

    template <typename x, typename y>
    struct plus<x, y> :
        Monoid<
            typename datatype<typename x::type>::type,
            typename datatype<typename y::type>::type
        >::template plus_impl<typename x::type, typename y::type>
    { };

    template <typename Datatype>
    struct zero : Monoid<Datatype>::template zero_impl<> { };
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_MONOID_HPP


/**
 * @file atomistics.hpp
 */

#ifndef SQSGENERATOR_ATOMISTICS_HPP
#define SQSGENERATOR_ATOMISTICS_HPP

#include "types.hpp"
#include "structure_utils.hpp"
#include <map>
#include <queue>
#include <vector>
#include <string>
#include <boost/multi_array.hpp>

using namespace boost;

namespace sqsgenerator::utils::atomistics {

    /**
     * Data class which holds some information of the periodic table. To construct instances, the static utility
     * functions of `Atoms`.
     */
    class Atom {

    public:
        const species_t Z; /**< The ordinal number of the element in the periodic table */
        const std::string name; /**< The full english name of the element. E. g. "Iron" */
        const std::string symbol; /**< The two character symbol of an element. E.g "Fe" for iron */
        const std::string electronic_configuration; /**< The electronic configuration as a string */
        const double atomic_radius; /**< Atomic radius in pm */
        const double mass; /**< The mass of the atoms in atomic units */
        const double en; /**< Electronegativity */
    };

    /**
     * Utility class, exposing public constructors for the `Atom` class
     * @see Atom
     */
    class Atoms {

    private:
        friend class Structure;
        const static std::vector<Atom> m_elements; /** < the list of elements available */
        static std::map<std::string , species_t> make_symbol_map() {
            static std::map<std::string, species_t> symbol_map;
            for( const auto &atom : Atoms::m_elements)  symbol_map.emplace(atom.symbol, atom.Z);
            return symbol_map;
        }

        static std::map<std::string , species_t> m_symbol_map;

    public:
        /**
         * resolves the ordinal number of an atomic element and looks up the Atom object
         * @param Z the ordinal number in the periodic table
         * @return the atom object
         */
        static Atom from_z(species_t Z);

        /***
         * Resolves a vector of ordinal number into a list of Atoms objects
         * @param numbers vector of ordinal numbers
         * @return vector of Atom objects
         */
        static std::vector<Atom> from_z(const std::vector<species_t> &numbers);

        /**
         * resolves the symbol of an atomic element e. g. "Fe" and looks up the Atom object. The input argument is
         * case sensitive
         * @param symbol a string with the species symbol
         * @return the Atom object
         */
        static Atom from_symbol(const std::string &symbol);

        /**
         * Resolves a vector of species symbols into a list of Atoms objects
         * @param symbols vector of symbols
         * @return vector of Atom objects
         */
        static std::vector<Atom> from_symbol(const std::vector<std::string> &symbols);

        static species_t symbol_to_z(const std::string &symbol);

        static std::vector<species_t> symbol_to_z(const std::vector<std::string> &symbol);

        static std::string z_to_symbol(species_t species);

        static std::vector<std::string> z_to_symbol(const std::vector<species_t> &species);

        static std::vector<Atom> all_elements();

    };

    class Structure {
    private:
        size_t m_natoms;
        array_2d_t m_lattice;
        array_2d_t m_frac_coords;
        array_2d_t m_distance_matrix;
        array_3d_t m_pbc_vecs;
        std::array<bool, 3> m_pbc;
        std::vector<Atom> m_species;

    public:
        Structure(const_array_2d_ref_t lattice, const_array_2d_ref_t frac_coords, std::vector<Atom> species, std::array<bool, 3> pbc);
        Structure(const_array_2d_ref_t lattice, const_array_2d_ref_t frac_coords, std::vector<std::string> species, std::array<bool, 3> pbc);
        Structure(const_array_2d_ref_t lattice, const_array_2d_ref_t frac_coords, configuration_t species, std::array<bool, 3> pbc);
        Structure(const Structure& other);
        Structure(Structure&& other);

        [[nodiscard]] Structure sorted() const;
        [[nodiscard]] Structure rearranged(const arrangement_t &order) const;
        [[nodiscard]] Structure with_species(const configuration_t &species) const;
        [[nodiscard]] size_t num_atoms() const;
        [[nodiscard]] const_array_2d_ref_t lattice() const;
        [[nodiscard]] const_array_2d_ref_t frac_coords() const;
        [[nodiscard]] std::array<bool, 3> pbc() const;
        [[nodiscard]] const std::vector<Atom>& species() const;
        [[nodiscard]] const_array_3d_ref_t distance_vecs() const;
        [[nodiscard]] const_array_2d_ref_t distance_matrix() const;
        [[nodiscard]] configuration_t configuration() const;
        [[nodiscard]] std::vector<std::string> symbols() const;
        [[nodiscard]] pair_shell_matrix_t shell_matrix(double atol=1.0e-5, double rtol=1.0e-8) const;
        [[nodiscard]] pair_shell_matrix_t shell_matrix(const std::vector<double> &shell_distances, double atol=1.0e-5, double rtol=1.0e-8) const;

        static std::vector<AtomPair> create_pair_list(pair_shell_matrix_t shell_matrix, const std::map<shell_t, double> &weights) {
            return sqsgenerator::utils::create_pair_list(shell_matrix, weights);
        }

    };
}

#endif //SQSGENERATOR_ATOMISTICS_HPP


#include "mem.hpp"
#include <boost/python.hpp>

namespace gd {
using namespace boost::python;

void py_export_mem() {

	class_< MEM, boost::noncopyable >("MEM", init<double, double, double>( (boost::python::arg("sigma"), boost::python::arg("gamma"), boost::python::arg("xmax"))   ))
		.def("__call__", (&MEM::operator()))
		.def("moment", (&MEM::moment))
		;
}

};


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_TEST_LAWS_MONOID_HPP
#define BOOST_HANA_TEST_LAWS_MONOID_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/concept/comparable.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/functional/capture.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/concept/monoid.hpp>

#include <laws/base.hpp>

namespace boost { namespace hana { namespace test {
    template <typename M, typename = when<true>>
    struct TestMonoid : TestMonoid<M, laws> {
        using TestMonoid<M, laws>::TestMonoid;
    };

    template <typename M>
    struct TestMonoid<M, laws> {
        template <typename Xs>
        TestMonoid(Xs xs) {
#ifdef BOOST_HANA_WORKAROUND_MSVC_DECLTYPEAUTO_RETURNTYPE_662735
            zero<M>(); // force adding zero<M>'s member function to pending temploid list
#endif

            hana::for_each(xs, hana::capture(xs)([](auto xs, auto a) {
                static_assert(Monoid<decltype(a)>{}, "");

                // left identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::plus(zero<M>(), a),
                    a
                ));

                // right identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::plus(a, zero<M>()),
                    a
                ));

                hana::for_each(xs,
                hana::capture(xs, a)([](auto xs, auto a, auto b) {
                    hana::for_each(xs,
                    hana::capture(a, b)([](auto a, auto b, auto c) {
                        // associativity
                        BOOST_HANA_CHECK(equal(
                            hana::plus(a, hana::plus(b, c)),
                            hana::plus(hana::plus(a, b), c)
                        ));
                    }));
                }));

            }));
        }
    };

    template <typename C>
    struct TestMonoid<C, when<Constant<C>::value>>
        : TestMonoid<C, laws>
    {
        template <typename Xs>
        TestMonoid(Xs xs) : TestMonoid<C, laws>{xs} {

            BOOST_HANA_CHECK(hana::equal(
                hana::value(zero<C>()),
                zero<typename C::value_type>()
            ));

            foreach2(xs, [](auto x, auto y) {
                BOOST_HANA_CHECK(hana::equal(
                    hana::plus(hana::value(x), hana::value(y)),
                    hana::value(hana::plus(x, y))
                ));
            });
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_LAWS_MONOID_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Copyright (c) 2016-2021 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_TEST_LENGTH_HPP
#define BOOST_GEOMETRY_TEST_LENGTH_HPP

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/length.hpp>

#ifndef BOOST_GEOMETRY_TEST_DEBUG
#include <boost/geometry/geometries/adapted/boost_variant.hpp>
#include <boost/geometry/geometries/adapted/boost_variant2.hpp>
#include <boost/geometry/geometries/geometry_collection.hpp>
#endif

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/strategies.hpp>

template <typename Geometry>
void test_length(Geometry const& geometry, long double expected_length)
{
    typename bg::default_length_result<Geometry>::type
        length = bg::length(geometry);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << std::endl
        << typeid(typename bg::default_length_result<Geometry>::type).name()
        << std::endl
        << "length : " << bg::length(geometry)
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(length, expected_length, 0.0001);
}

template <typename Geometry, typename Strategy>
void test_length(Geometry const& geometry, long double expected_length, Strategy strategy)
{
    typename bg::default_length_result<Geometry>::type
        length = bg::length(geometry, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << std::endl
        << typeid(typename bg::default_length_result<Geometry>::type).name()
        << std::endl
        << "length : " << bg::length(geometry, strategy)
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(length, expected_length, 0.0001);
}

template <typename Geometry>
void test_geometry(std::string const& wkt, double expected_length)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    test_length(geometry, expected_length);

#ifndef BOOST_GEOMETRY_TEST_DEBUG
    using variant_t = boost::variant<Geometry>;
    using variant2_t = boost::variant2::variant<Geometry>;
    using gc_t = bg::model::geometry_collection<variant2_t>;
    test_length(variant_t(geometry), expected_length);
    test_length(variant2_t(geometry), expected_length);
    test_length(gc_t{variant2_t(geometry), variant2_t(geometry)}, expected_length * 2);
#endif
}

template <typename Geometry, typename Strategy>
void test_geometry(std::string const& wkt, double expected_length, Strategy strategy)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    test_length(geometry, expected_length, strategy);

#ifndef BOOST_GEOMETRY_TEST_DEBUG
    using variant_t = boost::variant<Geometry>;
    using variant2_t = boost::variant2::variant<Geometry>;
    using gc_t = bg::model::geometry_collection<variant2_t>;
    test_length(variant_t(geometry), expected_length, strategy);
    test_length(variant2_t(geometry), expected_length, strategy);
    test_length(gc_t{variant2_t(geometry), variant2_t(geometry)}, expected_length * 2, strategy);
#endif
}

template <typename Geometry>
void test_empty_input(Geometry const& geometry)
{
    try
    {
        bg::length(geometry);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown" );
}

#endif


#include <Eigen/Core>
#include <iostream>
#include <cstring>

#include "drawmesh.h"
#include "glinclude.h"

using std::string;
using std::vector;

GLfloat color_cycle[][4] = {
    { 1., 0., 0., 1. },
    { 1., 1., 0., 1. },
    { 0., 1., 0., 1. },
    { 0., 1., 1., 1. },
    { 0., 0., 1., 1. },
    { 1., 0., 1., 1. }
};
int color_cycle_len = 6;

void draw_string(string s) {
    glDisable(GL_LIGHTING);

    int matrixMode;
    glGetIntegerv(GL_MATRIX_MODE, &matrixMode);  /* matrix mode? */

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    gluOrtho2D(0.0, 1.0, 0.0, 1.0);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();
    glPushAttrib(GL_COLOR_BUFFER_BIT);       /* save current colour */

    glRasterPos3f(0.01, 0.01, 0);
    glColor4f(1., 1., 1., 1.);
    for (unsigned int i = 0; i < s.length(); i++) {
        glutBitmapCharacter(GLUT_BITMAP_HELVETICA_10, s[i]);
    }

    glPopAttrib();
    glPopMatrix();
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(matrixMode);

    glEnable(GL_LIGHTING);
}

void vector3_to_gl(Vector3f v) {
    glVertex3f(v[0], v[1], v[2]);
}

void draw_vertex(vertex &v, face *face, drawopts opts) {
    if (!v.normal.isZero()) {
        glNormal3f(v.normal[0], v.normal[1], v.normal[2]);
    } else {
        glNormal3f(face->normal[0], face->normal[1], face->normal[2]);
    }
    vector3_to_gl(v.loc);
}

void draw_face(face* face, drawopts opts) {
    if (opts.draw_faces) {
        if (face->sides() == 3) glBegin(GL_TRIANGLES);
        //else if (face->sides() == 4) glBegin(GL_QUADS);
        else glBegin(GL_TRIANGLE_FAN);

        edge *e0 = face->e;
        edge *e = e0;
        do {
            draw_vertex(*e->vert, face, opts);
            e = e->next;
        } while (e != e0);

        glEnd();
    }
}

void draw_edges(face* face, drawopts opts) {
    glBegin(GL_LINE_STRIP); {
        edge *e0 = face->e;
        edge *e = e0;
        do {
            vector3_to_gl(e->vert->loc);
            e = e->next;
        } while (e != e0);
        vector3_to_gl(e0->vert->loc);
    } glEnd();
}

void draw_normals(face* face, drawopts opts) {

    for (int i = 0; i < 3; i++) {
        edge *e0 = face->e;
        edge *e = e0;
        do {
            vertex *v = e->vert;
            glBegin(GL_LINE_STRIP); {
                vector3_to_gl(v->loc);
                vector3_to_gl(v->loc + v->normal);
            } glEnd();
            e = e->next;
        } while (e != e0);
    }
}

void draw_hull(mesh &mesh, drawopts opts) {
    if (!opts.draw_hull) return;

    glEnable(GL_POLYGON_OFFSET_FILL);
    glPolygonOffset(1.0, 1.0);
    glLineWidth(2.0);
    glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, opts.hull_color);

    for (auto fit = mesh.faces.begin(); fit != mesh.faces.end(); fit++) {
        draw_edges(*fit, opts);
    }

    glutWireCube(1.);
}

void draw_faces(vector<face*> faces, drawopts opts) {
    glEnable(GL_POLYGON_OFFSET_FILL);
    glPolygonOffset(1.0, 1.0);

    glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, opts.mesh_color);
    for (auto it = faces.begin(); it != faces.end(); it++) {
        draw_face(*it, opts);
    }

    if (opts.draw_edges) {
        glLineWidth(2.0);
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, opts.edge_color);
        for (auto it = faces.begin(); it != faces.end(); it++) {
            draw_edges(*it, opts);
        }
    }
}

void draw_mesh(mesh &mesh, drawopts opts) {
    glEnable(GL_POLYGON_OFFSET_FILL);
    glPolygonOffset(1.0, 1.0);
    if (opts.draw_faces || opts.draw_verteces) {
        draw_faces(mesh.faces, opts);
    }

    if (opts.draw_normals) {
        glLineWidth(1.0);
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, opts.normal_color);
        for (auto v = mesh.verteces.begin(); v != mesh.verteces.end(); v++) {
            glBegin(GL_LINE_STRIP); {
                vector3_to_gl((*v)->loc);
                vector3_to_gl((*v)->loc + (*v)->normal);
            } glEnd();
        }
    }

    if (opts.draw_verteces) {
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, opts.mesh_color);
        for (auto v = mesh.verteces.begin(); v != mesh.verteces.end(); v++) {
            glPushMatrix(); {
                glTranslatef((*v)->loc[0], (*v)->loc[1], (*v)->loc[2]);
                glutSolidSphere(0.1, 5, 5);
            } glPopMatrix();
        }
    }
}

void draw_xy_plane(float z, bounds &b, drawopts opts) {
    glLineWidth(1.0);
    glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, opts.normal_color);

    float xp_lim = b.max_x + 2;
    float xn_lim = b.min_x - 2;
    float yp_lim = b.max_y + 2;
    float yn_lim = b.min_y - 2;

    float x_step = (xp_lim - xn_lim) / 20.;
    float y_step = (yp_lim - yn_lim) / 20.;

    for (float x = b.min_x; x <= b.max_x; x += x_step) {
        glBegin(GL_LINE_STRIP); {
            glVertex3f(x, yn_lim, z);
            glVertex3f(x, yp_lim, z);
        } glEnd();
    }
    for (float y = b.min_y; y <= b.max_y; y += y_step) {
        glBegin(GL_LINE_STRIP); {
            glVertex3f(xn_lim, y, z);
            glVertex3f(xp_lim, y, z);
        } glEnd();
    }
}

void draw_linesegs(vector<lineseg> lines, drawopts opts) {
    glLineWidth(3.0);
    glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, opts.hull_color);
    for (auto line = lines.begin(); line != lines.end(); line++) {
        glBegin(GL_LINE_STRIP); {
            vector3_to_gl(line->p1);
            vector3_to_gl(line->p2);
        } glEnd();
    }
}

void draw_perimeters(
        vector<Vector3f> verts, vector<vector<uint32_t>> perimeters,
        drawopts opts) {
    glLineWidth(3.0);
    int i = 0;
    for (auto perim = perimeters.begin(); perim != perimeters.end(); perim++) {
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color_cycle[i]);
        i = (i + 1) % color_cycle_len;
        glBegin(GL_LINE_STRIP); {
            for (auto vid = perim->begin(); vid != perim->end(); vid++) {
                vector3_to_gl(verts[*vid]);
            }
        } glEnd();
    }
}

void draw_path(path &p, drawopts opts) {
    glLineWidth(2.0);
    glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color_cycle[0]);
    glBegin(GL_LINE_STRIP); {
        for (auto vec = p.points.begin(); vec != p.points.end(); vec++) {
            vector3_to_gl(*vec);
        }
    } glEnd();
}

drawopts default_draw_options() {
    drawopts opts;
    opts.draw_edges = true;
    opts.draw_normals = false;
    opts.draw_faces = true;
    opts.draw_verteces = false;
    opts.draw_hull = false;

    opts.normal_color[0] = .0;
    opts.normal_color[1] = .0;
    opts.normal_color[2] = .0;
    opts.normal_color[3] = 1.;

    for (int i=0; i<3; i++) opts.edge_color[i] = 0.;
    opts.edge_color[3] = .2;

    opts.mesh_color[0] = 0.;
    opts.mesh_color[1] = .5;
    opts.mesh_color[2] = 1.;
    opts.mesh_color[3] = 1.;

    opts.hull_color[0] = 1.;
    opts.hull_color[1] = .0;
    opts.hull_color[2] = .0;
    opts.hull_color[3] = 1.;

    return opts;
}


//
// For licensing see accompanying LICENSE.txt file.
// Copyright (C) 2020 Apple Inc. All Rights Reserved.
//

#include <algorithm>
#include <cassert>
#include <iostream>
#include <limits>
#include <string>

#include <args.hxx>

#include <armadillo>

#include <embree3/rtcore.h>
#include <embree3/rtcore_scene.h>
#include <embree3/rtcore_ray.h>

#include <octomap/octomap.h>
#include <octomap/OcTree.h>

struct Vertex   { float x, y, z, a; };
struct Triangle { unsigned int v0, v1, v2; };

int main (int argc, const char** argv) {

    //
    // parse input arguments
    //

    args::ArgumentParser parser("generate_octomap", "");

    args::HelpFlag               help                            (parser, "__DUMMY__",                   "Display this help menu",      {'h', "help"});
    args::ValueFlag<std::string> mesh_vertices_file_arg          (parser, "MESH_VERTICES_FILE",          "mesh_vertices",               {"mesh_vertices_file"},          args::Options::Required);
    args::ValueFlag<std::string> mesh_faces_vi_file_arg          (parser, "MESH_FACES_VI_FILE",          "mesh_faces_vi_file",          {"mesh_faces_vi_file"},          args::Options::Required);
    args::ValueFlag<std::string> start_camera_positions_file_arg (parser, "START_CAMERA_POSITIONS_FILE", "start_camera_positions_file", {"start_camera_positions_file"}, args::Options::Required);
    args::ValueFlag<std::string> ray_directions_file_arg         (parser, "RAY_DIRECTIONS_FILE",         "ray_directions_file",         {"ray_directions_file"},         args::Options::Required);
    args::ValueFlag<std::string> ray_neighbor_indices_file_arg   (parser, "RAY_NEIGHBOR_INDICES_FILE",   "ray_neighbor_indices_file",   {"ray_neighbor_indices_file"},   args::Options::Required);
    args::ValueFlag<std::string> octomap_min_file_arg            (parser, "OCTOMAP_MIN_FILE",            "octomap_min_file",            {"octomap_min_file"},            args::Options::Required);
    args::ValueFlag<std::string> octomap_max_file_arg            (parser, "OCTOMAP_MAX_FILE",            "octomap_max_file",            {"octomap_max_file"},            args::Options::Required);
    args::ValueFlag<int>         n_iters_arg                     (parser, "N_ITERS",                     "n_iters",                     {"n_iters"},                     args::Options::Required);
    args::ValueFlag<float>       n_voxel_size_arg                (parser, "N_VOXEL_SIZE",                "n_voxel_size",                {"n_voxel_size"},                args::Options::Required);
    args::ValueFlag<std::string> octomap_file_arg                (parser, "OCTOMAP_FILE",                "octomap_file",                {"octomap_file"},                args::Options::Required);
    args::ValueFlag<std::string> free_space_min_file_arg         (parser, "FREE_SPACE_MIN_FILE",         "free_space_min_file",         {"free_space_min_file"},         args::Options::Required);
    args::ValueFlag<std::string> free_space_max_file_arg         (parser, "FREE_SPACE_MAX_FILE",         "free_space_max_file",         {"free_space_max_file"},         args::Options::Required);
    args::Flag                   silent_arg                      (parser, "__DUMMY__",                   "silent",                      {"silent"});

    try {
        parser.ParseCLI(argc, argv);
    } catch (args::Completion e) {
        std::cout << parser;
        return 1;
    } catch (args::Help e) {
        std::cout << parser;
        return 1;
    } catch (args::ParseError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    } catch (args::RequiredError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    }

    auto mesh_vertices_file          = args::get(mesh_vertices_file_arg);
    auto mesh_faces_vi_file          = args::get(mesh_faces_vi_file_arg);
    auto start_camera_positions_file = args::get(start_camera_positions_file_arg);
    auto ray_directions_file         = args::get(ray_directions_file_arg);
    auto ray_neighbor_indices_file   = args::get(ray_neighbor_indices_file_arg);
    auto octomap_min_file            = args::get(octomap_min_file_arg);
    auto octomap_max_file            = args::get(octomap_max_file_arg);
    auto n_iters                     = args::get(n_iters_arg);
    auto n_voxel_size                = args::get(n_voxel_size_arg);
    auto octomap_file                = args::get(octomap_file_arg);
    auto free_space_min_file         = args::get(free_space_min_file_arg);
    auto free_space_max_file         = args::get(free_space_max_file_arg);
    auto silent                      = args::get(silent_arg);

    arma::mat mesh_vertices, start_camera_positions, ray_directions;
    arma::imat mesh_faces_vi;
    arma::umat ray_neighbor_indices;
    arma::vec octomap_min, octomap_max;

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Begin..." << std::endl;
    }

    //
    // load input data
    //

    mesh_vertices.load(mesh_vertices_file, arma::hdf5_binary_trans);
    mesh_faces_vi.load(mesh_faces_vi_file, arma::hdf5_binary_trans);
    start_camera_positions.load(start_camera_positions_file, arma::hdf5_binary_trans);
    ray_directions.load(ray_directions_file, arma::hdf5_binary_trans);
    ray_neighbor_indices.load(ray_neighbor_indices_file, arma::hdf5_binary_trans);
    octomap_min.load(octomap_min_file, arma::hdf5_binary_trans);
    octomap_max.load(octomap_max_file, arma::hdf5_binary_trans);

    assert(mesh_vertices.n_cols == 3);
    assert(mesh_faces_vi.n_cols == 3);
    assert(start_camera_positions.n_cols == 3);
    assert(ray_directions.n_cols == 3);
    assert(ray_neighbor_indices.n_rows == ray_directions.n_rows);
    assert(octomap_min.n_rows == 3);
    assert(octomap_max.n_rows == 3);

    arma::uvec ray_neighbor_indices_vec = arma::vectorise(ray_neighbor_indices);

    if (!silent) {
        octomap_min.raw_print(std::cout, "octomap_min = ");
        octomap_max.raw_print(std::cout, "octomap_max = ");
    }

    //
    // construct Embree data
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Constructing Embree data..." << std::endl;
    }

    assert(RTC_MAX_INSTANCE_LEVEL_COUNT == 1);

    auto rtc_device = rtcNewDevice(nullptr);
    assert(rtcGetDeviceError(nullptr) == RTC_ERROR_NONE);

    auto rtc_scene = rtcNewScene(rtc_device);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    auto rtc_triangle_mesh = rtcNewGeometry(rtc_device, RTC_GEOMETRY_TYPE_TRIANGLE);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    // set vertices
    auto rtc_vertices = (Vertex*) rtcSetNewGeometryBuffer(rtc_triangle_mesh, RTC_BUFFER_TYPE_VERTEX, 0, RTC_FORMAT_FLOAT3, sizeof(Vertex), mesh_vertices.n_rows);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    for (int i = 0; i < mesh_vertices.n_rows; i++) {
        rtc_vertices[i].x = mesh_vertices(i,0);
        rtc_vertices[i].y = mesh_vertices(i,1);
        rtc_vertices[i].z = mesh_vertices(i,2);
    }

    // set triangles
    auto rtc_triangles = (Triangle*) rtcSetNewGeometryBuffer(rtc_triangle_mesh, RTC_BUFFER_TYPE_INDEX, 0, RTC_FORMAT_UINT3, sizeof(Triangle), mesh_faces_vi.n_rows);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    for (int i = 0; i < mesh_faces_vi.n_rows; i++) {
        rtc_triangles[i].v0 = mesh_faces_vi(i,0);
        rtc_triangles[i].v1 = mesh_faces_vi(i,1);
        rtc_triangles[i].v2 = mesh_faces_vi(i,2);
    }

    rtcCommitGeometry(rtc_triangle_mesh);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    unsigned int geomID = rtcAttachGeometry(rtc_scene, rtc_triangle_mesh);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    rtcReleaseGeometry(rtc_triangle_mesh);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    rtcCommitScene(rtc_scene);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    RTCIntersectContext rtc_intersect_context;
    rtcInitIntersectContext(&rtc_intersect_context);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    //
    // sample mesh
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Generating mesh samples..." << std::endl;
    }

    octomap::OcTree octree_octomap(n_voxel_size);
    octomap::point3d octomap_min_octomap(octomap_min(0), octomap_min(1), octomap_min(2));
    octomap::point3d octomap_max_octomap(octomap_max(0), octomap_max(1), octomap_max(2));
    octree_octomap.setBBXMin(octomap_min_octomap);
    octree_octomap.setBBXMax(octomap_max_octomap);

    octomap::KeySet occupied_cells_octomap;

    for (int i = 0; i < mesh_faces_vi.n_rows; i++) {

        arma::vec v_0 = mesh_vertices.row(mesh_faces_vi(i,0)).t();
        arma::vec v_1 = mesh_vertices.row(mesh_faces_vi(i,1)).t();
        arma::vec v_2 = mesh_vertices.row(mesh_faces_vi(i,2)).t();

        // if v_0 or v_1 or v_2 is inside our bounding box, then sample face
        if ((arma::all(v_0 >= octomap_min) && arma::all(v_0 <= octomap_max)) ||
            (arma::all(v_1 >= octomap_min) && arma::all(v_1 <= octomap_max)) ||
            (arma::all(v_2 >= octomap_min) && arma::all(v_2 <= octomap_max))) {

            octomap::point3d v_0_octomap(v_0(0), v_0(1), v_0(2));
            octomap::point3d v_1_octomap(v_1(0), v_1(1), v_1(2));
            octomap::point3d v_2_octomap(v_2(0), v_2(1), v_2(2));
            octomap::OcTreeKey v_0_key_octomap, v_1_key_octomap, v_2_key_octomap;
            bool key_valid;
            key_valid = octree_octomap.coordToKeyChecked(v_0_octomap, v_0_key_octomap);
            assert(key_valid);
            key_valid = octree_octomap.coordToKeyChecked(v_1_octomap, v_1_key_octomap);
            assert(key_valid);
            key_valid = octree_octomap.coordToKeyChecked(v_2_octomap, v_2_key_octomap);
            assert(key_valid);

            if ((v_0_key_octomap == v_1_key_octomap) && (v_1_key_octomap == v_2_key_octomap)) {
                // if v_0 and v_1 and v_2 are all in the same octomap cell, then add v_0 to samples
                occupied_cells_octomap.insert(v_0_key_octomap);
            } else {

                // otherwise we need to generate samples over the face
                arma::vec p_c  = (v_0+v_1+v_2)/3.0;

                arma::vec d_c0 = v_0 - p_c;
                arma::vec d_c1 = v_1 - p_c;

                arma::vec tangent_space_x = arma::normalise(d_c0);
                arma::vec tangent_space_z = arma::normalise(arma::cross(d_c0, d_c1));
                arma::vec tangent_space_y = arma::normalise(arma::cross(tangent_space_z, tangent_space_x));

                arma::mat R_world_from_tangent = arma::join_rows(arma::join_rows(tangent_space_x, tangent_space_y), tangent_space_z);
                arma::vec t_world_from_tangent = p_c;

                arma::mat R_tangent_from_world = R_world_from_tangent.t();
                arma::vec t_tangent_from_world = -t_world_from_tangent;

                arma::vec v_0_tangent = R_tangent_from_world*(v_0 + t_tangent_from_world);
                arma::vec v_1_tangent = R_tangent_from_world*(v_1 + t_tangent_from_world);
                arma::vec v_2_tangent = R_tangent_from_world*(v_2 + t_tangent_from_world);

                arma::mat verts_tangent = arma::join_cols(arma::join_cols(v_0_tangent.t(), v_1_tangent.t()), v_2_tangent.t());

                arma::vec verts_tangent_min = arma::min(verts_tangent, 0).t();
                arma::vec verts_tangent_max = arma::max(verts_tangent, 0).t();

                auto grid_spacing = 0.25*n_voxel_size; // generate grid of points in tangent space with this much space between points
                auto eps = 0.001*n_voxel_size;         // make the grid extend slightly beyond the min and max extent of each face

                auto verts_tangent_min_x          = verts_tangent_min(0) - eps;
                auto verts_tangent_max_x          = verts_tangent_max(0) + eps;
                arma::vec grid_x_neg_conservative = arma::reverse(arma::regspace(0, -grid_spacing, verts_tangent_min_x));
                arma::vec grid_x_pos_conservative = arma::regspace(0, grid_spacing, verts_tangent_max_x);
                arma::vec grid_x_neg              = arma::join_cols(arma::vec({grid_x_neg_conservative(0) - grid_spacing}), grid_x_neg_conservative);
                arma::vec grid_x_pos              = arma::join_cols(grid_x_pos_conservative, arma::vec({grid_x_pos_conservative.tail(1) + grid_spacing}));
                arma::vec grid_x                  = arma::join_cols(grid_x_neg.head(grid_x_neg.n_elem - 1), grid_x_pos);

                auto verts_tangent_min_y          = verts_tangent_min(1) - eps;
                auto verts_tangent_max_y          = verts_tangent_max(1) + eps;
                arma::vec grid_y_neg_conservative = arma::reverse(arma::regspace(0, -grid_spacing, verts_tangent_min_y));
                arma::vec grid_y_pos_conservative = arma::regspace(0, grid_spacing, verts_tangent_max_y);
                arma::vec grid_y_neg              = arma::join_cols(arma::vec({grid_y_neg_conservative(0) - grid_spacing}), grid_y_neg_conservative);
                arma::vec grid_y_pos              = arma::join_cols(grid_y_pos_conservative, arma::vec({grid_y_pos_conservative.tail(1) + grid_spacing}));
                arma::vec grid_y                  = arma::join_cols(grid_y_neg.head(grid_y_neg.n_elem - 1), grid_y_pos);

                // precompute these values outside the for loop to compute barycentric coordinates
                // see https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
                arma::vec a_ = v_0_tangent;
                arma::vec b_ = v_1_tangent;
                arma::vec c_ = v_2_tangent;
                arma::vec v_0_ = b_ - a_;
                arma::vec v_1_ = c_ - a_;
                auto d_00_     = arma::dot(v_0_, v_0_);
                auto d_01_     = arma::dot(v_0_, v_1_);
                auto d_11_     = arma::dot(v_1_, v_1_);

                for (auto y_tangent : grid_y) {
                    for (auto x_tangent : grid_x) {

                        arma::vec p_tangent({x_tangent, y_tangent, 0});
                        arma::vec p = R_world_from_tangent*p_tangent + t_world_from_tangent;

                        // if sample point is inside our bounding box
                        if (arma::all(p >= octomap_min) && arma::all(p <= octomap_max)) {

                            // compute barycentric coordinates for sample point
                            arma::vec p_   = p_tangent;
                            arma::vec v_2_ = p_ - a_;

                            auto d_20_  = arma::dot(v_2_, v_0_);
                            auto d_21_  = arma::dot(v_2_, v_1_);
                            auto denom_ = d_00_ * d_11_ - d_01_ * d_01_;
                            auto v_     = (d_11_ * d_20_ - d_01_ * d_21_) / denom_;
                            auto w_     = (d_00_ * d_21_ - d_01_ * d_20_) / denom_;
                            auto u_     = 1.0 - v_ - w_;

                            // if sample point is inside the current face, then add it to our octomap
                            if (u_ > 0 && v_ > 0 && w_ > 0) {
                                octomap::point3d p_octomap(p(0), p(1), p(2));
                                octomap::OcTreeKey p_key_octomap;
                                auto key_valid = octree_octomap.coordToKeyChecked(p_octomap, p_key_octomap);
                                assert(key_valid);
                                occupied_cells_octomap.insert(p_key_octomap);
                            }
                        }
                    }
                }

                // due to numerical issues with very small triangles, the triangle center is not guaranteed
                // to be added in the loop above, so add it here explicitly
                if (arma::all(p_c >= octomap_min) && arma::all(p_c <= octomap_max)) {
                    octomap::point3d p_c_octomap(p_c(0), p_c(1), p_c(2));
                    octomap::OcTreeKey p_c_key_octomap;
                    auto key_valid = octree_octomap.coordToKeyChecked(p_c_octomap, p_c_key_octomap);
                    assert(key_valid);
                    occupied_cells_octomap.insert(p_c_key_octomap);
                }
            }
        }
    }

    //
    // pre-fill octomap with known occupied space from the mesh samples
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Updating Octomap..." << std::endl;
    }

    for (auto it : occupied_cells_octomap) {
        auto occupied  = true;
        auto lazy_eval = true;
        octree_octomap.updateNode(it, occupied, lazy_eval);
    }

    octree_octomap.updateInnerOccupancy();

    // if (!silent) {
    //     std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Writing Octomap..." << std::endl;
    // }

    // octree_octomap.writeBinary(octomap_file);

    //
    // perform space carving
    //

    arma::arma_rng::set_seed(0);
    std::cout.precision(20);
    std::cout.setf(std::ios::fixed);

    arma::vec free_space_min({std::numeric_limits<float>::infinity(), std::numeric_limits<float>::infinity(), std::numeric_limits<float>::infinity()});
    arma::vec free_space_max({-std::numeric_limits<float>::infinity(), -std::numeric_limits<float>::infinity(), -std::numeric_limits<float>::infinity()});

    auto n_max_ray_length      = arma::norm(octomap_max - octomap_min);
    auto n_ray_sample_step     = 0.25*n_voxel_size; // we may need to perturb our starting position so it is not in occupied space; use this step size when perturbing
    auto encountered_free_cell = false;

    if (!silent) {
        free_space_min.raw_print(std::cout, "free_space_min = ");
        free_space_max.raw_print(std::cout, "free_space_max = ");
    }

    // for all starting camera positions...
    for (int i = 0; i < start_camera_positions.n_rows; i++) {

        if (!silent) {
            std::cout << "[HYPERSIM: GENERATE_OCTOMAP] i = " << i << std::endl;
            start_camera_positions.row(i).raw_print(std::cout, "start_camera_positions[i] = ");
        }

        //
        // attempt to perturb the starting camera position in case it is in an occupied cell
        //

        if (!silent) {
            std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Verifying start_camera_position[i]..." << std::endl;
        }

        arma::vec camera_position = start_camera_positions.row(i).t();
        arma::vec intersection_distances = arma::ones<arma::vec>(ray_directions.n_rows) * std::numeric_limits<float>::infinity();

        // for each ray direction...
        for (int k = 0; k < ray_directions.n_rows; k++) {

            arma::vec ray_direction_normalized = arma::normalise(ray_directions.row(k).t());

            RTCRayHit rtc_ray_hit;

            rtc_ray_hit.ray.org_x = (float)camera_position(0);
            rtc_ray_hit.ray.org_y = (float)camera_position(1);
            rtc_ray_hit.ray.org_z = (float)camera_position(2);
            rtc_ray_hit.ray.dir_x = (float)ray_direction_normalized(0);
            rtc_ray_hit.ray.dir_y = (float)ray_direction_normalized(1);
            rtc_ray_hit.ray.dir_z = (float)ray_direction_normalized(2);
            rtc_ray_hit.ray.tnear = 0.0f;
            rtc_ray_hit.ray.tfar  = std::numeric_limits<float>::infinity();
            rtc_ray_hit.ray.time  = 0.0f;
            rtc_ray_hit.ray.mask  = 0;
            rtc_ray_hit.ray.id    = 0;
            rtc_ray_hit.ray.flags = 0;

            rtc_ray_hit.hit.Ng_x      = std::numeric_limits<float>::infinity();
            rtc_ray_hit.hit.Ng_y      = std::numeric_limits<float>::infinity();
            rtc_ray_hit.hit.Ng_z      = std::numeric_limits<float>::infinity();
            rtc_ray_hit.hit.u         = std::numeric_limits<float>::infinity();
            rtc_ray_hit.hit.v         = std::numeric_limits<float>::infinity();
            rtc_ray_hit.hit.primID    = RTC_INVALID_GEOMETRY_ID;
            rtc_ray_hit.hit.geomID    = RTC_INVALID_GEOMETRY_ID;
            rtc_ray_hit.hit.instID[0] = RTC_INVALID_GEOMETRY_ID;

            rtcIntersect1(rtc_scene, &rtc_intersect_context, &rtc_ray_hit);

            if (rtc_ray_hit.ray.tfar < std::numeric_limits<float>::infinity()) {
                assert(rtc_ray_hit.hit.primID != RTC_INVALID_GEOMETRY_ID);
                assert(rtc_ray_hit.hit.geomID != RTC_INVALID_GEOMETRY_ID);
            } else {
                assert(rtc_ray_hit.hit.primID == RTC_INVALID_GEOMETRY_ID);
                assert(rtc_ray_hit.hit.geomID == RTC_INVALID_GEOMETRY_ID);
            }

            if (rtc_ray_hit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                auto intersection_distance = rtc_ray_hit.ray.tfar;
                intersection_distances(k) = intersection_distance;
            }
        }

        arma::vec intersection_distances_neighbors_vec = intersection_distances.elem(ray_neighbor_indices_vec);
        arma::mat intersection_distances_neighbors     = arma::reshape(intersection_distances_neighbors_vec, ray_neighbor_indices.n_rows, ray_neighbor_indices.n_cols);
        arma::vec intersection_distances_min           = arma::min(intersection_distances_neighbors, 1); // perform min-pooling operation
        intersection_distances                         = intersection_distances_min;
        arma::uvec intersection_distances_argsort      = sort_index(intersection_distances, "descend");
        auto encountered_unknown_or_free_cell          = false;
        auto num_perturbation_attempts                 = 0;
        auto num_perturbation_directions               = 0;

        // for each ray direction...
        for (int k = 0; k < intersection_distances_argsort.n_rows; k++) {

            auto max_ray_length                             = std::min(intersection_distances(intersection_distances_argsort(k)),n_max_ray_length);
            arma::vec ray_direction_normalized              = arma::normalise(ray_directions.row(intersection_distances_argsort(k)).t());
            arma::vec query_position                        = camera_position;
            auto current_ray_length                         = 0.0;
            auto attempted_to_perturb_for_current_direction = false;

            // use a conservative sampling approach here instead of computing the exact set of voxels intersected by the ray;
            // we could also use an exact approach (as we do later in this source file), but it seems wasteful because we
            // probably don't need to do much perturbing to find an unknown or free voxel
            while (current_ray_length < max_ray_length && !encountered_unknown_or_free_cell) {

                if (!(arma::all(query_position >= octomap_min) && arma::all(query_position <= octomap_max))) {
                    break;
                }

                octomap::point3d query_position_octomap(query_position(0), query_position(1), query_position(2));
                auto node_octomap = octree_octomap.search(query_position_octomap);

                if (node_octomap == nullptr) {
                    camera_position                  = query_position;
                    encountered_unknown_or_free_cell = true;
                    break;
                } else if (node_octomap->getOccupancy() <= octree_octomap.getOccupancyThres()) {
                    camera_position                  = query_position;
                    encountered_unknown_or_free_cell = true;
                    break;
                }

                current_ray_length += n_ray_sample_step;
                query_position = camera_position + current_ray_length*ray_direction_normalized;
                num_perturbation_attempts += 1;
                if (!attempted_to_perturb_for_current_direction) {
                    attempted_to_perturb_for_current_direction = true;
                    num_perturbation_directions += 1;
                }
            }

            if (encountered_unknown_or_free_cell) {
                break;
            }
        }

        if (encountered_unknown_or_free_cell) {
            if (!silent) {
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Finished verifying start_camera_position[i]." << std::endl;
                camera_position.raw_print(std::cout, "camera_position = ");
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Attempted " << num_perturbation_attempts << " perturbations along " << num_perturbation_directions << " distinct directions..." << std::endl;
            }
        } else {
            if (!silent) {
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP]" << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP]" << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP]" << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] WARNING: COULD NOT VERIFY start_camera_position[i], GIVING UP." << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Attempted " << num_perturbation_attempts << " perturbations along " << num_perturbation_directions << " distinct directions..." << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP]" << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP]" << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP]" << std::endl;
            }
            continue;
        }

        //
        // at this point, the camera position is guaranteed to be in free or unknown space...
        //

        // for all iterations...
        for (int j = 0; j < n_iters; j++) {

            if (!silent) {
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] j = " << j << std::endl;
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Performing space carving along rays..." << std::endl;
            }

            // sanity check the camera position prior to space carving
            auto node = octree_octomap.search(octomap::point3d(camera_position(0), camera_position(1), camera_position(2)));
            if (node != nullptr) {
                // if the camera position is in known space, then it must be unoccupied
                assert(node->getOccupancy() <= octree_octomap.getOccupancyThres());
            }

            //
            // raycast against triangle mesh
            //

            if (!silent) {
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Raycasting against triangle mesh..." << std::endl;
            }

            octomap::KeySet occupied_cells_octomap_current;
            arma::vec intersection_distances = arma::ones<arma::vec>(ray_directions.n_rows) * std::numeric_limits<float>::infinity();

            // for each ray direction...
            for (int k = 0; k < ray_directions.n_rows; k++) {

                arma::vec ray_direction_normalized = arma::normalise(ray_directions.row(k).t());

                RTCRayHit rtc_ray_hit;

                rtc_ray_hit.ray.org_x = (float)camera_position(0);
                rtc_ray_hit.ray.org_y = (float)camera_position(1);
                rtc_ray_hit.ray.org_z = (float)camera_position(2);
                rtc_ray_hit.ray.dir_x = (float)ray_direction_normalized(0);
                rtc_ray_hit.ray.dir_y = (float)ray_direction_normalized(1);
                rtc_ray_hit.ray.dir_z = (float)ray_direction_normalized(2);
                rtc_ray_hit.ray.tnear = 0.0f;
                rtc_ray_hit.ray.tfar  = std::numeric_limits<float>::infinity();
                rtc_ray_hit.ray.time  = 0.0f;
                rtc_ray_hit.ray.mask  = 0;
                rtc_ray_hit.ray.id    = 0;
                rtc_ray_hit.ray.flags = 0;

                rtc_ray_hit.hit.Ng_x      = std::numeric_limits<float>::infinity();
                rtc_ray_hit.hit.Ng_y      = std::numeric_limits<float>::infinity();
                rtc_ray_hit.hit.Ng_z      = std::numeric_limits<float>::infinity();
                rtc_ray_hit.hit.u         = std::numeric_limits<float>::infinity();
                rtc_ray_hit.hit.v         = std::numeric_limits<float>::infinity();
                rtc_ray_hit.hit.primID    = RTC_INVALID_GEOMETRY_ID;
                rtc_ray_hit.hit.geomID    = RTC_INVALID_GEOMETRY_ID;
                rtc_ray_hit.hit.instID[0] = RTC_INVALID_GEOMETRY_ID;

                rtcIntersect1(rtc_scene, &rtc_intersect_context, &rtc_ray_hit);

                if (rtc_ray_hit.ray.tfar < std::numeric_limits<float>::infinity()) {
                    assert(rtc_ray_hit.hit.primID != RTC_INVALID_GEOMETRY_ID);
                    assert(rtc_ray_hit.hit.geomID != RTC_INVALID_GEOMETRY_ID);
                } else {
                    assert(rtc_ray_hit.hit.primID == RTC_INVALID_GEOMETRY_ID);
                    assert(rtc_ray_hit.hit.geomID == RTC_INVALID_GEOMETRY_ID);
                }

                if (rtc_ray_hit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                    auto intersection_distance = rtc_ray_hit.ray.tfar;
                    arma::vec intersection_position = camera_position + intersection_distance*ray_direction_normalized;
                    intersection_distances(k) = intersection_distance;
                }
            }

            //
            // raycast against octomap
            //

            if (!silent) {
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Raycasting against Octomap..." << std::endl;
            }

            octomap::KeySet free_cells_octomap;
            octomap::KeyRay key_rays_octomap;

            // for each ray direction...
            for (int k = 0; k < ray_directions.n_rows; k++) {

                auto ray_length                    = std::min(intersection_distances(k),n_max_ray_length);
                arma::vec ray_direction_normalized = arma::normalise(ray_directions.row(k).t());
                arma::vec query_position           = camera_position;
                arma::vec end_position             = query_position + ray_length*ray_direction_normalized;
                octomap::point3d query_position_octomap(query_position(0), query_position(1), query_position(2));
                octomap::point3d end_position_octomap(end_position(0), end_position(1), end_position(2));

                // compute the exact set of voxels intersected by ray
                key_rays_octomap.reset();
                auto success = octree_octomap.computeRayKeys(query_position_octomap, end_position_octomap, key_rays_octomap);
                assert(success);

                for(auto it : key_rays_octomap) {

                    if (!octree_octomap.inBBX(it)) {
                        break;
                    }

                    auto node_octomap = octree_octomap.search(it);
                    if (node_octomap == nullptr) {
                        // if we encounter unknown space, then mark it as free because we're somewhere along a ray that starts in free space and hasn't encountered surface geometry yet
                        free_cells_octomap.insert(it);
                        auto key_position_octomap = octree_octomap.keyToCoord(it);
                        arma::vec key_position({ key_position_octomap.x(), key_position_octomap.y(), key_position_octomap.z() });
                        free_space_max = arma::max(free_space_max, key_position);
                        free_space_min = arma::min(free_space_min, key_position);
                        encountered_free_cell = true;
                    } else {
                        if (node_octomap->getOccupancy() > octree_octomap.getOccupancyThres()) {
                            // if we encounter occupied space, then terminate space carving for this ray
                            break;
                        }
                    }
                }
            }

            // make sure that we have found at least one free cell, since we sample within the free space to choose the next camera position
            assert(encountered_free_cell);

            if (!silent) {
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Updating Octomap..." << std::endl;
            }

            for (auto it : free_cells_octomap) {
                auto occupied  = false;
                auto lazy_eval = true;
                octree_octomap.updateNode(it, occupied, lazy_eval);
            }

            octree_octomap.updateInnerOccupancy();

            if (!silent) {
                std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Finding free camera position..." << std::endl;
                free_space_min.raw_print(std::cout, "free_space_min = ");
                free_space_max.raw_print(std::cout, "free_space_max = ");
            }

            bool query_position_valid = false;
            while (!query_position_valid) {

                arma::vec free_space_extent = free_space_max - free_space_min;
                arma::vec query_position     = arma::randu<arma::vec>(3) % free_space_extent + free_space_min;

                auto node_octomap = octree_octomap.search(octomap::point3d(query_position(0), query_position(1), query_position(2)));
                if (node_octomap != nullptr) {
                    if (node_octomap->getOccupancy() <= octree_octomap.getOccupancyThres()) {
                        query_position_valid = true;
                        camera_position = query_position;

                        if (!silent) {
                            camera_position.raw_print(std::cout, "camera_position = ");
                        }
                    }
                }
            }
        }
    }

    //
    // save octomap
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Writing Octomap..." << std::endl;
    }

    free_space_min.save(free_space_min_file, arma::hdf5_binary_trans);
    free_space_max.save(free_space_max_file, arma::hdf5_binary_trans);

    octree_octomap.writeBinary(octomap_file);

    //
    // release Embree data
    //

    rtcReleaseScene(rtc_scene); rtc_scene = nullptr;
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    rtcReleaseDevice(rtc_device); rtc_device = nullptr;
    assert(rtcGetDeviceError(nullptr) == RTC_ERROR_NONE);

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP] Finished." << std::endl;
    }

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::response::right_truncated::include.hpp      		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/event.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/event_factory.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/mean_event.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/meta_factory.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/is_event.hpp>

#endif


#include <boost/compute/algorithm/nth_element.hpp>


#pragma once

#include <gtest/gtest.h>
#include <boost/config.hpp>

#include <algorithm>
#include <vector>
#include <utility>
#include <iostream>

#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace boost;

TEST(Boost, Create) {

  typedef boost::adjacency_list< boost::mapS, boost::vecS,
    boost::bidirectionalS,
    boost::property< boost::vertex_color_t, boost::default_color_type,
    boost::property< boost::vertex_degree_t, int,
    boost::property< boost::vertex_in_degree_t, int,
    boost::property< boost::vertex_out_degree_t, int > > > > >
    Graph;

  Graph G(5);

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGORITHMS_OFFSET_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGORITHMS_OFFSET_HPP

#include <boost/config.hpp>

#include <boost/range/functions.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/extensions/algorithms/buffer/buffer_inserter.hpp>
#include <boost/geometry/extensions/strategies/buffer.hpp>
#include <boost/geometry/algorithms/detail/disjoint.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/geometries/segment.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace offset
{

template
<
    typename Range,
    typename RangeOut
>
struct offset_range
    : public geometry::detail::buffer::buffer_range
        <
            RangeOut,
            linestring_tag
        >
{
    template
    <
        typename Collection, typename DistanceStrategy, typename JoinStrategy
    >
    static inline void apply(Collection& collection, Range const& range,
                DistanceStrategy const& distance,
                JoinStrategy const& join)
    {
        collection.add_input();
        iterate(collection, boost::begin(range), boost::end(range),
            buffer_side_left,
            distance, join);
    }
};

}} // namespace detail::offset
#endif

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename GeometryTag,
    typename GeometryOutTag,
    typename Geometry,
    typename GeometryOut
>
struct offset
{};

template
<
    typename Geometry,
    typename GeometryOut
>
struct offset
    <
        linestring_tag,
        linestring_tag,
        Geometry,
        GeometryOut
    >
    : detail::offset::offset_range
        <
            Geometry,
            GeometryOut
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template
<
    typename Geometry,
    typename GeometryOut,
    typename JoinStrategy,
    typename Distance
>
inline void offset(Geometry const& geometry, GeometryOut& out,
            JoinStrategy const& join,
            Distance const& distance)
{
    concept::check<Geometry const>();
    concept::check<GeometryOut>();

    typedef strategy::buffer::distance_assymetric
        <
            typename geometry::coordinate_type<Geometry>::type
        > distance_strategy_type;
    distance_strategy_type distance_strategy(distance, distance);

    detail::buffer::buffered_piece_collection
        <
            //typename geometry::ring_type<GeometryOut>::type
            // TODO the piece collection will not require a polygonal argument
            model::ring<typename point_type<Geometry>::type>
        > collection;

    dispatch::offset
    <
        typename tag<Geometry>::type,
        typename tag<GeometryOut>::type,
        Geometry,
        GeometryOut
    >::apply(collection, geometry, distance_strategy, join);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGORITHMS_OFFSET_HPP


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

// Third party includes
#include <boost/format.hpp>
#include <utility>

// Local VOTCA includes
#include "votca/xtp/esp2multipole.h"
#include "votca/xtp/espfit.h"
#include "votca/xtp/nbo.h"
#include "votca/xtp/populationanalysis.h"

namespace votca {
namespace xtp {
using std::flush;

void Esp2multipole::Initialize(tools::Property& options) {
  do_svd_ = false;

  use_mulliken_ = false;
  use_CHELPG_ = false;
  use_lowdin_ = false;
  state_ = options.get(".state").as<QMState>();

  method_ = options.get(".method").as<std::string>();

  if (method_ == "mulliken") {
    use_mulliken_ = true;
  } else if (method_ == "loewdin") {
    use_lowdin_ = true;
  } else if (method_ == "CHELPG") {
    use_CHELPG_ = true;
  }

  if (options.exists(".constraints")) {
    std::vector<tools::Property*> prop_region =
        options.Select(".constraints.region");
    Index index = 0;
    for (tools::Property* prop : prop_region) {
      std::string indices = prop->get("indices").as<std::string>();
      QMFragment<double> reg = QMFragment<double>(index, indices);
      index++;
      reg.value() = prop->get("charge").as<double>();
      regionconstraint_.push_back(reg);
      XTP_LOG(Log::error, log_) << "Fit constrained by Region" << flush;
      XTP_LOG(Log::error, log_) << reg;
    }
    std::vector<tools::Property*> prop_pair =
        options.Select(".constraints.pair");
    for (tools::Property* prop : prop_pair) {
      std::vector<Index> pairvec = prop->as<std::vector<Index>>();
      pairconstraint_.emplace_back(pairvec[0], pairvec[1]);
      XTP_LOG(Log::error, log_) << "Charges " << pairvec[0] << " " << pairvec[1]
                                << " constrained to be equal." << flush;
    }
  }

  gridsize_ = options.get(".gridsize").as<std::string>();

  if (options.exists(".svd")) {
    do_svd_ = true;
    conditionnumber_ = options.get(".svd.conditionnumber").as<double>();
  }

  return;
}

void Esp2multipole::PrintDipoles(const Orbitals& orbitals,
                                 const StaticSegment& seg) const {
  Eigen::Vector3d classical_dip = seg.CalcDipole();

  XTP_LOG(Log::error, log_)
      << "El Dipole from fitted charges [e*bohr]:\n\t\t"
      << boost::format(
             " dx = %1$+1.4f dy = %2$+1.4f dz = %3$+1.4f |d|^2 = %4$+1.4f") %
             classical_dip.x() % classical_dip.y() % classical_dip.z() %
             classical_dip.squaredNorm()
      << flush;
  Eigen::Vector3d qm_dip = orbitals.CalcElDipole(state_);
  XTP_LOG(Log::error, log_)
      << "El Dipole from exact qm density [e*bohr]:\n\t\t"
      << boost::format(
             " dx = %1$+1.4f dy = %2$+1.4f dz = %3$+1.4f |d|^2 = %4$+1.4f") %
             qm_dip.x() % qm_dip.y() % qm_dip.z() % qm_dip.squaredNorm()
      << flush;
}

StaticSegment Esp2multipole::Extractingcharges(const Orbitals& orbitals) const {
  XTP_LOG(Log::error, log_) << "===== Running on " << OPENMP::getMaxThreads()
                            << " threads ===== " << flush;
  StaticSegment result("result", 0);
  if (use_mulliken_) {
    Mulliken mulliken;
    result = mulliken.CalcChargeperAtom(orbitals, state_);
  } else if (use_lowdin_) {
    Lowdin lowdin;
    result = lowdin.CalcChargeperAtom(orbitals, state_);
  } else if (use_CHELPG_) {
    Espfit esp = Espfit(log_);
    if (pairconstraint_.size() > 0) {
      esp.setPairConstraint(pairconstraint_);
    }
    if (regionconstraint_.size() > 0) {
      esp.setRegionConstraint(regionconstraint_);
    }

    if (do_svd_) {
      esp.setUseSVD(conditionnumber_);
    }
    result = esp.Fit2Density(orbitals, state_, gridsize_);
  }

  PrintDipoles(orbitals, result);
  return result;
}

}  // namespace xtp
}  // namespace votca


#include "UniformRemeshing.h"
#include <igl/is_edge_manifold.h>
#include <wmtk/TriMesh.h>
#include <wmtk/utils/VectorUtils.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <wmtk/ExecutionScheduler.hpp>
#include <wmtk/utils/TupleUtils.hpp>

using namespace remeshing;
using namespace wmtk;

auto renew = [](auto& m, auto op, auto& tris) {
    auto edges = m.new_edges_after(tris);
    auto optup = std::vector<std::pair<std::string, TriMesh::Tuple>>();
    for (auto& e : edges) optup.emplace_back(op, e);
    return optup;
};

auto edge_locker = [](auto& m, const auto& e, int task_id) {
    return m.try_set_edge_mutex_two_ring(e, task_id);
};

std::vector<TriMesh::Tuple> UniformRemeshing::new_edges_after(
    const std::vector<TriMesh::Tuple>& tris) const
{
    std::vector<TriMesh::Tuple> new_edges;
    std::vector<size_t> one_ring_fid;

    for (auto t : tris) {
        for (auto j = 0; j < 3; j++) {
            new_edges.push_back(tuple_from_edge(t.fid(*this), j));
        }
    }
    wmtk::unique_edge_tuples(*this, new_edges);
    return new_edges;
}
bool UniformRemeshing::swap_edge_after(const TriMesh::Tuple& t)
{
    std::vector<TriMesh::Tuple> tris;
    tris.push_back(t);
    tris.push_back(t.switch_edge(*this));
    return true;
}

bool UniformRemeshing::collapse_edge_after(const TriMesh::Tuple& t)
{
    const Eigen::Vector3d p = (position_cache.local().v1p + position_cache.local().v2p) / 2.0;
    auto vid = t.vid(*this);
    vertex_attrs[vid].pos = p;

    return true;
}

bool UniformRemeshing::split_edge_after(const TriMesh::Tuple& t)
{
    const Eigen::Vector3d p = (position_cache.local().v1p + position_cache.local().v2p) / 2.0;
    auto vid = t.vid(*this);
    vertex_attrs[vid].pos = p;

    return true;
}

bool UniformRemeshing::smooth_after(const TriMesh::Tuple& t)
{
    auto one_ring_tris = get_one_ring_tris_for_vertex(t);
    if (one_ring_tris.size() < 2) {
        return false;
    }
    Eigen::Vector3d after_smooth = tangential_smooth(t);

    vertex_attrs[t.vid(*this)].pos = after_smooth;
    return true;
}

double UniformRemeshing::compute_edge_cost_collapse(const TriMesh::Tuple& t, double L) const
{
    double l =
        (vertex_attrs[t.vid(*this)].pos - vertex_attrs[t.switch_vertex(*this).vid(*this)].pos)
            .norm();
    if (l < (4. / 5.) * L) return ((4. / 5.) * L - l);
    return -1;
}
double UniformRemeshing::compute_edge_cost_split(const TriMesh::Tuple& t, double L) const
{
    double l =
        (vertex_attrs[t.vid(*this)].pos - vertex_attrs[t.switch_vertex(*this).vid(*this)].pos)
            .norm();
    if (l > (4. / 3.) * L) return (l - (4. / 3.) * L);
    return -1;
}

double UniformRemeshing::compute_vertex_valence(const TriMesh::Tuple& t) const
{
    std::vector<std::pair<TriMesh::Tuple, int>> valences(3);
    valences[0] = std::make_pair(t, get_one_ring_tris_for_vertex(t).size());
    auto t2 = t.switch_vertex(*this);
    valences[1] = std::make_pair(t2, get_one_ring_tris_for_vertex(t2).size());
    auto t3 = (t.switch_edge(*this)).switch_vertex(*this);
    valences[2] = std::make_pair(t3, get_one_ring_tris_for_vertex(t3).size());

    if ((t.switch_face(*this)).has_value()) {
        auto t4 = (((t.switch_face(*this)).value()).switch_edge(*this)).switch_vertex(*this);
        valences.emplace_back(t4, get_one_ring_tris_for_vertex(t4).size());
    }
    double cost_before_swap = 0.0;
    double cost_after_swap = 0.0;

    // check if it's internal vertex or bondary vertex
    // navigating starting one edge and getting back to the start

    for (int i = 0; i < valences.size(); i++) {
        TriMesh::Tuple vert = valences[i].first;
        int val = 6;
        auto one_ring_edges = get_one_ring_edges_for_vertex(vert);
        for (auto edge : one_ring_edges) {
            if (is_boundary_edge(edge)) {
                val = 4;
                break;
            }
        }
        cost_before_swap += (double)(valences[i].second - val) * (valences[i].second - val);
        cost_after_swap +=
            (i < 2) ? (double)(valences[i].second - 1 - val) * (valences[i].second - 1 - val)
                    : (double)(valences[i].second + 1 - val) * (valences[i].second + 1 - val);
    }
    return (cost_before_swap - cost_after_swap);
}

std::vector<double> UniformRemeshing::average_len_valen()
{
    double average_len = 0.0;
    double average_valen = 0.0;
    auto edges = get_edges();
    auto verts = get_vertices();
    double maxlen = std::numeric_limits<double>::min();
    double maxval = std::numeric_limits<double>::min();
    double minlen = std::numeric_limits<double>::max();
    double minval = std::numeric_limits<double>::max();
    for (auto& loc : edges) {
        double currentlen = (vertex_attrs[loc.vid(*this)].pos -
                             vertex_attrs[loc.switch_vertex(*this).vid(*this)].pos)
                                .norm();
        average_len += currentlen;
        if (maxlen < currentlen) maxlen = currentlen;
        if (minlen > currentlen) minlen = currentlen;
    }
    average_len /= edges.size();
    for (auto& loc : verts) {
        double currentval = get_one_ring_edges_for_vertex(loc).size();
        average_valen += currentval;
        if (maxval < currentval) maxval = currentval;
        if (minval > currentval) minval = currentval;
    }
    average_valen /= verts.size();
    int cnt = 0;
    std::vector<double> rtn{average_len, maxlen, minlen, average_valen, maxval, minval};
    return rtn;
}

std::vector<TriMesh::Tuple> UniformRemeshing::new_edges_after_swap(const TriMesh::Tuple& t) const
{
    std::vector<TriMesh::Tuple> new_edges;
    std::vector<size_t> one_ring_fid;

    new_edges.push_back(t.switch_edge(*this));
    new_edges.push_back((t.switch_face(*this).value()).switch_edge(*this));
    new_edges.push_back((t.switch_vertex(*this)).switch_edge(*this));
    new_edges.push_back(((t.switch_vertex(*this)).switch_face(*this).value()).switch_edge(*this));
    return new_edges;
}

bool UniformRemeshing::collapse_remeshing(double L)
{
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_collapse", loc);

    auto setup_and_execute = [&](auto executor) {
        executor.renew_neighbor_tuples = renew;
        executor.priority = [&](auto& m, auto _, auto& e) {
            return m.compute_edge_cost_collapse(e, L);
        };
        executor.lock_vertices = edge_locker;
        executor.num_threads = NUM_THREADS;
        executor.should_renew = [](auto val) { return (val > 0); };
        executor.is_weight_up_to_date = [](auto& m, auto& ele) {
            auto& [val, op, e] = ele;
            if (val < 0) return false; // priority is negated.
            return true;
        };
        executor(*this, collect_all_ops);
    };
    if (NUM_THREADS > 0) {
        auto executor = wmtk::ExecutePass<UniformRemeshing, ExecutionPolicy::kPartition>();
        setup_and_execute(executor);
    } else {
        auto executor = wmtk::ExecutePass<UniformRemeshing, ExecutionPolicy::kSeq>();
        setup_and_execute(executor);
    }

    return true;
}
bool UniformRemeshing::split_remeshing(double L)
{
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();

    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_split", loc);
    wmtk::logger().info("size for edges to be split is {}", collect_all_ops.size());
    auto setup_and_execute = [&](auto executor) {
        executor.num_threads = NUM_THREADS;

        executor.lock_vertices = edge_locker;

        executor.renew_neighbor_tuples = renew;
        executor.priority = [&](auto& m, auto _, auto& e) {
            return m.compute_edge_cost_split(e, L);
        };
        executor.should_renew = [](auto val) { return (val > 0); };
        executor.is_weight_up_to_date = [](auto& m, auto& ele) {
            auto& [val, op, e] = ele;
            if (val < 0) return false;
            return true;
        };
        executor(*this, collect_all_ops);
    };
    if (NUM_THREADS > 0) {
        auto executor = wmtk::ExecutePass<UniformRemeshing, ExecutionPolicy::kPartition>();
        setup_and_execute(executor);
    } else {
        auto executor = wmtk::ExecutePass<UniformRemeshing, ExecutionPolicy::kSeq>();
        setup_and_execute(executor);
    }

    return true;
}

bool UniformRemeshing::swap_remeshing()
{
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_swap", loc);

    auto setup_and_execute = [&](auto executor) {
        executor.renew_neighbor_tuples = renew;
        executor.num_threads = NUM_THREADS;
        executor.priority = [](auto& m, auto op, const Tuple& e) {
            return m.compute_vertex_valence(e);
        };
        executor.lock_vertices = edge_locker;
        executor.should_renew = [](auto val) { return (val > 0); };
        executor.is_weight_up_to_date = [](auto& m, auto& ele) {
            auto& [val, _, e] = ele;
            auto val_energy = (m.compute_vertex_valence(e));
            return (val_energy > 1e-5) && ((val_energy - val) * (val_energy - val) < 1e-8);
        };
        executor(*this, collect_all_ops);
    };
    if (NUM_THREADS > 0) {
        auto executor = wmtk::ExecutePass<UniformRemeshing, ExecutionPolicy::kPartition>();
        setup_and_execute(executor);
    } else {
        auto executor = wmtk::ExecutePass<UniformRemeshing, ExecutionPolicy::kSeq>();
        setup_and_execute(executor);
    }

    return true;
}
double area(UniformRemeshing& m, std::array<TriMesh::Tuple, 3>& verts)
{
    return ((m.vertex_attrs[verts[0].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos)
                .cross(m.vertex_attrs[verts[1].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos))
               .norm() /
           2.0;
}

Eigen::Vector3d normal(UniformRemeshing& m, std::array<TriMesh::Tuple, 3>& verts)
{
    return ((m.vertex_attrs[verts[0].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos)
                .cross(m.vertex_attrs[verts[1].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos))
        .normalized();
}

Eigen::Vector3d UniformRemeshing::smooth(const TriMesh::Tuple& t)
{
    auto one_ring_edges = get_one_ring_edges_for_vertex(t);
    if (one_ring_edges.size() < 3) return vertex_attrs[t.vid(*this)].pos;
    Eigen::Vector3d after_smooth(0, 0, 0);
    Eigen::Vector3d after_smooth_boundary(0, 0, 0);
    int boundary = 0;
    for (auto e : one_ring_edges) {
        if (is_boundary_edge(e)) {
            after_smooth_boundary += vertex_attrs[e.vid(*this)].pos;
            boundary++;
            continue;
        }
        after_smooth += vertex_attrs[e.vid(*this)].pos;
    }

    if (boundary)
        after_smooth = after_smooth_boundary / boundary;
    else
        after_smooth /= one_ring_edges.size();
    return after_smooth;
}

Eigen::Vector3d UniformRemeshing::tangential_smooth(const Tuple& t)
{
    auto one_ring_tris = get_one_ring_tris_for_vertex(t);
    if (one_ring_tris.size() < 2) return vertex_attrs[t.vid(*this)].pos;
    Eigen::Vector3d after_smooth = smooth(t);
    // get normal and area of each face
    auto area = [](auto& m, auto& verts) {
        return ((m.vertex_attrs[verts[0].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos)
                    .cross(
                        m.vertex_attrs[verts[1].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos))
                   .norm() /
               2.0;
    };
    auto normal = [](auto& m, auto& verts) {
        return ((m.vertex_attrs[verts[0].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos)
                    .cross(
                        m.vertex_attrs[verts[1].vid(m)].pos - m.vertex_attrs[verts[2].vid(m)].pos))
            .normalized();
    };
    auto w0 = 0.0;
    Eigen::Vector3d n0(0.0, 0.0, 0.0);
    for (auto& e : one_ring_tris) {
        auto verts = oriented_tri_vertices(e);
        w0 += area(*this, verts);
        n0 += area(*this, verts) * normal(*this, verts);
    }
    n0 /= w0;
    after_smooth += n0 * n0.transpose() * (vertex_attrs[t.vid(*this)].pos - after_smooth);
    assert(check_mesh_connectivity_validity());
    return after_smooth;
}

bool UniformRemeshing::uniform_remeshing(double L, int iterations)
{
    int cnt = 0;
    std::vector<double> properties = average_len_valen();
    wmtk::logger().info("avg len is: {}", properties[0]);
    wmtk::logger().info("target len is: {}", L);

    wmtk::logger().info("input mesh is mani {}", check_edge_manifold());
    while ((properties[0] - L) * (properties[0] - L) > 1e-8 && cnt < iterations) {
        cnt++;
        // split
        split_remeshing(L);
        // collpase
        collapse_remeshing(L);

        // swap edges
        swap_remeshing();

        // smoothing
        auto vertices = get_vertices();
        for (auto& loc : vertices) smooth_vertex(loc);

        properties = average_len_valen();
        wmtk::logger().info("avg edge len: {}", properties[0]);

        wmtk::logger().info("avg valence: {}", properties[3]);
    }
    wmtk::logger().info("finished {} remeshing iterations", iterations);
    wmtk::logger().info("avg edge len: {}", properties[0]);

    wmtk::logger().info("avg valence: {}", properties[3]);
    wmtk::logger().info("+++++++++finished+++++++++");
    return true;
}
// write the collapsed mesh into a obj and assert the mesh is manifold
bool UniformRemeshing::write_triangle_mesh(std::string path)
{
    Eigen::MatrixXd V = Eigen::MatrixXd::Zero(vert_capacity(), 3);
    for (auto& t : get_vertices()) {
        auto i = t.vid(*this);
        V.row(i) = vertex_attrs[i].pos;
    }

    Eigen::MatrixXi F = Eigen::MatrixXi::Constant(tri_capacity(), 3, -1);
    for (auto& t : get_faces()) {
        auto i = t.fid(*this);
        auto vs = oriented_tri_vertices(t);
        for (int j = 0; j < 3; j++) {
            F(i, j) = vs[j].vid(*this);
        }
    }
    igl::write_triangle_mesh(path, V, F);
    bool manifold = check_edge_manifold();
    assert(manifold);

    return manifold;
}


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE Supercapacitor_2d_vs_3d

#include "main.cc"

#include <cap/supercapacitor.h>
#include <cap/utils.h>
#include <boost/test/unit_test.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>

BOOST_AUTO_TEST_CASE(test_3d_charge)
{
  // First do a charge on a 2D device and then compare the result with a charge
  // using a 3D device.
  // Parse input file
  boost::property_tree::ptree device_database;
  boost::property_tree::info_parser::read_info("super_capacitor.info",
                                               device_database);
  boost::property_tree::ptree geometry_database;
  boost::property_tree::info_parser::read_info("generate_mesh.info",
                                               geometry_database);
  // Use the same divisions for 2D and 3D
  std::vector<unsigned int> divisions_2(2);
  // Collector
  divisions_2[0] = 1;
  divisions_2[1] = 3;
  geometry_database.put("collector.divisions", cap::to_string(divisions_2));
  // Anode
  divisions_2[0] = 1;
  divisions_2[1] = 2;
  geometry_database.put("anode.divisions", cap::to_string(divisions_2));
  // Separator
  divisions_2[0] = 1;
  divisions_2[1] = 2;
  geometry_database.put("separator.divisions", cap::to_string(divisions_2));
  // Cathode
  divisions_2[0] = 1;
  divisions_2[1] = 2;
  geometry_database.put("cathode.divisions", cap::to_string(divisions_2));
  // Number of refinements
  geometry_database.put("n_refinements", 2);
  device_database.put_child("geometry", geometry_database);

  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(device_database,
                                      boost::mpi::communicator());
  double const charge_current = 5e-3;
  double const time_step = 1e-2;
  double voltage_2d;
  for (unsigned int i = 0; i < 5; ++i)
    device->evolve_one_time_step_constant_current(time_step, charge_current);
  device->get_voltage(voltage_2d);

  // Now compute the 3D device
  std::vector<unsigned int> divisions_3(3);
  // Collector
  divisions_3[0] = 1;
  divisions_3[1] = 1;
  divisions_3[2] = 3;
  geometry_database.put("collector.divisions", cap::to_string(divisions_3));
  // Anode
  divisions_3[0] = 1;
  divisions_3[1] = 1;
  divisions_3[2] = 2;
  geometry_database.put("anode.divisions", cap::to_string(divisions_3));
  // Separator
  divisions_3[0] = 1;
  divisions_3[1] = 1;
  divisions_3[2] = 2;
  geometry_database.put("separator.divisions", cap::to_string(divisions_3));
  // Cathode
  divisions_3[0] = 1;
  divisions_3[1] = 1;
  divisions_3[2] = 2;
  geometry_database.put("cathode.divisions", cap::to_string(divisions_3));
  device_database.put_child("geometry", geometry_database);
  device_database.put("dim", 3);

  device = cap::EnergyStorageDevice::build(device_database,
                                           boost::mpi::communicator());
  double voltage_3d;
  for (unsigned int i = 0; i < 5; ++i)
    device->evolve_one_time_step_constant_current(time_step, charge_current);
  device->get_voltage(voltage_3d);

  double const percent_tolerance = 1e-2;
  BOOST_CHECK_CLOSE(voltage_2d, voltage_3d, percent_tolerance);
}


#define BOOST_TEST_MODULE pcraster com mv_op
#include <boost/test/unit_test.hpp>
#include "com_mvop.h"

// template<typename T>
// static std::string asStr(char const* bits)
// {
//    std::string res;
//
//    UINT4 *v=(UINT4 *)bits;
//    std::bitset<32> bs(v[0]);
//    res = bs.to_string();
//    if (sizeof(T) == 8) {
//      bs = std::bitset<32>(v[1]);
//      res += bs.to_string();
//    }
//    return res;
// }

// BOOST_CHECK_EQUAL(asStr<T>(fr.debugBits()), std::string("11111111"));
// BOOST_CHECK_EQUAL(asStr<T>(R(f).debugBits()),std::string("11111111"));

template<
    typename T>
void test_assignment()
{
    {
        T argument;
        pcr::setMV(argument);
        auto result = argument;
        BOOST_CHECK(pcr::isMV(result));
    }
}

template<
    typename T>
void test_add()
{
    // 3 + 4
    {
        T argument1 = 3;
        T argument2 = 4;
        auto result = com::add<T>(argument1, argument2);
        BOOST_CHECK(!pcr::isMV(result));
        BOOST_CHECK_EQUAL(result, 7);
    }

    // mv + 4
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        auto result = com::add<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(result));
    }

    // 3 + mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument2);
        auto result = com::add<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(result));
    }

    // mv + mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        pcr::setMV(argument2);
        auto result = com::add<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(result));
    }
}

template<
    typename T>
void test_inplace_add()
{
    // 3 += 4
    {
        T argument1 = 3;
        T argument2 = 4;
        com::inplace_add<T>(argument1, argument2);
        BOOST_CHECK(!pcr::isMV(argument1));
        BOOST_CHECK_EQUAL(argument1, 7);
    }

    // mv += 4
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        com::inplace_add<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(argument1));
    }

    // 3 += mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument2);
        com::inplace_add<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(argument1));
    }

    // mv += mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        pcr::setMV(argument2);
        com::inplace_add<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(argument1));
    }
}

template<
    typename T>
void test_subtract()
{
    // 3 - 4
    {
        T argument1 = 3;
        T argument2 = 4;
        auto result = com::subtract<T>(argument1, argument2);
        BOOST_CHECK(!pcr::isMV(result));
        BOOST_CHECK_EQUAL(result, -1);
    }

    // mv - 4
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        auto result = com::subtract<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(result));
    }

    // 3 - mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument2);
        auto result = com::subtract<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(result));
    }

    // mv - mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        pcr::setMV(argument2);
        auto result = com::subtract<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(result));
    }
}

template<
    typename T>
void test_inplace_subtract()
{
    // 3 -= 4
    {
        T argument1 = 3;
        T argument2 = 4;
        com::inplace_subtract<T>(argument1, argument2);
        BOOST_CHECK(!pcr::isMV(argument1));
        BOOST_CHECK_EQUAL(argument1, -1);
    }

    // mv -= 4
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        com::inplace_subtract<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(argument1));
    }

    // 3 -= mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument2);
        com::inplace_subtract<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(argument1));
    }

    // mv -= mv
    {
        T argument1 = 3;
        T argument2 = 4;
        pcr::setMV(argument1);
        pcr::setMV(argument2);
        com::inplace_subtract<T>(argument1, argument2);
        BOOST_CHECK(pcr::isMV(argument1));
    }
}

template<
    typename T>
void test_operations_()
{
    test_assignment<T>();
    test_add<T>();
    test_inplace_add<T>();
    test_subtract<T>();
    test_inplace_subtract<T>();
}

BOOST_AUTO_TEST_CASE(test_operations)
{
  test_operations_<float>();
  test_operations_<double>();
}


#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/coarsening/rigid_body_modes.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/deflated_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/reorder.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>

#include <amgcl/profiler.hpp>

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    namespace po = boost::program_options;
    namespace io = amgcl::io;

    using amgcl::prof;
    using std::vector;
    using std::string;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "Show this help.")
        ("prm-file,P",
         po::value<string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< vector<string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ("matrix,A",
         po::value<string>()->required(),
         "System matrix in the MatrixMarket format."
        )
        (
         "rhs,f",
         po::value<string>(),
         "The RHS vector in the MatrixMarket format. "
         "When omitted, a vector of ones is used by default. "
         "Should only be provided together with a system matrix. "
        )
        (
         "scale,s",
         po::bool_switch()->default_value(false),
         "Scale the matrix so that the diagonal is unit. "
        )
        (
         "null,N",
         po::value<string>(),
         "Starting null-vectors in the MatrixMarket format. "
        )
        (
         "numvec,n",
         po::value<int>()->default_value(3),
         "The number of near nullspace vectors to search for. "
        )
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "output,o",
         po::value<string>(),
         "Output the computed nullspace to the MatrixMarket file."
        )
        ;

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

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

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

    for (int i = 0; i < argc; ++i) {
        if (i) std::cout << " ";
        std::cout << argv[i];
    }
    std::cout << std::endl;

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<string>(), prm);
    }

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string> >()) {
            amgcl::put(prm, v);
        }
    }

    ptrdiff_t rows, nv = 0, numvec = vm["numvec"].as<int>();
    vector<ptrdiff_t> ptr, col;
    vector<double> val, rhs;
    std::list<std::vector<double>> Z;

    {
        auto t = prof.scoped_tic("read");

        string Afile  = vm["matrix"].as<string>();
        bool   binary = vm["binary"].as<bool>();

        if (binary) {
            io::read_crs(Afile, rows, ptr, col, val);
        } else {
            ptrdiff_t cols;
            std::tie(rows, cols) = io::mm_reader(Afile)(ptr, col, val);
            precondition(rows == cols, "Non-square system matrix");
        }

        if (vm.count("rhs")) {
            string bfile = vm["rhs"].as<string>();

            ptrdiff_t n, m;

            if (binary) {
                io::read_dense(bfile, n, m, rhs);
            } else {
                std::tie(n, m) = io::mm_reader(bfile)(rhs);
            }

            precondition(n == rows && m == 1, "The RHS vector has wrong size");
        } else {
            rhs.resize(rows, 1.0);
        }

        if (vm.count("null")) {
            string nfile = vm["null"].as<string>();

            std::vector<double> null;
            ptrdiff_t m;

            if (binary) {
                io::read_dense(nfile, m, nv, null);
            } else {
                std::tie(m, nv) = io::mm_reader(nfile)(null);
            }

            precondition(m == rows, "Near null-space vectors have wrong size");

            for(ptrdiff_t i = 0; i < nv; ++i) {
                Z.emplace_back(rows);
                for(ptrdiff_t j = 0; j < rows; ++j) {
                    Z.back()[j] = null[j * nv + i];
                }
            }
        }
    }

    if (vm["scale"].as<bool>()) {
        auto t = prof.scoped_tic("scaling");
        std::vector<double> dia(rows, 1.0);

        for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(rows); ++i) {
            double d = 1.0;
            for(ptrdiff_t j = ptr[i], e = ptr[i+1]; j < e; ++j) {
                if (col[j] == i) {
                    d = 1 / sqrt(val[j]);
                }
            }
            if (!std::isnan(d)) dia[i] = d;
        }

        for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(rows); ++i) {
            rhs[i] *= dia[i];
            for(ptrdiff_t j = ptr[i], e = ptr[i+1]; j < e; ++j) {
                val[j] *= dia[i] * dia[col[j]];
            }
        }
    }

    typedef amgcl::backend::builtin<double> Backend;
    typedef amgcl::make_solver<
        amgcl::amg<
            Backend,
            amgcl::runtime::coarsening::wrapper,
            amgcl::runtime::relaxation::wrapper
            >,
        amgcl::runtime::solver::wrapper<Backend>
        > Solver;

    std::mt19937 rng;
    std::uniform_real_distribution<double> rnd(-1, 1);
    std::vector<double> x(rows), zero(rows, 0.0);

    auto A = std::tie(rows, ptr, col, val);

    prm.put("solver.ns_search", true);

    prof.tic("search");
    for(int k = nv; k < numvec; ++k) {
        auto t = prof.scoped_tic(std::string("vector ") + std::to_string(k));
        std::vector<double> N;

        if (k) {
            N.resize(k * rows);
            int j = 0;
            for(const auto &z : Z) {
                for(ptrdiff_t i = 0; i < rows; ++i) {
                    N[i * k + j] = z[i];
                }
                ++j;
            }

            prm.put("precond.coarsening.nullspace.rows", rows);
            prm.put("precond.coarsening.nullspace.cols", k);
            prm.put("precond.coarsening.nullspace.B", N.data());
        }

        prof.tic("setup");
        Solver S(A, prm);
        prof.toc("setup");

        std::cout << std::endl
                  << "-------------------------" << std::endl
                  << "-- Searching for vector " << k << std::endl
                  << "-------------------------" << std::endl
                  << S << std::endl;

        for(auto &v : x) v = rnd(rng);

        int iters;
        double error;

        prof.tic("solve");
        std::tie(iters, error) = S(zero, x);
        prof.toc("solve");

        std::cout << "Iterations: " << iters << std::endl
                  << "Error:      " << error << std::endl;

        // Orthonormalize the new vector
        for(const auto &z : Z) {
            double c = amgcl::backend::inner_product(x,z) / amgcl::backend::inner_product(z,z);
            amgcl::backend::axpby(-c, z, 1, x);
        }

        double nx = sqrt(amgcl::backend::inner_product(x, x));
        for(auto &v : x) v /= nx;
        Z.push_back(x);
    }
    prof.toc("search");

    // Solve the system using the near nullspace vectors:
    std::vector<double> N(numvec * rows);
    {
        auto t = prof.scoped_tic("apply");

        int j = 0;
        for(const auto &z : Z) {
            for(ptrdiff_t i = 0; i < rows; ++i) {
                N[i * numvec + j] = z[i];
            }
            ++j;
        }

        prm.put("precond.coarsening.nullspace.rows", rows);
        prm.put("precond.coarsening.nullspace.cols", numvec);
        prm.put("precond.coarsening.nullspace.B", N.data());

        prof.tic("setup");
        Solver S(A, prm);
        prof.toc("setup");

        std::cout << std::endl
                  << "-------------------------" << std::endl
                  << "-- Solving the system " << std::endl
                  << "-------------------------" << std::endl
                  << S << std::endl;

        amgcl::backend::clear(x);

        int iters;
        double error;

        prof.tic("solve");
        std::tie(iters, error) = S(rhs, x);
        prof.toc("solve");

        std::cout << "Iterations: " << iters << std::endl
            << "Error:      " << error << std::endl;
    }

    if (vm.count("output")) {
        auto t = prof.scoped_tic("write");
        amgcl::io::mm_write(vm["output"].as<string>(), N.data(), rows, numvec);
    }

    std::cout << prof << std::endl;
}