/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include <catch.hpp>

#include <cmath>
#include <vector>

#include <Eigen/Core>

#include "cavity/GePolCavity.hpp"
#include "TestingMolecules.hpp"

SCENARIO("GePol cavity for a single sphere in different Abelian point groups", "[gepol][gepol_point_symmetry]")
{
    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is C1")
        {
            Molecule point = dummy<0>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "c1");

            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_size tests GePol cavity size for a point charge in C1 symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_irreducible_size tests GePol cavity irreducible size for a point charge in C1 symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_area tests GePol cavity surface area for a point charge in C1 symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_volume tests GePol cavity volume for a point charge in C1 symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }

    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is C2")
        {
            Molecule point = dummy<1>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "c2");

            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_size tests GePol cavity size for a point charge in C2 symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_irreducible_size tests GePol cavity irreducible size for a point charge in C2 symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 16;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_area tests GePol cavity surface area for a point charge in C2 symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC1Test_volume tests GePol cavity volume for a point charge in C2 symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }

    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is Cs")
        {
            Molecule point = dummy<2>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "cs");

            /*! \class GePolCavity
             *  \test \b GePolCavityCsTest_size tests GePol cavity size for a point charge in Cs symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityCsTest_irreducible_size tests GePol cavity irreducible size for a point charge in Cs symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 16;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityCsTest_area tests GePol cavity surface area for a point charge in Cs symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityCsTest_volume tests GePol cavity volume for a point charge in Cs symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }

    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is Ci")
        {
            Molecule point = dummy<3>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "ci");

            /*! \class GePolCavity
             *  \test \b GePolCavityCiTest_size tests GePol cavity size for a point charge in Ci symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityCiTest_irreducible_size tests GePol cavity irreducible size for a point charge in Ci symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 16;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityCiTest_area tests GePol cavity surface area for a point charge in Ci symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityCiTest_volume tests GePol cavity volume for a point charge in Ci symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }

    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is D2")
        {
            Molecule point = dummy<4>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "d2");

            /*! \class GePolCavity
             *  \test \b GePolCavityD2Test_size tests GePol cavity size for a point charge in D2 symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityD2Test_irreducible_size tests GePol cavity irreducible size for a point charge in D2 symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 8;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityD2Test_area tests GePol cavity surface area for a point charge in D2 symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityD2Test_volume tests GePol cavity volume for a point charge in D2 symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }

    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is C2v")
        {
            Molecule point = dummy<5>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "c2v");

            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_size tests GePol cavity size for a point charge in C2v symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_irreducible_size tests GePol cavity irreducible size for a point charge in C2v symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 8;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_area tests GePol cavity surface area for a point charge in C2v symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_volume tests GePol cavity volume for a point charge in C2v symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }

    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is C2h")
        {
            Molecule point = dummy<6>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "c2h");

            /*! \class GePolCavity
             *  \test \b GePolCavityC2hTest_size tests GePol cavity size for a point charge in C2h symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2hTest_irreducible_size tests GePol cavity irreducible size for a point charge in C2h symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 8;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2hTest_area tests GePol cavity surface area for a point charge in C2h symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2hTest_volume tests GePol cavity volume for a point charge in C2h symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }

    GIVEN("A single sphere")
    {
        double area = 0.4;
        double probeRadius = 0.0;
        double minRadius = 100.0;
        WHEN("the point group is D2h")
        {
            Molecule point = dummy<7>();
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius, "d2h");

            /*! \class GePolCavity
             *  \test \b GePolCavityD2hTest_size tests GePol cavity size for a point charge in D2h symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 32;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityD2hTest_irreducible_size tests GePol cavity irreducible size for a point charge in D2h symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 4;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityD2hTest_area tests GePol cavity surface area for a point charge in D2h symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 4.0 * M_PI * pow(1.0, 2);
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityD2hTest_volume tests GePol cavity volume for a point charge in D2h symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 4.0 * M_PI * pow(1.0, 3) / 3.0;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }
}


// Boost.Geometry

// Copyright (c) 2017-2019 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, 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_STRATEGIES_SPHERICAL_DISJOINT_SEGMENT_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISJOINT_SEGMENT_BOX_HPP

#include <cstddef>
#include <utility>

#include <boost/numeric/conversion/cast.hpp>

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

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/disjoint/segment_box.hpp>

// TODO: spherical_point_box currently defined in the same file as cartesian
#include <boost/geometry/strategies/cartesian/point_in_box.hpp>
#include <boost/geometry/strategies/disjoint.hpp>
#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/spherical/azimuth.hpp>
#include <boost/geometry/strategies/spherical/disjoint_box_box.hpp>

namespace boost { namespace geometry { namespace strategy { namespace disjoint
{

// NOTE: This may be temporary place for this or corresponding strategy
// It seems to be more appropriate to implement the opposite of it
// e.g. intersection::segment_box because in disjoint() algorithm
// other strategies that are used are intersection and covered_by strategies.
struct segment_box_spherical
{
    typedef covered_by::spherical_point_box disjoint_point_box_strategy_type;

    static inline disjoint_point_box_strategy_type get_disjoint_point_box_strategy()
    {
        return disjoint_point_box_strategy_type();
    }

    template <typename Segment, typename Box>
    static inline bool apply(Segment const& segment, Box const& box)
    {
        typedef typename point_type<Segment>::type segment_point_type;
        typedef typename coordinate_type<segment_point_type>::type CT;
        geometry::strategy::azimuth::spherical<CT> azimuth_strategy;

        return geometry::detail::disjoint::disjoint_segment_box_sphere_or_spheroid
                <
                    spherical_equatorial_tag
                >::apply(segment, box,
                         azimuth_strategy,
                         strategy::normalize::spherical_point(),
                         strategy::covered_by::spherical_point_box(),
                         strategy::disjoint::spherical_box_box());
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Linear, typename Box, typename LinearTag>
struct default_strategy<Linear, Box, LinearTag, box_tag, 1, 2,
                        spherical_equatorial_tag, spherical_equatorial_tag>
{
    typedef segment_box_spherical type;
};

template <typename Box, typename Linear, typename LinearTag>
struct default_strategy<Box, Linear, box_tag, LinearTag, 2, 1,
                        spherical_equatorial_tag, spherical_equatorial_tag>
{
    typedef segment_box_spherical type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}}}} // namespace boost::geometry::strategy::disjoint

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISJOINT_SEGMENT_BOX_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVROUND_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVROUND_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/divround.hpp>
#include <boost/simd/include/functions/simd/iround.hpp>
#include <boost/simd/include/functions/simd/iround.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/rdivide.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return boost::simd::iround(boost::simd::tofloat(a0)/boost::simd::tofloat(a1)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<unsigned_<A0>,X>))((simd_<unsigned_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return boost::simd::rdivide(a0+a1/boost::simd::Two<A0>(), a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<int16_<A0>,X>))((simd_<int16_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype,X>                 ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h);
      boost::simd::split(a1, a1l, a1h);
      return simd::bitwise_cast<A0>(boost::simd::group(boost::simd::divround(a0l, a1l),
                                               boost::simd::divround(a0h, a1h)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<int8_<A0>,X>))((simd_<int8_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype, X>                 ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h);
      boost::simd::split(a1, a1l, a1h);
      return simd::bitwise_cast<A0>(group(divround(a0l, a1l),
                                          divround(a0h, a1h) ));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divround_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return boost::simd::round(a0/a1); }
  };
} } }

#endif


#include <dlib/dnn.h>
#include <dlib/opencv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>

#include "recognizer.h"

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = dlib::add_prev1<block<N,BN,1,dlib::tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = dlib::add_prev2<dlib::avg_pool<2,2,2,2,dlib::skip1<dlib::tag2<block<N,BN,2,dlib::tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<dlib::con<N,3,3,1,1,dlib::relu<BN<dlib::con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = dlib::relu<residual<block,N,dlib::affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = dlib::relu<residual_down<block,N,dlib::affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

const int IMAGE_SIZE = 150;

using RecognizerNet = dlib::loss_metric<dlib::fc_no_bias<128,dlib::avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            dlib::max_pool<3,3,2,2,dlib::relu<dlib::affine<dlib::con<32,7,7,2,2,
                            dlib::input_rgb_image_sized<IMAGE_SIZE>
                            >>>>>>>>>>>>;

static std::vector<dlib::matrix<dlib::rgb_pixel>> jitter_image(const dlib::matrix<dlib::rgb_pixel>& image, int count) {
    thread_local dlib::rand random;

    std::vector<dlib::matrix<dlib::rgb_pixel>> crops;
    for (int i = 0; i < count; i++) {
        crops.push_back(dlib::jitter_image(image, random));
    }

    return crops;
}

class Recognizer {

public:
    Recognizer(const char* shaper_model_file_path, const char* recognizer_model_file_path) {
        dlib::deserialize(shaper_model_file_path) >> shaper;
        dlib::deserialize(recognizer_model_file_path) >> net;
    }

    dlib::matrix<float,0,1> recognize(const dlib::matrix<dlib::rgb_pixel>& image, dlib::rectangle face_location, double padding, int jittering) {
        auto shape = shaper(image, face_location);

        dlib::matrix<dlib::rgb_pixel> chip;
        dlib::extract_image_chip(image, dlib::get_face_chip_details(shape, IMAGE_SIZE, padding), chip);

        dlib::matrix<float,0,1> descriptor;

        if (jittering > 0) {
            auto chips = jitter_image(chip, jittering);
            std::vector<dlib::matrix<float,0,1>> descriptors;
            {
                std::lock_guard lock(net_mutex);
                descriptors = net(chips);
            }
            descriptor = dlib::mean(dlib::mat(descriptors));
        } else {
            std::lock_guard lock(net_mutex);
            descriptor = net(chip);
        }

        return descriptor;
    }

private:
    dlib::shape_predictor shaper;

    RecognizerNet net;
    std::mutex net_mutex;
};

recognizer_init_result* recognizer_init(char* shaper_model_file_path, char* recognizer_model_file_path) {
    recognizer_init_result* result = (recognizer_init_result*)malloc(sizeof(recognizer_init_result));

    try {
        Recognizer* recognizer = new Recognizer(shaper_model_file_path, recognizer_model_file_path);
        result->recognizer = (void*)recognizer;
        result->error_message = NULL;
    } catch (std::exception& e) {
        result->recognizer = NULL;
        result->error_message = strdup(e.what());
    }

    return result;
}

void recognizer_free(void* recognizer) {
    delete (Recognizer*)(recognizer);
}

recognizer_recognize_result* recognizer_recognize(void* recognizer, void* image, rectangle* face_location, double padding, int jittering) {
    recognizer_recognize_result* result = (recognizer_recognize_result*)malloc(sizeof(recognizer_recognize_result));

    try {
        dlib::matrix<dlib::rgb_pixel> dlib_image;

        cv::Mat* opencv_image = (cv::Mat*)image;

        if (opencv_image->channels() > 1) {
            dlib::assign_image(dlib_image, dlib::cv_image<dlib::bgr_pixel>(*opencv_image));
        } else {
            dlib::assign_image(dlib_image, dlib::cv_image<uchar>(*opencv_image));
        }

        dlib::rectangle dlib_face_location;

        dlib_face_location.set_left(face_location->min.x);
        dlib_face_location.set_top(face_location->min.y);
        dlib_face_location.set_right(face_location->max.x);
        dlib_face_location.set_bottom(face_location->max.y);

        auto dlib_descriptor = ((Recognizer*)(recognizer))->recognize(dlib_image, dlib_face_location, padding, jittering);

        float* descriptor = (float*)calloc(dlib_descriptor.nr(), sizeof(float));

        for (int i = 0; i < dlib_descriptor.nr(); i++) {
            descriptor[i] = dlib_descriptor(i, 0);
        }

        result->descriptor = descriptor;
        result->error_message = NULL;

    } catch (std::exception& e) {
        result->descriptor = NULL;
        result->error_message = strdup(e.what());
        return result;
    }

    return result;
}


/* Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/defs.h"

#include "bio/adjust_hits.h"
#include "bio/binding_model.h"
#include "bio/binding_hit.h"

#include <boost/foreach.hpp>
#include <boost/multi_index_container.hpp>
#include <boost/multi_index/identity.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/tag.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>

BIO_NS_START

struct change_p_binding
{
	change_p_binding( double new_p_binding )
		: new_p_binding( new_p_binding )
	{
	}

	void operator()( BindingHit< BindingModel > & hit ) const
	{
		hit.p_binding = new_p_binding;
	}

private:
	double new_p_binding;
};

void
adjust_hits(
	BindingHitSet< BindingModel >::type & hits,
	seq_t::const_iterator phylo_begin,
	seq_t::const_iterator phylo_end,
	double threshold,
	BindingModelContext * context )
{
	//for each binder in the hits
#ifdef _MSC_VER
	typedef BindingHitSet< BindingModel >::binder binder_t;
	typedef BindingModel::hit_set_t::index< binder_t >::type hit_by_binder_t;
	hit_by_binder_t & by_binder = ::boost::multi_index::get< binder_t >( hits );
#else // _MSC_VER
	typedef BindingHitSet< BindingModel >::type container_t; //should be same as binding_hit_set_t::type
	container_t test; test.begin();
	//BOOST_STATIC_ASSERT( ( boost::is_same<
	//	container_t,
	//	BindingModel::hit_set_t > ) );
	//typedef BindingHitSet< BindingModel > binding_hit_set_t;
	//typedef binding_hit_set_t::binder binder_tag;
	//typedef container_t::index< binder_tag > index_t;
	//typedef index_t::type hit_by_binder_t;
	//hit_by_binder_t & by_binder = ::boost::multi_index::get< binder_tag >( hits );
#endif // _MSC_VER

#if 0
	for( hit_by_binder_t::iterator h = by_binder.begin();
		by_binder.end() != h;
		)
	{
		BindingModel * binder = h->binder;

		//score the model over the phylo sequence
		BindingModel::hit_set_t model_hits;
		make_sequence_scorer(
			phylo_begin,
			phylo_end,
			threshold,
			std::inserter( model_hits, model_hits.begin() ),
			context
		) (
			binder
		);

		//what is the p_binding anywhere in sequence?
		double p_doesnt_bind = 1.0;
		for( BindingModel::hit_set_t::const_iterator mh = model_hits.begin();
			model_hits.end() != mh;
			++mh )
		{
			p_doesnt_bind *= ( 1.0 - mh->p_binding );
		}
		const double p_does_bind = 1.0 - p_doesnt_bind;

		//adjust all the hits for this binder
		while ( by_binder.end() != h && binder == h->binder )
		{
			by_binder.modify( h, change_p_binding( h->p_binding * p_does_bind ) );
			++h;
		}

	}
#endif
}

void
adjust_hits(
	BindingModel::hit_set_t & hits,
	const SeqList & sequences,
	double threshold,
	BindingModelContext * context )
{
	unsigned num_seqs = 0;
	BOOST_FOREACH( const seq_t & s, sequences )
	{
		adjust_hits(
			hits,
			s.begin(),
			s.end(),
			threshold,
			context );
		++num_seqs;
	}

	//adjust all the hits
	for( BindingModel::hit_set_t::const_iterator h = hits.begin();
		hits.end() != h;
		++h )
	{
		hits.modify(
			h,
			change_p_binding(
				exp( log( h->get_p_binding() ) / ( num_seqs + 1 ) ) ) );
	}
}

void
adjust_hits(
	BindingModel::hit_set_t & hits,
	const std::vector< seq_t > & sequences,
	double threshold,
	BindingModelContext * context )
{
	unsigned num_seqs = 0;
	BOOST_FOREACH( const seq_t & s, sequences )
	{
		adjust_hits(
			hits,
			s.begin(),
			s.end(),
			threshold,
			context );
		++num_seqs;
	}

	//adjust all the hits
	for( BindingModel::hit_set_t::const_iterator h = hits.begin();
		hits.end() != h;
		++h )
	{
		hits.modify(
			h,
			change_p_binding(
				exp( log( h->get_p_binding() ) / ( num_seqs + 1 ) ) ) );
	}
}

#if 0
void
remove_under_threshold(
	BindingModel::hit_set_t & hits,
	double threshold )
{
	//for each binder in the hits
	typedef BindingModel::hit_set_t::index< BindingHitSet< BindingModel >::prob >::type hit_by_prob_t;
	hit_by_prob_t & by_prob = hits.get< BindingHitSet< BindingModel >::prob >();

	hit_by_prob_t::const_iterator h = by_prob.begin();
	for( ;
		by_prob.end() != h && h->get_p_binding() < threshold;
		++h )
	{
	}

	by_prob.erase( by_prob.begin(), h );
}
#endif

BIO_NS_END


//
// Created by kunhuang on 6/3/19.
//

#pragma once

#include <moveit/version.h>
#include <Eigen/Geometry>
#include <vector>

#define MOVEIT_VERSION_AT_LEAST(x, y, z) \
  (MOVEIT_VERSION_MAJOR > x ||           \
   (MOVEIT_VERSION_MAJOR >= x &&         \
    (MOVEIT_VERSION_MINOR > y || (MOVEIT_VERSION_MINOR >= y && MOVEIT_VERSION_PATCH >= z))))

#if MOVEIT_VERSION_AT_LEAST(1, 0, 1)
using Pose = Eigen::Isometry3d;
#else
using Pose = Eigen::Affine3d;
#endif

using PoseSequence = std::vector<Pose, Eigen::aligned_allocator<Pose>>;
using PointSequence = std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>;


// In KAI C++ 3.2, restrict causes problems for copy propagation.
// Temporary fix: disable restrict

#define BZ_DISABLE_RESTRICT

#include <blitz/vector.h>
#include <blitz/array.h>
#include <blitz/rand-uniform.h>
#include <blitz/benchext.h>

#ifdef BZ_HAVE_VALARRAY
	#define BENCHMARK_VALARRAY
#endif

#ifdef BENCHMARK_VALARRAY
#include <valarray>
#endif

BZ_USING_NAMESPACE(blitz)

#if defined(BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES)
 #define loop4_f77 loop4_f77_
 #define loop4_f90 loop4_f90_
#elif defined(BZ_FORTRAN_SYMBOLS_WITH_DOUBLE_TRAILING_UNDERSCORES)
 #define loop4_f77 loop4_f77__
 #define loop4_f90 loop4_f90__
#elif defined(BZ_FORTRAN_SYMBOLS_CAPS)
 #define loop4_f77 LOOP4_F77
 #define loop4_f90 LOOP4_F90
#endif

extern "C" {
  void loop4_f77(const int& niters, const int& N, double* x, double* a, double* b);

  void loop4_f90(const int& niters, const int& N, double* x, double* a, double* b);

}

void VectorVersion(BenchmarkExt<int>& bench);
void ArrayVersion(BenchmarkExt<int>& bench);
void F77Version(BenchmarkExt<int>& bench);
void F90Version(BenchmarkExt<int>& bench);

#ifdef BENCHMARK_VALARRAY
void ValarrayVersion(BenchmarkExt<int>& bench);
#endif

int main()
{
#ifdef BENCHMARK_VALARRAY
    int numBenchmarks = 5;
#else
    int numBenchmarks = 4;
#endif

    BenchmarkExt<int> bench("$x=$a+$b", numBenchmarks);

    const int numSizes = 19;
    bench.setNumParameters(numSizes);
    bench.setRateDescription("Mflops/s");

    Vector<int> parameters(numSizes);
    Vector<long> iters(numSizes);
    Vector<double> flops(numSizes);

    for (int i=0; i < numSizes; ++i)
    {
        parameters[i] = pow(10.0, (i+1)/4.0);
        iters[i] = 50000000L / parameters[i];
        if (iters[i] < 2)
            iters[i] = 2;
        flops[i] = 1 * parameters[i];
    }

    bench.setParameterVector(parameters);
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);

    bench.beginBenchmarking();

    VectorVersion(bench);
    ArrayVersion(bench);
    F77Version(bench);
#ifdef FORTRAN_90
    F90Version(bench);
#endif
#ifdef BENCHMARK_VALARRAY
    ValarrayVersion(bench);
#endif

    bench.endBenchmarking();

    bench.saveMatlabGraph("loop4.m");

    return 0;
}

void initializeRandomDouble(double* data, int numElements, int stride = 1)
{
    static Random<Uniform> rnd;

    for (int i=0; i < numElements; ++i)
        data[i*stride] = rnd.random();
}

template<class T>
void initializeArray(T& array, int numElements)
{
    static Random<Uniform> rnd;

    for (size_t i=0; i < numElements; ++i)
        array[i] = rnd.random();
}

void VectorVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Vector<T>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << "Vector<T>: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Vector<double> x(N);
        initializeArray(x);
        Vector<double> a(N);
        initializeArray(a);
        Vector<double> b(N);
        initializeArray(b);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=a+b;
        }
        bench.stop();
    }

    bench.endImplementation();
}

void ArrayVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Array<T,1>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << "Array<T,1>: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double> x(N);
        initializeArray(x);
        Array<double> a(N);
        initializeArray(a);
        Array<double> b(N);
        initializeArray(b);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=a+b;
        }
        bench.stop();
    }

    bench.endImplementation();
}

void F77Version(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Fortran 77");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Fortran 77: N = " << N << endl;
        cout.flush();

        int iters = bench.getIterations();

        double* x = new double[N];
        initializeRandomDouble(x, N);
        double* a = new double[N];
        initializeRandomDouble(a, N);
        double* b = new double[N];
        initializeRandomDouble(b, N);

        bench.start();
        loop4_f77(iters, N, x, a, b);
        bench.stop();

        delete [] x;
        delete [] a;
        delete [] b;
    }

    bench.endImplementation();
}

#ifdef FORTRAN_90
void F90Version(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Fortran 90");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Fortran 90: N = " << N << endl;
        cout.flush();

        int iters = bench.getIterations();

        double* x = new double[N];
        initializeRandomDouble(x, N);
        double* a = new double[N];
        initializeRandomDouble(a, N);
        double* b = new double[N];
        initializeRandomDouble(b, N);

        bench.start();
        loop4_f90(iters, N, x, a, b);
        bench.stop();

        delete [] x;
        delete [] a;
        delete [] b;
    }

    bench.endImplementation();
}
#endif


// Standard Librry
#include <iostream>
#include <tuple>
#include <vector>

// Eigen
#include <Eigen/Dense>
using Eigen::MatrixXd;
using Eigen::VectorXd;

// OMPL
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/OptimizationObjective.h>

// Internal Libraries
#include "Sampler/RejectionSampler.h"
#include "Sampler/MonteCarloSampler.h"
#include "Sampler/HitAndRunSampler.h"
#include "OmplWrappers/MyOptimizationObjective.h"
#include "OmplWrappers/OmplHelpers.h"
#include "OmplWrappers/DimtStateSpace.h"
#include "Dimt/DoubleIntegratorMinimumTime.h"
#include "Dimt/ProblemGeneration.h"
#include "Benchmark/TimeBenchmark.h"
#include "Benchmark/SampleBenchmark.h"
#include "Benchmark/OptionParse.h"

std::tuple<bool, std::vector<int>> handleArguments(int argc, char *argv[])
{
    if (cmdOptionExists(argv, argv + argc, "-h"))
    {
        std::cout << "________________________________________________" << std::endl;
        std::cout << "Main function for sampling" << std::endl;
        std::cout << "________________________________________________" << std::endl;
        std::cout << "Arguments:" << std::endl;
        std::cout << "\t -samples - Number of samples to get" << std::endl;
        std::cout << "\t -time - Boolean (0,1)" << std::endl;
        std::cout << "\t -hmc - Boolean (0,1)" << std::endl;
        std::cout << "\t -mcmc - Boolean (0,1)" << std::endl;
        std::cout << "\t -rej - Boolean (0,1)" << std::endl;
        std::cout << "\t -hrs - Boolean(0,1)" << std::endl;
        std::cout << "\t -hitnrun - Boolean(0,1)" << std::endl;
        std::cout << "\t -filename - Filename to save the samples to" << std::endl;
        std::cout << "________________________________________________" << std::endl;
        return std::make_tuple(false, std::vector<int>{});
    }
    else
    {
        std::vector<int> args;

        // Get the number of samples
        if (cmdOptionExists(argv, argv + argc, "-samples"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-samples")));
        else
            args.push_back(100);  // Default to 100 samples

        // Get the boolean to determine if we should time
        if (cmdOptionExists(argv, argv + argc, "-time"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-time")));
        else
            args.push_back(0);  // Default to not print time

        // Get the boolean to determine if we run hmc
        if (cmdOptionExists(argv, argv + argc, "-hmc"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-hmc")));
        else
            args.push_back(1);  // Default to run hmc

        // Get the boolean to determine if we run mcmc
        if (cmdOptionExists(argv, argv + argc, "-mcmc"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-mcmc")));
        else
            args.push_back(1);  // Default to run hmc

        // Get the boolean to determine if we run rej
        if (cmdOptionExists(argv, argv + argc, "-rej"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-rej")));
        else
            args.push_back(1);  // Default to run rej

        // Get the boolean to determine if we run dimt hierarchical rejection
        // sampling
        if (cmdOptionExists(argv, argv + argc, "-hrs"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-hrs")));
        else
            args.push_back(1);  // Default to run dimthrs

        // Get the boolean to determine if we run hit and run sampling
        if (cmdOptionExists(argv, argv + argc, "-hitnrun"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-hitnrun")));
        else
            args.push_back(1);  // Default to run dimthrs

        return std::make_tuple(true, args);
    }
}

int main(int argc, char *argv[])
{
    //
    // Example for how to use the above sampler
    //
    bool run;
    std::vector<int> args;
    std::tie(run, args) = handleArguments(argc, argv);
    if (!run)
        return 0;

    int numSamples = args[0];
    bool time = (args[1] == 1) ? true : false;
    bool runHmc = (args[2] == 1) ? true : false;
    bool runMcmc = (args[3] == 1) ? true : false;
    bool runRej = (args[4] == 1) ? true : false;
    bool runHrs = (args[5] == 1) ? true : false;
    bool runHitnrun = (args[6] == 1) ? true : false;

    std::string filename;
    bool save;
    std::tie(save, filename) = get_filename(argc, argv);

    // Create a problem definition
    int numDim = param.dimensions;
    double maxval = param.s_max;
    double minval = -param.s_max;
    UniformRealRandomGenerator uniRndGnr;
    std::chrono::high_resolution_clock::duration duration;

    // Initializations
    std::vector<double> maxVelocities(param.dof, param.v_max);
    std::vector<double> maxAccelerations(param.dof, param.a_max);
    //maxVelocities[1] = 0.1;
    //maxAccelerations[1] = 0.1;
    DIMTPtr dimt = std::make_shared<DIMT>(maxVelocities, maxAccelerations);

    ompl::base::SpaceInformationPtr si = createDimtSpaceInformation(dimt, minval, maxval);
    auto startState = si->allocState();
    auto goalState = si->allocState();
    for (int i = 0; i < numDim; i++)
    {
        startState->as<ompl::base::RealVectorStateSpace::StateType>()->values[i] = uniRndGnr.sample(minval, maxval);
        goalState->as<ompl::base::RealVectorStateSpace::StateType>()->values[i] = uniRndGnr.sample(minval, maxval);
    }

    ompl::base::ProblemDefinitionPtr pdef = createDimtProblem(startState, goalState, si, dimt);

    const double levelSet = 1.4 * dimt->getMinTime(startState, goalState);

    // Initialize the sampler
    // HMC parameters
    MatrixXd hmcSamples;
    if (runHmc)
    {
        double alpha = 0.5;
        double L = 5;
        double epsilon = 0.1;
        double sigma = 1;
        int maxSteps = 50000000;
        ompl::base::HMCSampler hmcSampler(si, pdef, levelSet, 100, 100, alpha, L, epsilon, sigma, maxSteps);
        std::cout << "Running HMC Sampling..." << std::endl;
        hmcSamples = hmcSampler.sample(numSamples, duration);
        if (time)
        {
            std::cout << "Total time ";
            printTime(duration, std::cout);
            std::cout << std::endl;
        }
    }

    MatrixXd mcmcSamples;
    if (runMcmc)
    {
        double alpha = 0.5;
        double sigma = 1;
        int maxSteps = 50000000;
        ompl::base::MCMCSampler mcmcSampler(si, pdef, levelSet, 100, 100, alpha, sigma, maxSteps);
        mcmcSamples = mcmcSampler.sample(numSamples, duration);
        std::cout << "Running MCMC Sampler..." << std::endl;
        if (time)
        {
            std::cout << "Total time ";
            printTime(duration, std::cout);
            std::cout << std::endl;
        }
    }

    MatrixXd rejSamples;
    if (runRej)
    {
        ompl::base::RejectionSampler rejSampler(si, pdef, levelSet, 100, 100);
        std::cout << "Running Rejection Sampler..." << std::endl;
        rejSamples = rejSampler.sample(numSamples, duration);
        if (time)
        {
            std::cout << "Total time ";
            printTime(duration, std::cout);
            std::cout << std::endl;
        }
    }

    MatrixXd dimthrsSamples;
    if (runHrs)
    {
        ompl::base::DimtHierarchicalRejectionSampler dimthrsSampler(si, pdef, dimt,
                                                                    levelSet, 100, 100);
        std::cout << "Running DIMT HRS..." << std::endl;
        dimthrsSamples = dimthrsSampler.sample(numSamples, duration);
        if (time)
        {
            std::cout << "Total time ";
            printTime(duration, std::cout);
            std::cout << std::endl;
        }
    }

    MatrixXd hitnrunSamples;
    if (runHitnrun)
    {
        ompl::base::HitAndRunSampler hitnrunSampler(si, pdef, levelSet, 100, 100, 10);
        std::cout << "Running Hit and Run Sampler..." << std::endl;
        hitnrunSamples = hitnrunSampler.sample(numSamples, duration);
        if (time)
        {
            std::cout << "Total time ";
            printTime(duration, std::cout);
            std::cout << std::endl;
        }
    }

    if (save)
    {
        std::cout << "START SAVING" << std::endl;
        if (runHmc)
        {
            std::ofstream hmcFile(filename + "_hmc.log");
            printSampleToFile(hmcSamples, hmcFile);
        }

        if (runMcmc)
        {
            std::ofstream mcmcFile(filename + "_mcmc.log");
            printSampleToFile(mcmcSamples, mcmcFile);
        }

        if (runRej)
        {
            std::ofstream rejFile(filename + "_rej.log");
            printSampleToFile(rejSamples, rejFile);
        }

        if (runHrs)
        {
            std::ofstream dimthrsFile(filename + "_hrs.log");
            printSampleToFile(dimthrsSamples, dimthrsFile);
        }

        if (runHitnrun)
        {
            std::ofstream hitnrunFile(filename + "_hitnrun.log");
            printSampleToFile(hitnrunSamples, hitnrunFile);
        }

        std::cout << "Saved samples and costs to " << filename << std::endl;
    }
}


/// \file
/// Maintainer: Luzian Hug
///
///

#include "cog_trajectory_builder.h"
#include "generic/center.h"
#include "generic/types.h"

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_CASE(two_points_cog) {
  MouseTrack::PointCloud pc;
  pc.resize(2, 0);

  pc[0].x(0.0);
  pc[0].y(-1.0);
  pc[0].z(3.0);
  pc[0].intensity(0);

  pc[1].x(4.0);
  pc[1].y(3.0);
  pc[1].z(3.0);
  pc[1].intensity(0);

  std::vector<size_t> indices;
  indices.push_back(0);
  indices.push_back(1);
  MouseTrack::Cluster cluster(indices);

  std::shared_ptr<MouseTrack::ClusterDescriptor> cd(
      new MouseTrack::Center(2, 1, 3));

  MouseTrack::CogTrajectoryBuilder tb;

  Eigen::Vector3d cog = tb(cd, cluster, pc);

  BOOST_CHECK_EQUAL(cog, Eigen::Vector3d(2, 1, 3));
}


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the maximum exposant of floating point numbers minus one

    @par Semantic:

    @code
    as_integer<T> r = Maxexponentm1<T>();
    @endcode

    @code
    if T is double
      r =  1022;
    else if T is float
      r =  126;
    @endcode

    @return The Maxexponentm1 constant for the proper type
  **/
  template<typename T> T Maxexponentm1();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant maxexponentm1.

      @return The Maxexponentm1 constant for the proper type
    **/
    const boost::dispatch::functor<tag::maxexponentm1_> maxexponentm1 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/maxexponentm1.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/*
 * Copyright 2009-2019 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.
 *
 */

#include "../../../include/votca/csg/potentialfunctions/potentialfunction.h"
#include <votca/tools/table.h>

#include <boost/lexical_cast.hpp>

using namespace std;
using namespace votca::tools;

namespace votca {
namespace csg {

PotentialFunction::PotentialFunction(const string &name, Index nlam, double min,
                                     double max) {

  name_ = name;
  lam_ = Eigen::VectorXd::Zero(nlam);
  min_ = min;
  cut_off_ = max;
}

void PotentialFunction::setParam(string filename) {

  Table param;
  param.Load(filename);

  if (param.size() != lam_.size()) {

    throw std::runtime_error("In potential " + name_ +
                             ": parameters size mismatch!\n"
                             "Check input parameter file \"" +
                             filename + "\" \nThere should be " +
                             boost::lexical_cast<string>(lam_.size()) +
                             " parameters");
  } else {
    for (Index i = 0; i < lam_.size(); i++) {
      lam_(i) = param.y(i);
    }
  }
}

void PotentialFunction::SaveParam(const string &filename) {

  Table param;
  param.SetHasYErr(false);
  param.resize(lam_.size());

  for (Index i = 0; i < lam_.size(); i++) {
    param.set(i, double(i), lam_(i), 'i');
  }

  param.Save(filename);
}

void PotentialFunction::SavePotTab(const string &filename, double step) {
  Index ngrid = (Index)((cut_off_ - min_) / step + 1.00000001);
  Table pot_tab;
  pot_tab.SetHasYErr(false);
  pot_tab.resize(ngrid);
  double r_init;
  Index i;

  for (r_init = min_, i = 0; i < ngrid - 1; r_init += step) {
    pot_tab.set(i++, r_init, CalculateF(r_init), 'i');
  }

  pot_tab.set(i, cut_off_, CalculateF(cut_off_), 'i');
  pot_tab.Save(filename);
}

void PotentialFunction::SavePotTab(const string &filename, double step,
                                   double rmin, double rcut) {
  Index ngrid = (Index)((rcut - rmin) / step + 1.00000001);
  Table pot_tab;
  pot_tab.SetHasYErr(false);
  pot_tab.resize(ngrid);
  double r_init;
  Index i;
  char flag = 'i';

  for (r_init = rmin, i = 0; i < ngrid - 1; r_init += step) {
    pot_tab.set(i++, r_init, CalculateF(r_init), flag);
  }

  pot_tab.set(i, rcut, CalculateF(rcut), flag);
  pot_tab.Save(filename);
}
}  // namespace csg
}  // namespace votca


//---------------------------------------------------------------------------//
//!
//! \file   tstInverseSquareCentimeterUnit.cpp
//! \author Alex Robinson
//! \brief  The inverse square centimeter unit unit tests
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs/area.hpp>
#include <boost/units/systems/si/area.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "Utility_InverseSquareCentimeterUnit.hpp"
#include "Utility_InverseCentimeterUnit.hpp"

using namespace Utility::Units;
using boost::units::quantity;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the inverse square centimeter unit can be initialized
TEUCHOS_UNIT_TEST( InverseSquareCentimeterUnit, initialize )
{
  quantity<InverseSquareCentimeter>
    inverse_sqr_centimeter( 1.0*inverse_square_centimeter );

  TEST_EQUALITY_CONST( inverse_sqr_centimeter.value(), 1.0 );

  inverse_sqr_centimeter = 2.0*inverse_square_centimeter;

  TEST_EQUALITY_CONST( inverse_sqr_centimeter.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the inv. sqr. cm unit can be initialized from the inv. cm unit
TEUCHOS_UNIT_TEST( InverseSquareCentimeterUnit, initialize_from_inv_cm )
{
  quantity<InverseSquareCentimeter>
    inverse_sqr_centimeter( 1.0*inverse_centimeter*inverse_centimeter );

  TEST_EQUALITY_CONST( inverse_sqr_centimeter.value(), 1.0 );

  inverse_sqr_centimeter = 2.0*inverse_centimeter*inverse_centimeter;

  TEST_EQUALITY_CONST( inverse_sqr_centimeter.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the inv. sqr. cm unit can be initialized from cgs, si units
TEUCHOS_UNIT_TEST( InverseSquareCentimeterUnit, initialize_from_cgs_si )
{
  quantity<InverseSquareCentimeter>
    inverse_sqr_centimeter_cgs( 1.0/cgs::square_centimeter );

  quantity<InverseSquareCentimeter>
    inverse_sqr_centimeter_si( 1.0/si::square_meter );

  TEST_EQUALITY_CONST( inverse_sqr_centimeter_cgs.value(), 1.0 );
  TEST_EQUALITY_CONST( inverse_sqr_centimeter_si.value(), 1e-4 );
}

//---------------------------------------------------------------------------//
// end tstInverseSquareCentimeterUnit.cpp
//---------------------------------------------------------------------------//


#include <boost/numeric/ublas/vector.hpp>

template < class value_type >
void test_scal(int size)
{
  boost::numeric::ublas::vector< value_type > vector(size) ;

  // random generate content for the vector
}

int main()
{
  int max_size = 1000 ;
  typedef std::complex< double > dcomplex ;

  for(int size = 1 ; size < max_size ; ++size) test_scal< double >(size) ;
  for(int size = 1 ; size < max_size ; ++size) test_scal< dcomplex >(size) ;

  return 0 ;
}


//  (C) Copyright Simon West 2011.
//  Use, modification and distribution are 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)

#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_sum_kahan.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_weighted_sum_kahan
//
void test_weighted_sum_kahan()
{
    accumulator_set<float, stats<tag::weighted_sum_kahan>, float > acc;

    BOOST_CHECK_EQUAL(0.0f, weighted_sum_kahan(acc));

    for (size_t i = 0; i < 1e6; ++i) {
        acc(1, weight = 1e-6f);
    }

    BOOST_CHECK_EQUAL(1.0f, weighted_sum_kahan(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_weighted_sum_of_variates_kahan
//
void test_weighted_sum_of_variates_kahan()
{
    accumulator_set<
        float,
        stats<tag::weighted_sum_of_variates_kahan<float, tag::covariate1> >,
        float
    >
    acc;

    BOOST_CHECK_EQUAL(0.0f, weighted_sum_of_variates_kahan(acc));

    for (size_t i = 0; i < 1e6; ++i) {
      acc(0, weight = 1.0f, covariate1 = 1e-6f);
    }

    BOOST_CHECK_EQUAL(1.0f, weighted_sum_of_variates_kahan(acc));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted sum kahan tests");

    test->add(BOOST_TEST_CASE(&test_weighted_sum_kahan));
    test->add(BOOST_TEST_CASE(&test_weighted_sum_of_variates_kahan));

    return test;
}


/*-----------------------------------------------------------|
| Copyright (C) 2016 Yuan-Yen Tai, Hongchul Choi,            |
|                    Jian-Xin Zhu                            |
|                                                            |
| This file is distributed under the terms of the BSD        |
| Berkeley Software Distribution. See the file `LICENSE' in  |
| the root directory of the present distribution.            |
|                                                            |
|-----------------------------------------------------------*/
//
//  header.h
//  TBM^3
//
//  Created by Yuan-Yen Tai on 9/10/15.
//

#ifndef _header_h
#define _header_h

#include <vector>
#include <deque>
#include <map>
#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <cmath>
#include <stdlib.h>

//#pragma once

#if defined __GNUC__
#   pragma GCC system_header
#elif defined __SUNPRO_CC
#   pragma disable_warn
#elif defined _MSC_VER
#   pragma warning(push)
#   pragma warning(disable : 4800)
#endif

// BEGIN INCLUDING 3RD PARTY HEADERS
#include <boost/geometry/index/rtree.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

#if defined __SUNPRO_CC
#   pragma enable_warn
#elif defined _MSC_VER
#   pragma warning(pop)
#endif

#include "../Gramat/include/gramat.h"

namespace tbm{

using namespace std;
using namespace gmt;

//typedef		svar	r_var;
//typedef		smat	r_mat;
//typedef		cvar	x_var;
//typedef		cmat	x_mat;

typedef		dvar	r_var;
typedef		dmat	r_mat;
typedef		zvar	x_var;
typedef		zmat	x_mat;

void ErrorMessage(string errorStr) {
	cout<<errorStr<<endl<<endl;
	exit(0);
}

void ErrorMessage(string filename, unsigned lineNumber, string errorStr) {
	cout<<endl;
	cout<<"In file: \""<< filename <<"\""<<endl;;
	cout<<"error in "<< lineNumber <<": ";;
	cout<<errorStr<<endl<<endl;
	exit(0);
}

#include "string_func.hpp"
#include "math_func.hpp"
#include "block_parser.hpp"
#include "HamiltonianParser.hpp"
#include "import.hpp"
#include "atom.hpp"
#include "lattice.hpp"
#include "order_parameter.hpp"
#include "TBDataSource.hpp"
#include "TBModelBase.hpp"
#include "TBClassicalSpinBase.hpp"

}

#endif


//        Copyright Maarten L. Hekkelman 2013-2021
// 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)

#include <pinch/crypto-engine.hpp>
#include <pinch/error.hpp>
#include <pinch/pinch.hpp>

#include <boost/iostreams/filtering_stream.hpp>

#include <cryptopp/aes.h>
#include <cryptopp/cryptlib.h>
#include <cryptopp/des.h>
#include <cryptopp/factory.h>
#include <cryptopp/files.h>
#include <cryptopp/filters.h>
#include <cryptopp/gfpcrypt.h>
#include <cryptopp/modes.h>
#include <cryptopp/osrng.h>
#include <cryptopp/rsa.h>

namespace io = boost::iostreams;

// --------------------------------------------------------------------

namespace pinch
{

struct TransformDataImpl
{
	std::unique_ptr<CryptoPP::StreamTransformation> m_stream_transformation;
};

TransformData::~TransformData()
{
	delete m_impl;
}

void TransformData::clear()
{
	delete m_impl;
	m_impl = nullptr;
}

void TransformData::reset_encryptor(const std::string &name, const uint8_t *key, const uint8_t *iv)
{
	clear();

	if (name == "3des-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::DES_EDE3>::Encryption>(key, 24, iv) };
	else if (name == "aes128-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::AES>::Encryption>(key, 16, iv) };
	else if (name == "aes192-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::AES>::Encryption>(key, 24, iv) };
	else if (name == "aes256-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::AES>::Encryption>(key, 32, iv) };
	else if (name == "aes128-ctr")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CTR_Mode<CryptoPP::AES>::Encryption>(key, 16, iv) };
	else if (name == "aes192-ctr")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CTR_Mode<CryptoPP::AES>::Encryption>(key, 24, iv) };
	else if (name == "aes256-ctr")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CTR_Mode<CryptoPP::AES>::Encryption>(key, 32, iv) };
	else
		assert(false);
}

void TransformData::reset_decryptor(const std::string &name, const uint8_t *key, const uint8_t *iv)
{
	clear();

	if (name == "3des-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::DES_EDE3>::Decryption>(key, 24, iv) };
	else if (name == "aes128-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::AES>::Decryption>(key, 16, iv) };
	else if (name == "aes192-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::AES>::Decryption>(key, 24, iv) };
	else if (name == "aes256-cbc")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CBC_Mode<CryptoPP::AES>::Decryption>(key, 32, iv) };
	else if (name == "aes128-ctr")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CTR_Mode<CryptoPP::AES>::Decryption>(key, 16, iv) };
	else if (name == "aes192-ctr")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CTR_Mode<CryptoPP::AES>::Decryption>(key, 24, iv) };
	else if (name == "aes256-ctr")
		m_impl = new TransformDataImpl { std::make_unique<CryptoPP::CTR_Mode<CryptoPP::AES>::Decryption>(key, 32, iv) };
	else
		assert(false);
}

void TransformData::process(const uint8_t *in, std::size_t len, uint8_t *out)
{
	assert(m_impl);
	assert(m_impl->m_stream_transformation);
	m_impl->m_stream_transformation->ProcessData(out, in, len);
}

std::size_t TransformData::get_block_size() const
{
	return m_impl->m_stream_transformation->OptimalBlockSize();
}

// --------------------------------------------------------------------

struct MessageAuthenticationCodeImpl
{
	std::unique_ptr<CryptoPP::MessageAuthenticationCode> m_verify;
};

MessageAuthenticationCode::~MessageAuthenticationCode()
{
	delete m_impl;
}

void MessageAuthenticationCode::clear()
{
	delete m_impl;
	m_impl = nullptr;
}

void MessageAuthenticationCode::reset(const std::string &name, const uint8_t *iv)
{
	clear();

	if (name == "hmac-sha2-512")
		m_impl = new MessageAuthenticationCodeImpl{ std::make_unique<CryptoPP::HMAC<CryptoPP::SHA512>>(iv, 64) };
	else if (name == "hmac-sha2-256")
		m_impl = new MessageAuthenticationCodeImpl{ std::make_unique<CryptoPP::HMAC<CryptoPP::SHA256>>(iv, 32) };
	else if (name == "hmac-sha1")
		m_impl = new MessageAuthenticationCodeImpl{ std::make_unique<CryptoPP::HMAC<CryptoPP::SHA1>>(iv, 20) };
	else
		assert(false);
}

void MessageAuthenticationCode::update(const uint8_t *data, std::size_t len)
{
	m_impl->m_verify->Update(data, len);
}

bool MessageAuthenticationCode::verify(const uint8_t *signature)
{
	return m_impl->m_verify->Verify(signature);
}

std::size_t MessageAuthenticationCode::get_digest_size() const
{
	return m_impl->m_verify->DigestSize();
}

// --------------------------------------------------------------------

struct packet_encryptor
{
	typedef char char_type;
	struct category : io::multichar_output_filter_tag, io::flushable_tag
	{
	};

	packet_encryptor(CryptoPP::StreamTransformation &cipher,
	                 CryptoPP::MessageAuthenticationCode &signer, uint32_t blocksize, uint32_t seq_nr)
		: m_cipher(cipher)
		, m_signer(signer)
		, m_blocksize(blocksize)
		, m_flushed(false)
	{
		for (int i = 3; i >= 0; --i)
		{
			uint8_t ch = static_cast<uint8_t>(seq_nr >> (i * 8));
			m_signer.Update(&ch, 1);
		}

		m_block.reserve(m_blocksize);
	}

	template <typename Sink>
	std::streamsize write(Sink &sink, const char *s, std::streamsize n)
	{
		std::streamsize result = 0;

		for (std::streamsize o = 0; o < n; o += m_blocksize)
		{
			size_t k = n;
			if (k > m_blocksize - m_block.size())
				k = m_blocksize - m_block.size();

			const uint8_t *sp = reinterpret_cast<const uint8_t *>(s);

			m_signer.Update(sp, static_cast<size_t>(k));
			m_block.insert(m_block.end(), sp, sp + k);

			result += k;
			s += k;

			if (m_block.size() == m_blocksize)
			{
				blob block(m_blocksize);
				m_cipher.ProcessData(block.data(), m_block.data(), m_blocksize);

				for (uint32_t i = 0; i < m_blocksize; ++i)
					io::put(sink, block[i]);

				m_block.clear();
			}
		}

		return result;
	}

	template <typename Sink>
	bool flush(Sink &sink)
	{
		if (not m_flushed)
		{
			assert(m_block.size() == 0);

			blob digest(m_signer.DigestSize());
			m_signer.Final(digest.data());
			for (size_t i = 0; i < digest.size(); ++i)
				io::put(sink, digest[i]);

			m_flushed = true;
		}

		return true;
	}

	CryptoPP::StreamTransformation &m_cipher;
	CryptoPP::MessageAuthenticationCode &m_signer;
	blob m_block;
	uint32_t m_blocksize;
	bool m_flushed;
};

// --------------------------------------------------------------------

crypto_engine::crypto_engine()
{
}

void crypto_engine::newkeys(key_exchange &kex, bool authenticated)
{
	using namespace CryptoPP;

	// Client to server encryption
	m_alg_enc_c2s = kex.get_encryption_protocol(direction::c2s);

	const uint8_t *key = kex.key(key_exchange::C);
	const uint8_t *iv = kex.key(key_exchange::A);

	m_encryptor.reset_encryptor(m_alg_enc_c2s, key, iv);

	// Server to client encryption
	m_alg_enc_s2c = kex.get_encryption_protocol(direction::s2c);

	key = kex.key(key_exchange::D);
	iv = kex.key(key_exchange::B);

	m_decryptor.reset_decryptor(m_alg_enc_s2c, key, iv);

	// Client To Server verification
	m_alg_ver_c2s = kex.get_verification_protocol(direction::c2s);
	iv = kex.key(key_exchange::E);

	m_signer.reset(m_alg_ver_c2s, iv);

	// Server to Client verification

	m_alg_ver_s2c = kex.get_verification_protocol(direction::s2c);
	iv = kex.key(key_exchange::F);

	m_verifier.reset(m_alg_ver_s2c, iv);

	// Client to Server compression
	m_alg_cmp_c2s = kex.get_compression_protocol(direction::c2s);
	if ((not m_compressor and m_alg_cmp_c2s == "zlib") or (authenticated and m_alg_cmp_c2s == "zlib@openssh.com"))
		m_compressor.reset(new compression_helper(true));
	else if (m_alg_cmp_c2s == "zlib@openssh.com")
		m_delay_compressor = true;

	// Server to Client compression
	m_alg_cmp_s2c = kex.get_compression_protocol(direction::s2c);
	if ((not m_decompressor and m_alg_cmp_s2c == "zlib") or (authenticated and m_alg_cmp_s2c == "zlib@openssh.com"))
		m_decompressor.reset(new compression_helper(false));
	else if (m_alg_cmp_c2s == "zlib@openssh.com")
		m_delay_decompressor = true;

	if (m_decryptor)
	{
		m_iblocksize = m_decryptor.get_block_size();
		m_oblocksize = m_encryptor.get_block_size();
	}
}

void crypto_engine::reset()
{
	m_packet.reset();
	m_encryptor.clear();
	m_decryptor.clear();
	m_signer.clear();
	m_verifier.clear();
	m_compressor.reset(nullptr);
	m_decompressor.reset(nullptr);
	m_delay_decompressor = m_delay_compressor = false;
	m_in_seq_nr = m_out_seq_nr = 0;
	m_iblocksize = m_oblocksize = 8;

	m_alg_kex.clear();
	m_alg_enc_c2s.clear();
	m_alg_ver_c2s.clear();
	m_alg_cmp_c2s.clear();
	m_alg_enc_s2c.clear();
	m_alg_ver_s2c.clear();
	m_alg_cmp_s2c.clear();
}

std::string crypto_engine::get_connection_parameters(direction dir) const
{
	std::string result;

	if (dir == direction::c2s)
	{
		result = m_alg_enc_c2s + '/' + m_alg_ver_c2s;

		if (m_alg_cmp_c2s != "none")
			result = result + '/' + m_alg_cmp_c2s;
	}
	else
	{
		result = m_alg_enc_s2c + '/' + m_alg_ver_s2c;

		if (m_alg_cmp_s2c != "none")
			result = result + '/' + m_alg_cmp_s2c;
	}

	return result;
}

std::string crypto_engine::get_key_exchange_algorithm() const
{
	return m_alg_kex;
}

blob crypto_engine::get_next_block(boost::asio::streambuf &buffer, bool empty)
{
	blob block(m_iblocksize);
	buffer.sgetn(reinterpret_cast<char *>(block.data()), m_iblocksize);

	if (m_decryptor)
	{
		blob data(m_iblocksize);
		m_decryptor.process(block.data(), m_iblocksize, data.data());
		std::swap(data, block);
	}

	if (m_verifier)
	{
		if (empty)
		{
			for (int32_t i = 3; i >= 0; --i)
			{
				uint8_t b = m_in_seq_nr >> (i * 8);
				m_verifier.update(&b, 1);
			}
		}

		m_verifier.update(block.data(), block.size());
	}

	return block;
}

std::unique_ptr<ipacket> crypto_engine::get_next_packet(boost::asio::streambuf &buffer, boost::system::error_code &ec)
{
	std::lock_guard lock(m_in_mutex);

	if (not m_packet)
		m_packet = std::make_unique<ipacket>(m_in_seq_nr);

	bool complete_and_verified = false;

	while (buffer.size() >= m_iblocksize)
	{
		if (not m_packet->complete())
			m_packet->append(get_next_block(buffer, m_packet->empty()));

		if (m_packet->complete())
		{
			if (m_verifier)
			{
				const std::size_t digest_size = m_verifier.get_digest_size();

				if (buffer.size() < digest_size)
					break;

				blob digest(digest_size);
				buffer.sgetn(reinterpret_cast<char *>(digest.data()), digest_size);

				if (not m_verifier.verify(digest.data()))
				{
					ec = error::make_error_code(error::mac_error);
					break;
				}
			}

			if (m_decompressor)
				m_packet->decompress(*m_decompressor, ec);

			++m_in_seq_nr;

			complete_and_verified = true;
			break;
		}
	}

	return complete_and_verified ? std::move(m_packet) : std::unique_ptr<ipacket>();
}

std::unique_ptr<boost::asio::streambuf> crypto_engine::get_next_request(opacket &&p)
{
	std::lock_guard lock(m_out_mutex);

	auto request = std::make_unique<boost::asio::streambuf>();

	if (m_compressor)
	{
		boost::system::error_code ec;
		p.compress(*m_compressor, ec);

		if (ec)
			throw ec;
	}

	io::filtering_stream<io::output> out;
	if (m_encryptor)
		out.push(packet_encryptor(*m_encryptor.m_impl->m_stream_transformation, *m_signer.m_impl->m_verify, m_oblocksize, m_out_seq_nr));
	out.push(*request);

	p.write(out, m_oblocksize);

	++m_out_seq_nr;

	return request;
}

void crypto_engine::enable_compression()
{
	if (m_delay_compressor)
		m_compressor.reset(new compression_helper(true));

	if (m_delay_decompressor)
		m_decompressor.reset(new compression_helper(false));
}

} // namespace pinch


// Copyright (C) 2013 Massachusetts Institute of Technology, Lincoln Laboratory
// License: Boost Software License   See LICENSE.txt for the full license.
// Authors: Davis E. King (davis@dlib.net)

#include "word_vects.h"
#include <dlib/matrix.h>
#include <dlib/statistics.h>
#include <dlib/sliding_buffer.h>
#include <map>
#include <mitie/group_tokenizer.h>
#include <mitie/unigram_tokenizer.h>

using namespace std;
using namespace dlib;
using namespace mitie;

typedef std::vector<std::pair<dlib::uint32, float> > sparse_vector_type;

// ----------------------------------------------------------------------------------------

unsigned long get_word_id (
    const std::map<std::string, unsigned long>& vocab,
    const std::string& word
)
{
    std::map<std::string, unsigned long>::const_iterator i;
    i = vocab.find(word);
    if (i != vocab.end())
        return i->second;
    else
        return vocab.size();
}

// ----------------------------------------------------------------------------------------

template <typename sparse_vector_type>
void get_left_and_right_context_vectors (
    const std::map<std::string, unsigned long>& vocab,
    const dlib::circular_buffer<std::string>& buf,
    sparse_vector_type& left_context,
    sparse_vector_type& right_context
)
{
    // get the left context vector
    left_context.clear();
    for (unsigned long i = 0; i < buf.size()/2; ++i)
    {
        left_context.push_back(make_pair(i*(vocab.size()+1) + get_word_id(vocab, buf[i]), 1));
    }
    make_sparse_vector_inplace(left_context);

    // get the right context vector
    right_context.clear();
    long k = 0;
    for (unsigned long i = buf.size()/2+1; i < buf.size(); ++i)
    {
        right_context.push_back(make_pair(k*(vocab.size()+1) + get_word_id(vocab, buf[i]), 1));
        ++k;
    }
    make_sparse_vector_inplace(right_context);
}

// ----------------------------------------------------------------------------------------

template <typename tokenizer_type>
void do_cca_on_windows (
    const std::map<std::string, unsigned long>& vocab,
    const long window_size,
    const long num_contexts,
    const long num_correlations,
    tokenizer_type& tok,
    matrix<float>& Ltrans,
    matrix<float>& Rtrans
)
{
    tok.reset();

    random_subset_selector<sparse_vector_type> left_contexts, right_contexts;
    left_contexts.set_max_size(num_contexts);
    right_contexts.set_max_size(num_contexts);

    dlib::circular_buffer<std::string> buf;
    buf.resize(window_size);

    sparse_vector_type left_vect, right_vect;
    cout << "Sample " << num_contexts << " random context vectors" << endl;

    std::string token;
    uint64 count = 0;
    while (tok(token))
    {
        buf.push_back(token);
        // skip the rest of the loop if buf isn't full yet
        ++count;
        if (count < buf.size())
        {
            continue;
        }

        if (left_contexts.next_add_accepts())
        {
            get_left_and_right_context_vectors(vocab, buf, left_vect, right_vect);
            left_contexts.add(left_vect);
            right_contexts.add(right_vect);
        }
        else
        {
            left_contexts.add();
            right_contexts.add();
        }
    }

    std::vector<sparse_vector_type> left(left_contexts.begin(), left_contexts.end());
    std::vector<sparse_vector_type> right(right_contexts.begin(), right_contexts.end());
    // free RAM
    left_contexts.set_max_size(0);
    right_contexts.set_max_size(0);

    cout << "Now do CCA (left size: " << left.size() << ", right size: " << right.size() << ")." << endl;
    cout << "correlations: "<< trans(cca(left, right, Ltrans, Rtrans, num_correlations, 40, 5));
    //print_true_correlations(left, right, Ltrans, Rtrans);
}

// ----------------------------------------------------------------------------------------

template <typename tokenizer_type>
void get_average_context_window_vector_per_word (
    const std::map<std::string, unsigned long>& vocab,
    const long window_size,
    tokenizer_type& tok,
    const matrix<float>& Ltrans,
    const matrix<float>& Rtrans,
    std::map<std::string, matrix<float,0,1> >& word_vectors
)
{
    tok.reset();
    word_vectors.clear();

    std::map<std::string,long> word_hits;
    dlib::circular_buffer<std::string> buf;
    buf.resize(window_size);

    sparse_vector_type left_vect, right_vect;

    std::string token;
    unsigned long count = 1;
    while (tok(token))
    {
        buf.push_back(token);
        // skip the rest of the loop if buf isn't full yet
        if (count < buf.size())
        {
            ++count;
            continue;
        }

        const std::string& center_word = buf[buf.size()/2];
        // only consider words in the vocab
        if (vocab.count(center_word) == 0)
            continue;

        get_left_and_right_context_vectors(vocab, buf, left_vect, right_vect);
        word_vectors[center_word] += join_cols(sparse_matrix_vector_multiply(trans(Ltrans), left_vect),
                                               sparse_matrix_vector_multiply(trans(Rtrans), right_vect));
        word_hits[center_word] += 1;
    }

    // Divide all the word vectors by their hits so they are mean vectors
    for (std::map<std::string,long>::iterator i = word_hits.begin(); i != word_hits.end(); ++i)
    {
        if (i->second > 1)
        {
            word_vectors[i->first] /= i->second;
        }
    }
}

// ----------------------------------------------------------------------------------------

std::map<std::string, unsigned long> make_word_to_int_mapping (
    const std::map<std::string, unsigned long>& words,
    unsigned long num
)
{
    std::vector<std::pair<unsigned long, std::string> > temp;
    temp.reserve(words.size());
    for (std::map<std::string, unsigned long>::const_iterator i = words.begin(); i != words.end(); ++i)
    {
        temp.push_back(make_pair(i->second, i->first));
    }

    std::sort(temp.rbegin(), temp.rend());

    // pick the most common words for our result set
    std::map<std::string, unsigned long> res;
    for (unsigned long i = 0; i < temp.size() && res.size() < num; ++i)
    {
        res[temp[i].second] = i;
    }
    return res;
}

// ----------------------------------------------------------------------------------------

void word_vects(const dlib::command_line_parser& parser)
{
    const long vocab_size = 200000;
    const long window_size = 9;
    const long num_contexts = 50000000;
    const long num_correlations = 90;

    ifstream fin("top_word_counts.dat", ios::binary);
    std::map<std::string, unsigned long> words;
    deserialize(words, fin);

    words = make_word_to_int_mapping(words,vocab_size);

    std::vector<dlib::file> files = get_files_in_directory_tree(directory(parser[0]), match_all());
    cout << "number of raw ASCII files found: " << files.size() << endl;
    group_tokenizer<unigram_tokenizer> tok(files);

    matrix<float> Ltrans, Rtrans;
    do_cca_on_windows(words, window_size,  num_contexts, num_correlations, tok, Ltrans, Rtrans);
    cout << "CCA done, now build up average word vectors" << endl;

    std::map<std::string, matrix<float,0,1> > word_vectors;
    get_average_context_window_vector_per_word(words, window_size, tok, Ltrans, Rtrans, word_vectors);

    std::ofstream fout("word_vects.dat", ios::binary);
    serialize(word_vectors, fout);
}

// ----------------------------------------------------------------------------------------


#include "Curvilinear.h"
#include "Affine.h"
#include "Vector.h"
#include "mesh/LocalSimplexMesh.h"
#include "mesh/MeshData.h"
#include "tensor/EigenMap.h"
#include "tensor/Managed.h"
#include "tensor/Reshape.h"
#include "util/Math.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iterator>
#include <memory>
#include <stdexcept>

namespace tndm {

template <std::size_t D>
Curvilinear<D>::Curvilinear(LocalSimplexMesh<D> const& mesh, transform_t transform, unsigned degree,
                            NodesFactory<D> const& nodesFactory)
    : N(degree), refElement_(degree, nodesFactory) {
    // Get vertices
    std::size_t vertsPerElement = refElement_.refNodes().size();

    auto storage =
        std::make_shared<mneme::SingleStorage<Verts>>(mesh.numElements() * vertsPerElement);
    vertices.setStorage(storage, 0, mesh.numElements(), vertsPerElement);

    auto vertexData = dynamic_cast<VertexData<D> const*>(mesh.vertices().data());
    if (!vertexData) {
        throw std::runtime_error("Expected vertex data");
    }

    std::size_t vertexNo = 0;
    local_mesh_size_ = 0.0;
    for (auto const& element : mesh.elements()) {
        auto vlids = mesh.template downward<0>(element);
        std::array<std::array<double, D>, D + 1> verts;
        std::size_t localVertexNo = 0;
        for (auto const& vlid : vlids) {
            verts[localVertexNo++] = vertexData->getVertices()[vlid];
        }
        RefPlexToGeneralPlex<D> map(verts);
        for (auto& refNode : refElement_.refNodes()) {
            (*storage)[vertexNo] = transform(map(refNode));
            ++vertexNo;
        }

        for (auto& x : verts) {
            x = transform(x);
        }
        for (auto const& x : verts) {
            for (auto const& y : verts) {
                auto h = norm(x - y);
                local_mesh_size_ = std::max(local_mesh_size_, h);
            }
        }
    }

    Simplex<D> refPlex = Simplex<D>::referenceSimplex();
    f2v = refPlex.downward();

    // Compute reference normals
    std::size_t fsNo = 0;
    for (auto const& f : f2v) {
        std::array<double, D> normal;
        if constexpr (D == 1u) {
            normal[0] = 1.0;
        } else if constexpr (D == 2u) {
            // Compute normal by rotating edge
            normal = refVertices[f[1]] - refVertices[f[0]];
            std::swap(normal[0], normal[1]);
            normal[0] *= -1.0;
        } else {
            // Compute normal by cross product
            auto e1 = refVertices[f[1]] - refVertices[f[0]];
            auto e2 = refVertices[f[2]] - refVertices[f[0]];
            normal = cross(e1, e2);
        }

        refNormals[fsNo] = Eigen::Map<Eigen::Matrix<double, D, 1>>(normal.data());

        std::vector<uint64_t> missingVertex;
        std::set_difference(refPlex.begin(), refPlex.end(), f.begin(), f.end(),
                            std::inserter(missingVertex, missingVertex.begin()));
        assert(missingVertex.size() == 1);

        auto edge = refVertices[missingVertex[0]] - refVertices[f[0]];
        if (dot(normal, edge) > 0) {
            refNormals[fsNo] *= -1.0;
        }
        ++fsNo;
    }
}

template <std::size_t D>
TensorBase<Matrix<double>> Curvilinear<D>::mapResultInfo(std::size_t numPoints) const {
    return TensorBase<Matrix<double>>(D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::map(std::size_t eleNo, Matrix<double> const& E,
                         Tensor<double, 2u>& result) const {
    assert(eleNo < vertices.size());
    assert(result.shape(0) == D);
    assert(result.shape(1) == E.shape(1));
    assert(E.shape(0) == refElement_.numBasisFunctions());

    auto vertexSpan = vertices[eleNo];
    assert(vertexSpan.size() == refElement_.numBasisFunctions());
    Eigen::Map<Eigen::Matrix<double, D, Eigen::Dynamic>> vertMap(vertexSpan.data()->data(), D,
                                                                 refElement_.numBasisFunctions());
    EigenMap(result) = vertMap * EigenMap(E);
}

template <std::size_t D>
TensorBase<Tensor<double, 3u>> Curvilinear<D>::jacobianResultInfo(std::size_t numPoints) const {
    return TensorBase<Tensor<double, 3u>>(D, D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::jacobian(std::size_t eleNo, Tensor<double, 3u> const& gradE,
                              Tensor<double, 3u>& result) const {
    assert(eleNo < vertices.size());

    auto vertexSpan = vertices[eleNo];
    assert(vertexSpan.size() == refElement_.numBasisFunctions());
    Eigen::Map<Eigen::Matrix<double, D, Eigen::Dynamic>> vertMap(vertexSpan.data()->data(), D,
                                                                 refElement_.numBasisFunctions());

    assert(gradE.shape(0) == refElement_.numBasisFunctions());
    assert(gradE.shape(1) == D);
    auto gradEMat = reshape(gradE, refElement_.numBasisFunctions(), D * gradE.shape(2));

    assert(result.shape(0) == D);
    assert(result.shape(1) == D);
    assert(result.shape(2) == gradE.shape(2));
    auto resultMat = reshape(result, D, D * gradE.shape(2));

    EigenMap(resultMat) = vertMap * EigenMap(gradEMat);
}

template <std::size_t D>
void Curvilinear<D>::jacobianInv(Tensor<double, 3u> const& jacobian,
                                 Tensor<double, 3u>& result) const {
    for (std::ptrdiff_t i = 0; i < result.shape(2); ++i) {
        auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
        auto resAtP = result.subtensor(slice{}, slice{}, i);
        EigenMap<Matrix<double>, D, D>(resAtP) =
            EigenMap<const Matrix<double>, D, D>(jAtP).inverse();
    }
}

template <std::size_t D>
TensorBase<Vector<double>> Curvilinear<D>::detJResultInfo(std::size_t numPoints) const {
    return TensorBase<Vector<double>>(numPoints);
}

template <std::size_t D>
void Curvilinear<D>::detJ(std::size_t eleNo, Tensor<double, 3u> const& jacobian,
                          Tensor<double, 1u>& result) const {
    for (std::ptrdiff_t i = 0; i < result.shape(0); ++i) {
        auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
        result(i) = EigenMap<const Matrix<double>, D, D>(jAtP).determinant();
    }
}

template <std::size_t D>
void Curvilinear<D>::absDetJ(std::size_t eleNo, Tensor<double, 3u> const& jacobian,
                             Tensor<double, 1u>& result) const {
    for (std::ptrdiff_t i = 0; i < result.shape(0); ++i) {
        auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
        result(i) = std::fabs(EigenMap<const Matrix<double>, D, D>(jAtP).determinant());
    }
}

template <std::size_t D>
TensorBase<Matrix<double>> Curvilinear<D>::normalResultInfo(std::size_t numPoints) const {
    return TensorBase<Matrix<double>>(D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::normal(std::size_t faceNo, Tensor<double, 1u> const& detJ,
                            Tensor<double, 3u> const& jInv, Tensor<double, 2u>& result) const {
    assert(faceNo < D + 1u);
    // n_{iq} = |J|_q J^{-T}_{ijq} N_j
    for (std::ptrdiff_t i = 0; i < detJ.shape(0); ++i) {
        auto jInvAtP = jInv.subtensor(slice{}, slice{}, i);
        auto res = result.subtensor(slice{}, i);
        EigenMap<Vector<double>, D>(res) =
            std::fabs(detJ(i)) * EigenMap<const Matrix<double>, D, D>(jInvAtP).transpose() *
            refNormals[faceNo];
    }
}

template <std::size_t D> void Curvilinear<D>::normalize(Tensor<double, 2u>& normal) const {
    for (std::ptrdiff_t i = 0; i < normal.shape(1); ++i) {
        auto n = normal.subtensor(slice{}, i);
        EigenMap<Vector<double>, D>(n).normalize();
    }
}

template <std::size_t D>
TensorBase<Tensor<double, 3u>> Curvilinear<D>::facetBasisResultInfo(std::size_t numPoints) const {
    return TensorBase<Tensor<double, 3u>>(D, D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::facetBasis(std::array<double, D> const& up, Matrix<double> const& normal,
                                Tensor<double, 3u>& result) const {
    assert(result.shape(2) == normal.shape(1));

    constexpr double colinear_tol = 10000.0 * std::numeric_limits<double>::epsilon();

    for (std::ptrdiff_t i = 0; i < result.shape(2); ++i) {
        auto n_in = normal.subtensor(slice{}, i);
        auto n_in_eigen = EigenMap<Vector<double>, D>(n_in);
        auto n = result.subtensor(slice{}, 0, i);
        auto n_eigen = EigenMap<Vector<double>, D>(n);
        n_eigen = n_in_eigen.normalized();

        if constexpr (D == 2u) {
            double s = sgn(up[0] * n(1) - up[1] * n(0));
            if (std::fabs(s) < colinear_tol) {
                throw std::logic_error("Up vector and normal are almost colinear.");
            }
            auto d = result.subtensor(slice{}, 1, i);
            d(0) = -s * n(1);
            d(1) = s * n(0);
        } else if constexpr (D == 3u) {
            auto u = Eigen::Vector3d(up.data());
            auto d = result.subtensor(slice{}, 1, i);
            auto d_eigen = EigenMap<Vector<double>, D>(d);
            auto s = result.subtensor(slice{}, 2, i);
            auto s_eigen = EigenMap<Vector<double>, D>(s);
            s_eigen = u.cross(n_eigen).normalized();
            if (s_eigen.norm() < colinear_tol) {
                throw std::logic_error("Up vector and normal are almost colinear.");
            }
            d_eigen = s_eigen.cross(n_eigen).normalized();
        }
    }
}

template <std::size_t D>
void Curvilinear<D>::facetBasisFromPlexTangents(std::size_t faceNo,
                                                Tensor<double, 3u> const& jacobian,
                                                Matrix<double> const& normal,
                                                Tensor<double, 3u>& result) const {
    assert(result.shape(2) == normal.shape(1));
    assert(result.shape(2) == jacobian.shape(2));

    auto& f = f2v[faceNo];
    auto refTangent = refVertices[f[1]] - refVertices[f[0]];
    for (std::ptrdiff_t i = 0; i < result.shape(2); ++i) {
        auto n = normal.subtensor(slice{}, i);
        auto n_eigen = EigenMap<Vector<double>, D>(n);
        auto n_res = result.subtensor(slice{}, 0, i);
        auto n_res_eigen = EigenMap<Vector<double>, D>(n_res);
        n_res_eigen = n_eigen.normalized();

        if constexpr (D >= 2u) {
            auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
            auto t1_res = result.subtensor(slice{}, 1, i);
            auto t1_res_eigen = EigenMap<Vector<double>, D>(t1_res);
            // first tangent = J * refTangent
            t1_res_eigen = EigenMap<const Matrix<double>, D, D>(jAtP) *
                           Eigen::Map<Eigen::Matrix<double, D, 1>>(refTangent.data());
            t1_res_eigen.normalize();

            if constexpr (D == 3u) {
                auto t2_res = result.subtensor(slice{}, 2, i);
                auto t2_res_eigen = EigenMap<Vector<double>, D>(t2_res);
                t2_res_eigen = n_res_eigen.cross(t1_res_eigen);
                t2_res_eigen.normalize();
            }
        }
    }
}

template <std::size_t D>
std::array<double, D> Curvilinear<D>::facetParam(std::size_t faceNo,
                                                 std::array<double, D - 1> const& chi) const {
    auto& f = f2v[faceNo];
    std::array<double, D> xi;
    double chiSum = 0.0;
    for (std::size_t d = 0; d < chi.size(); ++d) {
        chiSum += chi[d];
    }
    xi = (1.0 - chiSum) * refVertices[f[0]];
    for (std::size_t d = 0; d < chi.size(); ++d) {
        xi = xi + chi[d] * refVertices[f[d + 1]];
    }
    return xi;
}

template <std::size_t D>
std::vector<std::array<double, D>>
Curvilinear<D>::facetParam(std::size_t faceNo,
                           std::vector<std::array<double, D - 1>> const& chis) const {
    std::vector<std::array<double, D>> xis;
    xis.reserve(chis.size());
    for (auto const& chi : chis) {
        xis.emplace_back(facetParam(faceNo, chi));
    }
    return xis;
}

template class Curvilinear<1ul>;
template class Curvilinear<2ul>;
template class Curvilinear<3ul>;

} // namespace tndm