//=======================================================================
// Copyright 2009 Trustees of Indiana University.
// Authors: Michael Hansen, Andrew Lumsdaine
//
// 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 <fstream>
#include <iostream>
#include <set>

#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/random.hpp>
#include <boost/core/lightweight_test.hpp>

using namespace boost;

// Function that prints a vertex to std::cout
template < typename Vertex > void print_vertex(Vertex vertex_to_print)
{

    std::cout << "(";

    for (std::size_t dimension_index = 0;
         dimension_index < vertex_to_print.size(); ++dimension_index)
    {
        std::cout << vertex_to_print[dimension_index];

        if (dimension_index != (vertex_to_print.size() - 1))
        {
            std::cout << ", ";
        }
    }

    std::cout << ")";
}

template < unsigned int Dims > void do_test(minstd_rand& generator)
{
    typedef grid_graph< Dims > Graph;
    typedef
        typename graph_traits< Graph >::vertices_size_type vertices_size_type;
    typedef typename graph_traits< Graph >::edges_size_type edges_size_type;

    typedef typename graph_traits< Graph >::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits< Graph >::edge_descriptor edge_descriptor;

    std::cout << "Dimensions: " << Dims << ", lengths: ";

    // Randomly generate the dimension lengths (3-10) and wrapping
    boost::array< vertices_size_type, Dims > lengths;
    boost::array< bool, Dims > wrapped;

    for (unsigned int dimension_index = 0; dimension_index < Dims;
         ++dimension_index)
    {
        lengths[dimension_index] = 3 + (generator() % 8);
        wrapped[dimension_index] = ((generator() % 2) == 0);

        std::cout << lengths[dimension_index]
                  << (wrapped[dimension_index] ? " [W]" : " [U]") << ", ";
    }

    std::cout << std::endl;

    Graph graph(lengths, wrapped);

    // Verify dimension lengths and wrapping
    for (unsigned int dimension_index = 0; dimension_index < Dims;
         ++dimension_index)
    {
        BOOST_TEST(
            graph.length(dimension_index) == lengths[dimension_index]);
        BOOST_TEST(
            graph.wrapped(dimension_index) == wrapped[dimension_index]);
    }

    // Verify matching indices
    for (vertices_size_type vertex_index = 0;
         vertex_index < num_vertices(graph); ++vertex_index)
    {
        BOOST_TEST(
            get(boost::vertex_index, graph, vertex(vertex_index, graph))
            == vertex_index);
    }

    for (edges_size_type edge_index = 0; edge_index < num_edges(graph);
         ++edge_index)
    {

        edge_descriptor current_edge = edge_at(edge_index, graph);
        BOOST_TEST(
            get(boost::edge_index, graph, current_edge) == edge_index);
    }

    // Verify all vertices are within bounds
    vertices_size_type vertex_count = 0;
    BOOST_FOREACH (vertex_descriptor current_vertex, vertices(graph))
    {

        vertices_size_type current_index
            = get(boost::vertex_index, graph, current_vertex);

        for (unsigned int dimension_index = 0; dimension_index < Dims;
             ++dimension_index)
        {
            BOOST_TEST(
                /*(current_vertex[dimension_index] >= 0) && */ // Always true
                (current_vertex[dimension_index] < lengths[dimension_index]));
        }

        // Verify out-edges of this vertex
        edges_size_type out_edge_count = 0;
        std::set< vertices_size_type > target_vertices;

        BOOST_FOREACH (
            edge_descriptor out_edge, out_edges(current_vertex, graph))
        {

            target_vertices.insert(
                get(boost::vertex_index, graph, target(out_edge, graph)));

            ++out_edge_count;
        }

        BOOST_TEST(out_edge_count == out_degree(current_vertex, graph));

        // Verify in-edges of this vertex
        edges_size_type in_edge_count = 0;

        BOOST_FOREACH (edge_descriptor in_edge, in_edges(current_vertex, graph))
        {

            BOOST_TEST(target_vertices.count(get(boost::vertex_index, graph,
                              source(in_edge, graph)))
                > 0);

            ++in_edge_count;
        }

        BOOST_TEST(in_edge_count == in_degree(current_vertex, graph));

        // The number of out-edges and in-edges should be the same
        BOOST_TEST(degree(current_vertex, graph)
            == out_degree(current_vertex, graph)
                + in_degree(current_vertex, graph));

        // Verify adjacent vertices to this vertex
        vertices_size_type adjacent_count = 0;

        BOOST_FOREACH (vertex_descriptor adjacent_vertex,
            adjacent_vertices(current_vertex, graph))
        {

            BOOST_TEST(target_vertices.count(
                              get(boost::vertex_index, graph, adjacent_vertex))
                > 0);

            ++adjacent_count;
        }

        BOOST_TEST(adjacent_count == out_degree(current_vertex, graph));

        // Verify that this vertex is not listed as connected to any
        // vertices outside of its adjacent vertices.
        BOOST_FOREACH (vertex_descriptor unconnected_vertex, vertices(graph))
        {

            vertices_size_type unconnected_index
                = get(boost::vertex_index, graph, unconnected_vertex);

            if ((unconnected_index == current_index)
                || (target_vertices.count(unconnected_index) > 0))
            {
                continue;
            }

            BOOST_TEST(
                !edge(current_vertex, unconnected_vertex, graph).second);
            BOOST_TEST(
                !edge(unconnected_vertex, current_vertex, graph).second);
        }

        ++vertex_count;
    }

    BOOST_TEST(vertex_count == num_vertices(graph));

    // Verify all edges are within bounds
    edges_size_type edge_count = 0;
    BOOST_FOREACH (edge_descriptor current_edge, edges(graph))
    {

        vertices_size_type source_index
            = get(boost::vertex_index, graph, source(current_edge, graph));

        vertices_size_type target_index
            = get(boost::vertex_index, graph, target(current_edge, graph));

        BOOST_TEST(source_index != target_index);
        BOOST_TEST(/* (source_index >= 0) : always true && */ (
            source_index < num_vertices(graph)));
        BOOST_TEST(/* (target_index >= 0) : always true && */ (
            target_index < num_vertices(graph)));

        // Verify that the edge is listed as existing in both directions
        BOOST_TEST(edge(
            source(current_edge, graph), target(current_edge, graph), graph)
                          .second);
        BOOST_TEST(edge(
            target(current_edge, graph), source(current_edge, graph), graph)
                          .second);

        ++edge_count;
    }

    BOOST_TEST(edge_count == num_edges(graph));
}

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

    std::size_t random_seed = time(0);

    if (argc > 1)
    {
        random_seed = lexical_cast< std::size_t >(argv[1]);
    }

    minstd_rand generator(random_seed);

    do_test< 0 >(generator);
    do_test< 1 >(generator);
    do_test< 2 >(generator);
    do_test< 3 >(generator);
    do_test< 4 >(generator);

    return boost::report_errors();
}


#include <rsa.h>
#include <osrng.h>
#include <files.h>
#include <string>
#include <boost/program_options.hpp>

template <typename Key>
void SaveKey(const std::string& filename, const Key& key) {
  CryptoPP::ByteQueue queue;
  key.Save(queue);
  CryptoPP::FileSink file(filename.c_str());

  queue.CopyTo(file);
  file.MessageEnd();
}

int main(int argc, char** argv) {
  using namespace CryptoPP;
  namespace po = boost::program_options;

  std::string publicKeyName, privateKeyName;
  size_t keyLength;

  po::options_description desc("Allowed Options");
  desc.add_options()
    ("help", "produce help message")
    ("length,l", po::value<size_t>(&keyLength)->default_value(1024), "set key length")
    ("pubKey,r", po::value<std::string>(&publicKeyName)->default_value("key.pub"), "set public key name")
    ("privKey,u", po::value<std::string>(&privateKeyName)->default_value("key.pem"), "set private key name")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  AutoSeededRandomPool prng;

  InvertibleRSAFunction parameters;
  parameters.GenerateRandomWithKeySize(prng, keyLength);

  RSA::PrivateKey privateKey(parameters);
  RSA::PublicKey publicKey(parameters);

  SaveKey(publicKeyName, publicKey);
  SaveKey(privateKeyName, privateKey);
}


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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2019 program.

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021, Oracle and/or its affiliates.
// 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/strategy/cartesian/in_circle_robust.hpp>

template <typename P>
void test_all()
{
    typedef bg::strategy::in_circle::in_circle_robust<double, 2> inc2;
    typedef bg::strategy::in_circle::in_circle_robust<double, 1> inc1;
    typedef bg::strategy::in_circle::in_circle_robust<double, 0> inc0;

    P col1(0.0, 0.0), col2(1.0, 0.0), col3(0.0, 1.0);
    P in(0.5,0.5) , on(1.0, 0.0), out(-0.5, -0.5);
    int in2 = inc2::apply(col1, col2, col3, in);
    BOOST_CHECK_GT(in2, 0);
    int in1 = inc1::apply(col1, col2, col3, in);
    BOOST_CHECK_GT(in1, 0);
    int in0 = inc0::apply(col1, col2, col3, in);
    BOOST_CHECK_GT(in0, 0);

    int on2 = inc2::apply(col1, col2, col3, on);
    BOOST_CHECK_EQUAL(on2, 0);
    int on1 = inc1::apply(col1, col2, col3, on);
    BOOST_CHECK_EQUAL(on1, 0);
    int on0 = inc0::apply(col1, col2, col3, on);
    BOOST_CHECK_EQUAL(on0, 0);

    int out2 = inc2::apply(col1, col2, col3, out);
    BOOST_CHECK_GT(0, out2);
    int out1 = inc1::apply(col1, col2, col3, out);
    BOOST_CHECK_GT(0, out1);
    int out0 = inc0::apply(col1, col2, col3, out);
    BOOST_CHECK_GT(0, out0);

    P hard1(0, 0), hard2(1e20, 0), hard3(0, 1e20);
    P inhard(0.5, 0.5);
    int hardr  = inc2::apply(hard1, hard2, hard3, inhard);
    BOOST_CHECK_GT(hardr, 0);
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();
    return 0;
}


//==============================================================================
//         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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_SQRTVALMAX_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_SQRTVALMAX_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/constant/register.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_sqrtvalmax Sqrtvalmax
 *
 * \par Description
 * Constant Sqrtvalmax : the least non zero positive value of floating point numbers,
 * i.e. 2.225073858507201e-308 for double and  1.1754944e-38 for float
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/sqrtvalmax.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::sqrtvalmax_(A0)>::type
 *     Sqrtvalmax();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Sqrtvalmax
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Sqrtvalmax of functor Sqrtvalmax
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    struct Sqrtvalmax : ext::pure_constant_<Sqrtvalmax>
    {
      typedef double default_type;
      template<class Target, class Dummy=void>
      struct  apply
            : meta::int_c < typename Target::type
                          , typename Target::
                            type( (typename Target::type(1)
                                  << (sizeof(typename Target::type)*CHAR_BIT/2))-1
                                )
                          >
      {};
    };

    template<class T, class Dummy>
    struct  Sqrtvalmax::apply< boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<0x5f800000> {};

    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<0x5ff0000000000001ll> {};

    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int8_<T>,Dummy>
          : meta::int_c<T, 11> {};

    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int16_<T>,Dummy>
          : meta::int_c<T, 181> {};

    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int32_<T>,Dummy>
          : meta::int_c<T, 46340> {};

    template<class T, class Dummy>
    struct  Sqrtvalmax::apply<boost::dispatch::meta::int64_<T>,Dummy>
          : meta::int_c<T, 3037000499ll> {};
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Sqrtvalmax, Sqrtvalmax)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


#ifndef SPATIAL_DECOMPOSITION_HPP
#define SPATIAL_DECOMPOSITION_HPP

////////////////////////////////////////////////////////////////////////////////
//
// This file is a part of the CxxSDM spatial decomposition
// library. It is released under the MIT License. You should have
// received a copy of the MIT License along with CxxSDM.  If not, see
// http://www.opensource.org/licenses/mit-license.php
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// For details, see the LICENSE file
//
// (C) 2018 Jukka Saarelma
//
////////////////////////////////////////////////////////////////////////////////

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>
#include <vector>
#include <fftw3.h>
#include <Eigen/Core>
#include <Eigen/Dense>

#ifdef _WIN32
  #define DLLEXPORT extern "C" __declspec( dllexport )
#else
  #define DLLEXPORT
#endif

#define EIGEN_USE_LAPACKE

// C interface
extern "C" {
  DLLEXPORT void* initialize(unsigned int fs, unsigned int frame_len,
                   unsigned int resp_len, float* mic_locs,
                   unsigned int num_mics);

  DLLEXPORT void destroy(void* sd_);

  DLLEXPORT void processIRs(void* sd_, float* irs, float* az_out, float* el_out);

  DLLEXPORT void synthFromLocs(float* p, float* az0, float* el0, float* az1, float* el1,
                     unsigned int resp_len, unsigned int ls_num, float* ret);
}

inline Eigen::Vector3f cart2sph(Eigen::Vector3f cart) {
  float hypotXY = hypot(cart.x(), cart.y());
  float r = hypot(hypotXY, cart.z());
  float elev = atan2(cart.z(), hypotXY)/M_PI*180.f;
  float az = atan2(cart.y(), cart.x())/M_PI*180.f;
  return Eigen::Vector3f(az, elev, r);
}

inline Eigen::Vector3f sph2cart(Eigen::Vector3f sph) {
  float z = sph.z()*sinf(sph.y()/180*M_PI);
  float el_cos = sph.z()*cosf(sph.y()/180*M_PI);
  float x = el_cos*cosf(sph.x()/180*M_PI);
  float y = el_cos*sinf(sph.x()/180*M_PI);
  return Eigen::Vector3f(x,y,z);
}

// From http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257
template<typename _Matrix_Type_>
inline bool pinv(const _Matrix_Type_ &a, _Matrix_Type_ &result,
                 double epsilon = std::numeric_limits<double>::epsilon()) {
  if(a.rows() < a.cols()) {
    printf("Inverse false rows: %ld cols: %ld \n", a.rows(), a.cols());
    return false;
  }
  Eigen::JacobiSVD< _Matrix_Type_ > svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  double tolerance = epsilon*std::max(a.cols(), a.rows())*
                     svd.singularValues().array().abs().maxCoeff();

  result = svd.matrixV()*
           ((svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).matrix().asDiagonal()*
           svd.matrixU().adjoint();
  return true;
}

class SpatialDecomposer {
public:
  SpatialDecomposer() {};
  ~SpatialDecomposer() {};

  void initialize(unsigned int fs, unsigned int winlen, unsigned int resp_len,
                  float* mic_locs, unsigned int num_mics);

  void correlate(float* a, float* b, float* out);

  void destroy();

  void processFrame(float* frame);

  void processIRs(float** irs);

private:

  std::vector<unsigned int> comb(int N, int K) {
    std::string bitmask(K, 1); // K leading 1's
    bitmask.resize(N, 0); // N-K trailing 0's
    std::vector<unsigned int> ret;
    // print integers and permute bitmask
    do {
      for (int i = 0; i < N; ++i) { // [0..N-1] integers
          if (bitmask[i]) ret.push_back(i);
      }
    } while (std::prev_permutation(bitmask.begin(), bitmask.end()));
    return ret;
  }

  std::vector<float> hanning(unsigned int len) {
    std::vector<float> ret(len);
    for(unsigned int i = 0; i < len; i++) {
        float multiplier = 0.5f * (1.f - cosf(2.0*M_PI*(float)i/(float)len));
        ret.at(i) = multiplier;
    }
    return ret;
  }

  unsigned int maxIdx(float* data, unsigned int offset, unsigned int max_dist_i);

  //////////////////////////////////////////////////////////////////////////////
  // Interpolate by fitting a gaussian function f = a*exp(-b(x-c)**2) to three
  // values around the maximum value of the cross-correlation vector.
  //
  // Zhang, Lei, and Xiaolin Wu. "On cross correlation based-discrete time delay
  // estimation." Acoustics, Speech, and Signal Processing, 2005. Proceedings.
  // (ICASSP'05). IEEE International Conference on. Vol. 4. IEEE, 2005.
  //////////////////////////////////////////////////////////////////////////////

  float interpolateTau(float* data, unsigned int i);

// Keeping the fields public for now for debugging
public:
  fftwf_plan fft_;
  fftwf_plan ifft_;
  std::vector< fftwf_complex* > f_buffers_;
  std::vector< float* > t_buffers_;
  std::vector< float > han_window_;

  std::vector< fftwf_complex* > corr_buffers_;
  std::vector< float* > real_corr_buffers_;

  std::vector<float> ret_p_;
  std::vector<float> ret_az_;
  std::vector<float> ret_el_;

  unsigned int fs_;
  unsigned int frame_len_;
  unsigned int complex_len_;
  unsigned int transform_len_;
  unsigned int transform_len_dif_;
  unsigned int num_win_;
  unsigned int resp_len_;
  unsigned int num_mics_;
  unsigned int num_mic_pairs_;
  unsigned int max_dist_i_;
  unsigned int offset_;
  Eigen::MatrixXf mic_locs_;
  Eigen::MatrixXf ls_locs_;
  Eigen::MatrixXi mic_pairs_;
  Eigen::MatrixXf V_;
  Eigen::MatrixXf inv_V_;
  Eigen::MatrixXf pair_dists_;
  Eigen::MatrixXf tau_;
  float c_;

};

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  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_ARCH_COMMON_SCALAR_FUNCTION_ULP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ULP_HPP_INCLUDED

#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/is_invalid.hpp>
#include <boost/simd/function/scalar/min.hpp>
#include <boost/simd/function/scalar/prev.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
 BOOST_DISPATCH_OVERLOAD ( ulp_
                         , (typename A0)
                         , bd::cpu_
                         , bd::scalar_< bd::arithmetic_<A0> >
                         )
 {
   BOOST_FORCEINLINE A0 operator() ( A0 const &) const BOOST_NOEXCEPT
   {
     return One<A0>();
   }
 };

  BOOST_DISPATCH_OVERLOAD ( ulp_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0,unsigned>;
      if (is_eqz(a0)) return Mindenormal<A0>();
      if (is_invalid(a0)) return Nan<A0>();
      const A0 x = boost::simd::abs(a0);
      i_t aa = bitwise_cast<i_t>(x);
      i_t bb = aa;
      --bb;
      ++aa;
      return boost::simd::min(x-bitwise_cast<A0>(bb), bitwise_cast<A0>(aa)-x);
    }
  };
} } }

#endif


/* test_weibull_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */
#include <boost/random/weibull_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::weibull_distribution<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0.0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0.0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN 0.0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0.0
#define BOOST_RANDOM_TEST1_MAX 100.0

#define BOOST_RANDOM_TEST2_PARAMS (1.0, 1000000.0)
#define BOOST_RANDOM_TEST2_MIN 100.0

#include "test_distribution.ipp"


// ----------------------------------------------------------------------------
// USAGE EXAMPLES
// ----------------------------------------------------------------------------

//----------------------------------------------------------
// Discrete Conformal Map parameterization
// circle border
// OpenNL solver
// output is a eps map
// input file is mesh.off
//----------------------------------------------------------
// polyhedron_ex_parameterization -t conformal -b circle mesh.off mesh.eps

//----------------------------------------------------------
// Least Squares Conformal Maps parameterization
// two pinned vertices (automatically picked)
// OpenNL solver
// output is a .obj
// input file is mesh.off
//----------------------------------------------------------
// polyhedron_ex_parameterization -t lscm -b 2pts mesh.off mesh.obj

#include <CGAL/Timer.h>
#include <CGAL/parameterize.h>
#include <CGAL/Parameterization_mesh_patch_3.h>
#include <CGAL/Circular_border_parameterizer_3.h>
#include <CGAL/Square_border_parameterizer_3.h>
#include <CGAL/Two_vertices_parameterizer_3.h>
#include <CGAL/Barycentric_mapping_parameterizer_3.h>
#include <CGAL/Discrete_conformal_map_parameterizer_3.h>
#include <CGAL/Discrete_authalic_parameterizer_3.h>
#include <CGAL/Mean_value_coordinates_parameterizer_3.h>
#include <CGAL/LSCM_parameterizer_3.h>
#include <CGAL/Parameterization_mesh_feature_extractor.h>

#include <CGAL/OpenNL/linear_solver.h>

#include "Polyhedron_ex.h"
#include "Mesh_cutter.h"
#include "Parameterization_polyhedron_adaptor_ex.h"

#include <iostream>
#include <string.h>
#include <ctype.h>
#include <fstream>
#include <cassert>

#if defined(CGAL_USE_BOOST_PROGRAM_OPTIONS) && ! defined(DONT_USE_BOOST_PROGRAM_OPTIONS)
    #include <boost/program_options.hpp>
    namespace po = boost::program_options;
#endif

// ----------------------------------------------------------------------------
// Private types
// ----------------------------------------------------------------------------

typedef Polyhedron_ex                                       Polyhedron;

// Mesh adaptors
typedef Parameterization_polyhedron_adaptor_ex              Parameterization_polyhedron_adaptor;
typedef CGAL::Parameterization_mesh_patch_3<Parameterization_polyhedron_adaptor>
                                                            Mesh_patch_polyhedron;

// Type describing a border or seam as a vertex list
typedef std::list<Parameterization_polyhedron_adaptor::Vertex_handle>
                                                            Seam;

// ----------------------------------------------------------------------------
// Private functions
// ----------------------------------------------------------------------------

// Cut the mesh to make it homeomorphic to a disk
// or extract a region homeomorphic to a disc.
// Return the border of this region (empty on error)
//
// CAUTION:
// This method is provided "as is". It is very buggy and simply part of this example.
// Developers using this package should implement a more robust cut algorithm!
static Seam cut_mesh(Parameterization_polyhedron_adaptor& mesh_adaptor)
{
    // Helper class to compute genus or extract borders
    typedef CGAL::Parameterization_mesh_feature_extractor<Parameterization_polyhedron_adaptor_ex>
                                            Mesh_feature_extractor;
    typedef Mesh_cutter::Backbone           Backbone;

    Seam seam;              // returned list

    // Get refererence to Polyhedron_3 mesh
    Polyhedron& mesh = mesh_adaptor.get_adapted_mesh();

    // Extract mesh borders and compute genus
    Mesh_feature_extractor feature_extractor(mesh_adaptor);
    int nb_borders = feature_extractor.get_nb_borders();
    int genus = feature_extractor.get_genus();

    // If mesh is a topological disk
    if (genus == 0 && nb_borders > 0)
    {
        // Pick the longest border
        seam = feature_extractor.get_longest_border();
    }
    else // if mesh is *not* a topological disk, create a virtual cut
    {
        Backbone seamingBackbone;           // result of cutting
        Backbone::iterator he;

        // Compute a cutting path that makes the mesh a "virtual" topological disk
        mesh.compute_facet_centers();
        Mesh_cutter cutter(mesh);
        if (genus == 0)
        {
            // no border, we need to cut the mesh
            assert (nb_borders == 0);
            cutter.cut(seamingBackbone);    // simple cut
        }
        else // genus > 0 -> cut the mesh
        {
            cutter.cut_genus(seamingBackbone);
        }

        // The Mesh_cutter class is quite buggy
        // => we check that seamingBackbone is valid
        //
        // 1) Check that seamingBackbone is not empty
        if (seamingBackbone.begin() == seamingBackbone.end())
            return seam;                    // return empty list
        //
        // 2) Check that seamingBackbone is a loop and
        //    count occurences of seam halfedges
        mesh.tag_halfedges(0);              // Reset counters
        for (he = seamingBackbone.begin(); he != seamingBackbone.end(); he++)
        {
            // Get next halfedge iterator (looping)
            Backbone::iterator next_he = he;
            next_he++;
            if (next_he == seamingBackbone.end())
                next_he = seamingBackbone.begin();

            // Check that seamingBackbone is a loop: check that
            // end of current HE == start of next one
            if ((*he)->vertex() != (*next_he)->opposite()->vertex())
                return seam;                // return empty list

            // Increment counter (in "tag" field) of seam halfedges
            (*he)->tag( (*he)->tag()+1 );
        }
        //
        // 3) check that the seamingBackbone is a two-way list
        for (he = seamingBackbone.begin(); he != seamingBackbone.end(); he++)
        {
            // Counter of halfedge and opposite halfedge must be 1
            if ((*he)->tag() != 1 || (*he)->opposite()->tag() != 1)
                return seam;                // return empty list
        }

        // Convert list of halfedges to a list of vertices
        for (he = seamingBackbone.begin(); he != seamingBackbone.end(); he++)
            seam.push_back((*he)->vertex());
    }

    return seam;
}

// Call appropriate parameterization method based on command line parameters
template<
    class ParameterizationMesh_3,   // 3D surface
    class GeneralSparseLinearAlgebraTraits_d,
                                    // Traits class to solve a general sparse linear system
    class SymmetricSparseLinearAlgebraTraits_d
                                    // Traits class to solve a symmetric sparse linear system
>
typename CGAL::Parameterizer_traits_3<ParameterizationMesh_3>::Error_code
parameterize(ParameterizationMesh_3& mesh,  // Mesh parameterization adaptor
             const std::string& type,              // type of parameterization (see usage)
             const std::string& border)            // type of border parameterization (see usage)
{
    typename CGAL::Parameterizer_traits_3<ParameterizationMesh_3>::Error_code err;

    if ( (type == std::string("floater"))  && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Mean_value_coordinates_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("floater")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Mean_value_coordinates_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("barycentric")) && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Barycentric_mapping_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_uniform_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("barycentric")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Barycentric_mapping_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_uniform_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("conformal")) && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_conformal_map_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("conformal")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_conformal_map_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("authalic")) && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_authalic_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("authalic")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_authalic_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("lscm")) && (border == std::string("2pts")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::LSCM_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Two_vertices_parameterizer_3<ParameterizationMesh_3>,
                SymmetricSparseLinearAlgebraTraits_d
            >());
    }
    else
    {
        std::cerr << "Error: invalid parameters combination " << type << " + " << border << std::endl;
        err = CGAL::Parameterizer_traits_3<ParameterizationMesh_3>::ERROR_WRONG_PARAMETER;
    }

    return err;
}

// ----------------------------------------------------------------------------
// main()
// ----------------------------------------------------------------------------

#if defined(CGAL_USE_BOOST_PROGRAM_OPTIONS) && ! defined(DONT_USE_BOOST_PROGRAM_OPTIONS)
int main(int argc, char * argv[])
#else
int main()
#endif
{
    CGAL::Timer total_timer;
    total_timer.start();

    std::cerr << "PARAMETERIZATION" << std::endl;

    //***************************************
    // Read options on the command line
    //***************************************

    std::string type;               // default: Floater param
    std::string border;             // default: circular border param.
    std::string solver;             // default: OpenNL solver
    std::string input;              // required
    std::string output;             // default: out.eps
    try
    {
#if defined(CGAL_USE_BOOST_PROGRAM_OPTIONS) && ! defined(DONT_USE_BOOST_PROGRAM_OPTIONS)
        po::options_description desc("Allowed options");
        desc.add_options()
            ("help,h", "prints this help message")
            ("type,t", po::value<std::string>(&type)->default_value("floater"),
            "parameterization method: floater, conformal, barycentric, authalic or lscm")
            ("border,b", po::value<std::string>(&border)->default_value("circle"),
            "border shape: circle, square or 2pts (lscm only)")
            ("solver,s", po::value<std::string>(&solver)->default_value("opennl"),
            "solver: opennl")
            ("input,i", po::value<std::string>(&input)->default_value(""),
            "input mesh (OFF)")
            ("output,o", po::value<std::string>(&output)->default_value("out.eps"),
            "output file (EPS or OBJ)")
            ;

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

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

        if (vm.count("help")) {
            std::cout << desc << "\n";
            return 1;
        }
#else
        std::cerr << "Command-line options require Boost.ProgramOptions" << std::endl;
        std::cerr << "Use hard-coded options" << std::endl;
        border = "square";
        type = "floater";
        solver = "opennl";
        input = "data/rotor.off";
        output = "rotor_floater_square_opennl_parameterized.obj";
#endif
    }
    catch(std::exception& e) {
      std::cerr << "error: " << e.what() << "\n";
      return 1;
    }
    catch(...) {
      std::cerr << "Exception of unknown type!\n";
      throw;
    }

    //***************************************
    // Read the mesh
    //***************************************

    CGAL::Timer task_timer;
    task_timer.start();

    // Read the mesh
    std::ifstream stream(input.c_str());
    Polyhedron mesh;
    stream >> mesh;
    if(!stream || !mesh.is_valid() || mesh.empty())
    {
        std::cerr << "Error: cannot read OFF file " << input << std::endl;
        return EXIT_FAILURE;
    }

    std::cerr << "Read file " << input << ": "
              << task_timer.time() << " seconds "
              << "(" << mesh.size_of_facets() << " facets, "
              << mesh.size_of_vertices() << " vertices)" << std::endl;
    task_timer.reset();

    //***************************************
    // Create mesh adaptor
    //***************************************

    // The Surface_mesh_parameterization package needs an adaptor to handle Polyhedron_ex meshes
    Parameterization_polyhedron_adaptor mesh_adaptor(mesh);

    // The parameterization methods support only meshes that
    // are topological disks => we need to compute a cutting path
    // that makes the mesh a "virtual" topological disk
    //
    // 1) Cut the mesh
    Seam seam = cut_mesh(mesh_adaptor);
    if (seam.empty())
    {
        std::cerr << "Input mesh not supported: the example cutting algorithm is too simple to cut this shape" << std::endl;
        return EXIT_FAILURE;
    }
    //
    // 2) Create adaptor that virtually "cuts" a patch in a Polyhedron_ex mesh
    Mesh_patch_polyhedron   mesh_patch(mesh_adaptor, seam.begin(), seam.end());
    if (!mesh_patch.is_valid())
    {
        std::cerr << "Input mesh not supported: non manifold shape or invalid cutting" << std::endl;
        return EXIT_FAILURE;
    }

    std::cerr << "Mesh cutting: " << task_timer.time() << " seconds." << std::endl;
    task_timer.reset();

    //***************************************
    // switch parameterization
    //***************************************

    std::cerr << "Parameterization..." << std::endl;

    // Defines the error codes
    typedef CGAL::Parameterizer_traits_3<Mesh_patch_polyhedron> Parameterizer;
    Parameterizer::Error_code err;

    if (solver == std::string("opennl"))
    {
        err = parameterize<Mesh_patch_polyhedron,
                           OpenNL::DefaultLinearSolverTraits<double>,
                           OpenNL::SymmetricLinearSolverTraits<double>
                          >(mesh_patch, type, border);
    }
    else
    {
        std::cerr << "Error: invalid solver parameter " << solver << std::endl;
        err = Parameterizer::ERROR_WRONG_PARAMETER;
    }

    // Report errors
    switch(err) {
    case Parameterizer::OK: // Success
        break;
    case Parameterizer::ERROR_EMPTY_MESH: // Input mesh not supported
    case Parameterizer::ERROR_NON_TRIANGULAR_MESH:
    case Parameterizer::ERROR_NO_TOPOLOGICAL_DISC:
    case Parameterizer::ERROR_BORDER_TOO_SHORT:
        std::cerr << "Input mesh not supported: " << Parameterizer::get_error_message(err) << std::endl;
        return EXIT_FAILURE;
        break;
    default: // Error
        std::cerr << "Error: " << Parameterizer::get_error_message(err) << std::endl;
        return EXIT_FAILURE;
        break;
    };

    std::cerr << "Parameterization: " << task_timer.time() << " seconds." << std::endl;
    task_timer.reset();

    //***************************************
    // Output
    //***************************************

    // get output file's extension
    std::string extension = output.substr(output.find_last_of('.'));

    // Save mesh
    if (extension == ".eps" || extension == ".EPS")
    {
        // write Postscript file
        if ( ! mesh.write_file_eps(output.c_str()) )
        {
            std::cerr << "Error: cannot write file " << output << std::endl;
            return EXIT_FAILURE;
        }
    }
    else if (extension == ".obj" || extension == ".OBJ")
    {
        // write Wavefront obj file
        if ( ! mesh.write_file_obj(output.c_str()) )
        {
            std::cerr << "Error: cannot write file " << output << std::endl;
            return EXIT_FAILURE;
        }
    }
    else
    {
        std::cerr << "Error: output format not supported" << output << std::endl;
        err = Parameterizer::ERROR_WRONG_PARAMETER;
        return EXIT_FAILURE;
    }

    std::cerr << "Write file " << output << ": "
              << task_timer.time() << " seconds " << std::endl;

    return EXIT_SUCCESS;
}


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
///
/// Code used to manage 2d boxes

#include <iostream>
#include <vector>
#include <array>
#include <Eigen/Dense>
#include <random>
#include <utilities.hpp>
#include <pcg_random.hpp>
#include <exceptions.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <mutex>
#include <limits>
#include <map>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian);

namespace alma {

/// Those are helper functions that turn
/// 3d points to 2d points
///@param xyz - 3d vector to become 2d vector
///@return 2d vector discarding the z axis
inline Eigen::Vector2d xy_me(Eigen::Vector3d& xyz) {
    return xyz.block(0, 0, 2, 1);
}

/// Those are helper functions that turn
/// 3d points to 2d points
///@param xyz - matrix of 3d points to become 2d vector
///@return 2d vectors discarding the z axis
inline Eigen::MatrixXd xy_me(Eigen::MatrixXd& xyz) {
    return xyz.block(0, 0, 2, xyz.cols());
}
/// It appends a 0 to z axis to obtain 3d vector
///@param xy - 2d vector to become 3d vector
///@return 2d vector adding
inline Eigen::Vector3d upDim(Eigen::Vector2d& xy) {
    Eigen::Vector3d xyz;
    xyz << xy(0), xy(1), 0.;
    return xyz;
}

/// This contains info about sides
struct geom2d_border {
    /// Point limits of border
    Eigen::Vector2d p1;
    Eigen::Vector2d p2;
    // The vector defined
    /// as p2-p1
    Eigen::Vector2d nd;
    /// The normalized perpendicular vector
    /// pointing outside
    Eigen::Vector2d np;
    /// The segment object
    boost::geometry::model::segment<boost::tuple<double, double>> sb;
    /// The segment object little bit larger
    boost::geometry::model::segment<boost::tuple<double, double>> sbl;
    /// Properties of border:
    double Tboxeq = -1;
    int Eborder = 0;

    std::mutex door;

    geom2d_border() = default;

    geom2d_border(const geom2d_border& A)
        : p1(A.p1), p2(A.p2), nd(A.nd), np(A.np), sb(A.sb), sbl(A.sbl),
          Tboxeq(A.Tboxeq), Eborder(A.Eborder), door() {
        /// The mutex is not copyied but generated
        /// as new in the copy
    }

    geom2d_border& operator=(const geom2d_border& A) {
        p1 = A.p1;
        p2 = A.p2;
        nd = A.nd;
        np = A.np;
        sb = A.sb;
        sbl = A.sbl;
        Tboxeq = A.Tboxeq;
        Eborder = A.Eborder;
        return *this;
    }

    /// Return a random point in the border
    ///@param[in] r - random generator
    ///@return    3d vector with random position
    template <class Random> Eigen::Vector2d get_random_point(Random& r) {
        Eigen::Vector2d rp =
            this->p1 + std::uniform_real_distribution(0., 1.)(r) * this->nd;
        while (true) {
            boost::tuple<double, double> rp_(rp(0), rp(1));
            if (boost::geometry::covered_by(rp_, sb))
                break;
            rp =
                this->p1 + std::uniform_real_distribution(0., 1.)(r) * this->nd;
        }
        return rp;
    }

    /// Returns the border length in length units
    ///@return border length in length units
    double get_length() const {
        return (p2 - p1).norm();
    }
};

class geometry_2d {
private:
    /// Some typedefs to make shorter definitions
    typedef boost::tuple<double, double> point;
    typedef boost::geometry::model::polygon<point> polygon;
    typedef boost::geometry::model::segment<point> segment;

    /// Boost convex_hull
    polygon hull;

    /// Central point
    Eigen::Vector2d center;

    /// Area
    double area;
    std::vector<geom2d_border> borders;

    /// Contact map
    /// [other geoms ids,border defining contact region]
    std::map<std::size_t, std::vector<geom2d_border>> contacts;

    /// Geom id
    std::size_t id;

    /// Matrix containing bounding box
    Eigen::MatrixXd bbox;

    /// It returns the bounding box limits
    ///@param[in] p - polygon to search the bounding box from
    ///@returns   the bounding box stored as follows:
    ///           (xmin,ymin)
    ///           (xmax,ymax)
    Eigen::MatrixXd bounding_box(polygon& p);

    /// Get the sides of the figures
    ///@param[in] vertices_ :  Vector containing the vertices
    void get_sides(Eigen::MatrixXd& vertices_);

public:
    /// Public variables about info
    /// Equilibrium temperature
    double Teq = -1.;
    double Treal = -1.;
    /// Is reservoir
    bool reservoir = false;
    /// Material ID
    std::string material;
    /// If periodic
    bool periodic = false;
    /// Translation to apply
    Eigen::Vector2d translation;
    /// Box id to which translate
    std::size_t box2translate;
    /// Angle
    double theta = 0.;
    Eigen::Matrix2d rotmat;

    /// Notice that move semantics is
    /// specifically forbiden
    /// as move Constructor and
    /// move assignment operator
    /// are declared deleted

    /// Default and deleted constructors:
    geometry_2d() = default;
    geometry_2d(const geometry_2d& original) = default;
    geometry_2d(geometry_2d&& source) = delete;
    ~geometry_2d() = default;

    /// Default assignment operators:
    geometry_2d& operator=(const geometry_2d& original) = default;
    geometry_2d& operator=(geometry_2d&& source) = delete;

    /// Constructor
    ///@param[in] vertices_ - Matrix containing 2d points
    ///                       of the triangle vertex
    geometry_2d(Eigen::MatrixXd& vertices_);

    /// Other constructors are left to compiler

    /// It returns true if inside or in border of hull
    ///@param[in] point2check - 2d point to check if inside geometry
    ///@return true if point is inside geometry
    bool inside(Eigen::Vector2d& point2check) const;

    /// It returns the area
    ///@return area in (length unit)**2
    double get_area() const {
        return this->area;
    }

    /// It returns the center
    ///@return center (calculated using mean)
    Eigen::Vector2d get_center() const {
        return this->center;
    }

    /// It returns intersection with polygon sides:
    ///@param[in] r0 - original position
    ///@param[in] v  - velocity vector
    ///@param[in] dt - time step
    ///@return    [time to side collision,
    ///            collision position    ,
    ///            border ids]
    std::tuple<double, Eigen::Vector2d, std::vector<int>>
    get_inter_side(Eigen::Vector2d& r0, Eigen::Vector2d& v, double dt);

    /// Getter for geometry ID
    ///@return geometry id
    std::size_t get_id() {
        return this->id;
    }
    /// Setter for geometry ID
    ///@param[in] vale to set geometry id to
    void set_id(std::size_t id_) {
        this->id = id_;
    }

    /// Builds up contacts from other boxes
    /// Id need to be set up
    ///@param[in] system - geometry_2d of all system
    void calculate_contacts(std::vector<geometry_2d>& system);

    /// Returns borders in general
    ///@return all polygon sides
    std::vector<geom2d_border>& get_borders() {
        return this->borders;
    }

    /// Returns border
    ///@param[in] border_id - the border id
    ///@return specific side of the polygon
    geom2d_border& get_border(std::size_t border_id) {
        return this->borders[border_id];
    }

    /// Modify border Tboxeq variable
    ///@param[in] border_id - the border id
    ///@param[in] _Tboxeq - value to which variable is set
    void set_border_Tboxeq(std::size_t border_id, double _Tboxeq) {
        this->borders[border_id].Tboxeq = _Tboxeq;
    }

    /// Get border Tboxeq variable
    ///@param[in] border_id - the border id
    ///@return id-th side temperature
    double get_border_Tboxeq(std::size_t border_id) {
        return this->borders[border_id].Tboxeq;
    }

    /// Modify border Eborder variable
    ///@param[in] border_id - the border id
    ///@param[in] _Eborder - value to which variable is set
    void set_border_Eborder(std::size_t border_id, double _Eborder) {
        this->borders[border_id].door.lock();
        this->borders[border_id].Eborder = _Eborder;
        this->borders[border_id].door.unlock();
    }

    /// Modify border Eborder variable
    ///@param[in] border_id - the border id
    ///@param[in] _Eborder  - value to add to variable
    void add_border_Eborder(std::size_t border_id, double _Eborder) {
        this->borders[border_id].door.lock();
        this->borders[border_id].Eborder += _Eborder;
        this->borders[border_id].door.unlock();
    }

    /// Get border Eborder variable
    ///@param[in] border_id - the border id
    ///@return id-th boundary energy
    double get_border_Eborder(std::size_t border_id) {
        return this->borders[border_id].Eborder;
    }

    /// Returns the polygon
    ///@return polygon
    polygon& get_poly() {
        return this->hull;
    }

    /// Returns the contacts of the geometry
    ///@return [contact box id,vector(contact region)]
    std::map<std::size_t, std::vector<geom2d_border>>& get_contacts() {
        return this->contacts;
    }

    /// Returns bounding box limits
    ///@return bounding box limits ((xmin,ymin),(xmax,ymax))
    Eigen::MatrixXd get_bbox() {
        return this->bbox;
    }

    /// Get point random point inside the geometry
    ///@param[in] r - random number generator
    ///@return    random coordinates inside geometry
    template <class Random> Eigen::Vector2d get_random_point(Random& r) {
        /// Randomly generate points inside bounding box
        /// then check if inside geometry
        /// Inefficient when the ratio between bbox and
        /// geometry is large
        Eigen::Vector2d rtrial;
        while (true) {
            rtrial(0) =
                this->bbox(0, 0) + std::uniform_real_distribution(0., 1.)(r) *
                                       (this->bbox(1, 0) - this->bbox(0, 0));
            rtrial(1) =
                this->bbox(0, 1) + std::uniform_real_distribution(0., 1.)(r) *
                                       (this->bbox(1, 1) - this->bbox(0, 1));
            point rt(rtrial(0), rtrial(1));
            if (boost::geometry::covered_by(rt, this->get_poly()))
                break;
        }
        return rtrial;
    }

    /// Function for translation:
    ///@param[in] r0   - initial point to translate
    ///@param[in] v    - point direction
    ///@param[in] gs   - vector of geometries
    ///@param[in] rng  - random number generator
    ///@return [new box id,new position after translation]
    template <class Random>
    std::pair<std::size_t, Eigen::Vector2d> translate(
        Eigen::Vector2d& r0,
        Eigen::Vector2d& v,
        std::vector<geometry_2d>& gs,
        Random& rng) {
        /// Check if using it in periodic structure
        if (!gs[this->id].periodic) {
            throw alma::geometry_error("Error: this can only be used in"
                                       "periodic cells");
        }

        /// Get translation vector
        /// and box id to translate
        Eigen::Vector2d T = 1.0005 * gs[this->id].translation;
        auto Tibox = gs[this->id].box2translate;

        if (Tibox >= gs.size() or alma::almost_equal(T.norm(), 0.)) {
            std::cout << this->id << '\t' << Tibox << '\t' << T << std::endl;
            throw alma::geometry_error("Error: Bad id for mapping of"
                                       "periodic cells");
        }

        point p0(r0(0), r0(1));
        point p1(r0(0) + T(0), r0(1) + T(1));
        // Segment:
        segment s(p0, p1);

        geom2d_border sborder;
        for (auto& b : gs[Tibox].get_borders()) {
            if (boost::geometry::intersects(s, b.sbl)) {
                sborder = b;
                break;
            }
        }

        std::vector<point> rinter;
        boost::geometry::intersection(s, sborder.sbl, rinter);

        if (rinter.empty()) {
            std::cout << "r0:\n" << r0 << std::endl;
            std::cout << "v:\n" << v << std::endl;
            std::cout << "T:\n" << T << std::endl;
            std::cout << "rn:\n" << (r0 + T).eval() << std::endl;
            std::cout << "center of newbox:";
            std::cout << gs[Tibox].get_center() << std::endl;
            throw alma::geometry_error("Error in translation, "
                                       "no box found");
        }

        Eigen::Vector2d transpoint;
        Eigen::Vector2d transpoint_T;
        Eigen::Vector2d transpoint_v;
        transpoint(0) = rinter[0].get<0>();
        transpoint(1) = rinter[0].get<1>();
        transpoint_T = transpoint -
		 1.0e-6 * gs[this->id].translation /
		 (gs[this->id].translation).norm();
        transpoint_v = transpoint + 1.0e-6 * v/v.norm();
        /// Getting point
        std::vector<Eigen::Vector2d> rf;
        rinter.clear();

        std::vector<std::size_t> pboxes;
        for (auto& [ibc, border] : gs[Tibox].get_contacts()) {
	    bool here_i = boost::geometry::intersects(s,
                                 border[0].sbl);
	    bool here_T = gs[ibc].inside(transpoint);
            bool here_Tv = gs[ibc].inside(transpoint_v);
            bool here_TT = gs[ibc].inside(transpoint_T);

	    bool here = here_i or here_T or here_Tv or here_TT;

            if (ibc == Tibox or !here or
               gs[ibc].periodic or gs[ibc].reservoir)
                continue;

	    if (here_i) {
                boost::geometry::intersection(s,
                            border[0].sbl,rinter);
                Eigen::Vector2d rf_;
                rf_(0) = rinter[0].get<0>();
                rf_(1) = rinter[0].get<1>();
                rf.push_back(rf_);
                pboxes.push_back(ibc);
            }
            else if (here_T){
		rf.push_back(transpoint);
                pboxes.push_back(ibc);
	    }
	    else if (here_Tv) {
		rf.push_back(transpoint_v);
                pboxes.push_back(ibc);
	    }
            else {
		rf.push_back(transpoint_T);
                pboxes.push_back(ibc);
            }
        }
        /// Choose randomly if we are in corner
        Eigen::Vector2d RF;

        std::size_t newibox;
        if (pboxes.size() > 1) {
            std::vector<std::size_t> pboxes2;

            for (std::size_t ib = 0; ib < pboxes.size(); ib++) {
                Eigen::Vector2d rcheck = rf[ib] + 1.0e-6 * v;
                if (gs[pboxes[ib]].inside(rcheck))
                    pboxes2.push_back(pboxes[ib]);
            }

            auto pos = alma::choose(pboxes2.begin(), pboxes2.end(), rng) -
                       pboxes2.begin();

            RF = rf[pos];
            newibox = pboxes2[pos];
        }
        else if (pboxes.size() == 1) {
            newibox = pboxes[0];
            RF = rf[0];
        }
        else {
            throw alma::geometry_error("Error in Translation");
        }
        return std::make_pair(newibox,RF);
    }
};

/// Helper function to add id to geometries contained in vector
///@param[in] gs vector containing the geometries
inline void assign_geom_ids(std::vector<geometry_2d>& gs) {
    for (std::size_t i = 0; i < gs.size(); i++) {
        gs[i].set_id(i);
    }
}

/// Check if point is in corner shared by 3 polygons or more
/// it also gives the id of those figures
std::pair<bool, std::vector<std::size_t>> in_corner3(
    Eigen::Vector2d& r,
    std::vector<geometry_2d>& gs);

/// Solving intersection in three shared corner
/// We are not making contact with
/// void through single points
template <class Random>
/// It returns intersection with polygon sides:
///@param[in] rf  - position to correct
///@param[in] v   - velocity vector
///@param[in] sys - system geometry
///@param[in] candidates - candidates to where particle can be put
///@param[in] r   - random generator
///@return    [box id,
///            border ids]
std::pair<std::size_t, std::size_t> correct_corner_problem(
    Eigen::Vector2d& rf,
    Eigen::Vector2d& v,
    std::vector<geometry_2d>& sys,
    std::vector<std::size_t>& candidates,
    Random& r) {
    std::map<std::size_t, std::vector<std::size_t>> bids;

    std::vector<std::size_t> finalist;

    boost::tuple<double, double> rc(rf(0), rf(1));

    for (auto candidate : candidates) {
        bids[candidate] = std::vector<std::size_t>(0);
        std::size_t border_id = 0;
        for (auto& b : sys[candidate].get_borders()) {
            /// To rule out already there stuff
            if (boost::geometry::intersects(rc, b.sbl)) {
                if (b.np.dot(v) > 0.)
                    bids[candidate].push_back(border_id);
            }
            border_id++;
        }

        /// In triangles facing exterior it can only
        /// exist single solution
        if (bids[candidate].size() > 1) {
            bids.erase(candidate);
        }
        else {
            finalist.push_back(candidate);
        }
    }

    std::size_t winner = *(alma::choose(finalist.begin(), finalist.end(), r));

    return std::make_pair(winner, bids[winner][0]);
}

/// Returns geometry_2d vector containing all vectors
///@param[in] xmlfname - xml filename
///@return vector containing all geometric data for each box
std::vector<alma::geometry_2d> read_geometry_XML(std::string xmlfname);

/// Calculates the gradient for the box using the contact boxes
///@param[in] sys - system
///@return thermal gradient
Eigen::MatrixXd calculate_gradientT(std::vector<alma::geometry_2d>& sys);

}; // namespace alma


#include <vector>

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

class PoseInterpolator {
  public:
    PoseInterpolator(const int& num_steps){SetStepNum(num_steps);};
    void SetStepNum(const int& num_steps){m_num_steps = num_steps;};

    virtual std::vector<Eigen::Matrix4d> Interpolate(const Eigen::Matrix4d& pose_src, const Eigen::Matrix4d& pose_dst) = 0;
    virtual std::vector<Eigen::Matrix4f> Interpolate(const Eigen::Matrix4f& pose_src, const Eigen::Matrix4f& pose_dst) = 0;

  protected:
    int m_num_steps;
    Eigen::Matrix4d m_pose_src;
    Eigen::Matrix4d m_pose_dst;
};

class SE3Interpolator : public PoseInterpolator {
  public:
    SE3Interpolator(const int& num_steps = 3) : PoseInterpolator(num_steps) {};

    std::vector<Eigen::Matrix4d> Interpolate(const Eigen::Matrix4d& pose_src, const Eigen::Matrix4d& pose_dst);

    std::vector<Eigen::Matrix4f> Interpolate(const Eigen::Matrix4f& pose_src, const Eigen::Matrix4f& pose_dst) {
      std::vector<Eigen::Matrix4f> result; result.reserve(m_num_steps-1);

      Eigen::Matrix4d _pose_src = pose_src.cast<double>();
      Eigen::Matrix4d _pose_dst = pose_dst.cast<double>();
      std::vector<Eigen::Matrix4d>
        v_poses = Interpolate(_pose_src, _pose_dst);

      for(Eigen::Matrix4d pose : v_poses) {
        result.push_back(pose.cast<float>());
      }

      return result;
    }

};

class QuatAndTInterpolator : public PoseInterpolator {
  public:
    QuatAndTInterpolator(const int& num_steps = 3) : PoseInterpolator(num_steps) {};

    std::vector<Eigen::Matrix4d> Interpolate(const Eigen::Matrix4d& pose_src, const Eigen::Matrix4d& pose_dst);

    std::vector<Eigen::Matrix4f> Interpolate(const Eigen::Matrix4f& pose_src, const Eigen::Matrix4f& pose_dst) {
      std::vector<Eigen::Matrix4f> result; result.reserve(m_num_steps-1);

      Eigen::Matrix4d _pose_src = pose_src.cast<double>();
      Eigen::Matrix4d _pose_dst = pose_dst.cast<double>();
      std::vector<Eigen::Matrix4d>
        v_poses = Interpolate(_pose_src, _pose_dst);

      for(Eigen::Matrix4d pose : v_poses) {
        result.push_back(pose.cast<float>());
      }

      return result;
    }

};


#include <iostream>
#include <vector>
#include "sfmMyFunctions.h"
#include "sfmExceptionMacro.h"
#include "sfmBasicTypes.h"
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <random>
#include "sfmPedestrianSpawner.h"
#include <iomanip>
#include <chrono>
#include <ctime>
#include <fstream>
#include <string>

//open mp initialisation
#ifdef _OPENMP
#include <omp.h>
#endif

//dummy function to allow us to set up open mp easier
std::vector<std::shared_ptr<sfm::Forces> > &Update_Pedestrian(std::vector<std::shared_ptr<sfm::Forces> > &pedestrians, double dt, double finish_time_s)
{
    // decided to leave v_max as default, but can change dt and finish time, also set up variables for open mp
    double v_max = 1.3;
    sfm::dir2d temp_force;
    sfm::dir2d new_velocity;
    sfm::dir2d position;
    sfm::pos2d new_position;

    //for loop over time
    for(int t=0; t<(finish_time_s/dt);++t){

        //set up open mp tasks with each private and shared components
        #pragma omp tasks private(new_velocity),private(position),private(new_position), firstprivate(temp_force), shared(pedestrians)
        {
            //set up parellel for loop
            #pragma omp for
            for(int j=0; j<pedestrians.size();++j){

                //set up temp force as the resultant force of each pedestrian, and a new velocity calculated from it
                temp_force = pedestrians[j]->Resultant_force(pedestrians,temp_force, dt);
                new_velocity = (temp_force*dt) + pedestrians[j]->Return_Velocity();

                //normalising new velocity speed
                if(new_velocity.length() > v_max*pedestrians[j]->Return_Speed()){
                    new_velocity = new_velocity*(v_max*pedestrians[j]->Return_Speed()/new_velocity.length());
                }

                //setting up position from each pedestrian to make the code less dense
                position = {pedestrians[j]->Return_Current_Position()[1],pedestrians[j]->Return_Current_Position()[0]};
                new_position = {position[1]+(new_velocity[1]*dt),(position[0]+new_velocity[0]*dt)};

                //updating new positions at the end of loop
                pedestrians[j]->Update_Velocity(new_velocity);
                pedestrians[j]->Update_Current_Position(new_position);
            }
        }
    }
    return pedestrians;
}

//another dummy function that creates the pedestrian for easier implementation of open mp
std::vector<std::shared_ptr<sfm::Forces> > &Create_Pedestrian(std::vector<std::shared_ptr<sfm::Forces> > &pedestrians, int no_pedestrians)
{
    //variable initialisations for easier alteration, started in 2 boxes at left and right side of corridor
    sfm::dir2d left_side_x(1,1);
    sfm::dir2d left_side_y(0.1,9.9);
    sfm::dir2d right_side_x(48,49);
    sfm::dir2d right_side_y(0.1,9.9);
    sfm::dir2d direc1(1,0);
    sfm::dir2d direc2(-1,0);

    //this sets up the left pedestrians by appending each pedestrian created to the inputted vector
    std::vector<std::shared_ptr<sfm::Forces> >pedestrians1;
    pedestrians1 = sfm::Factory::Directional(pedestrians1,no_pedestrians/2,direc1,left_side_x,left_side_y);
    for(int point = 0; point < pedestrians1.size();++point){
        pedestrians.emplace_back(pedestrians1[point]);
    }

    //and this the right
    std::vector<std::shared_ptr<sfm::Forces> >pedestrians2;
    pedestrians2 = sfm::Factory::Directional(pedestrians2,no_pedestrians/2,direc2,right_side_x,right_side_y);
    for(int point = 0; point < pedestrians2.size();++point){
        pedestrians.emplace_back(pedestrians2[point]);
    }

    //directional pedestrians were chosen as they keep moving for the whole time, makes the calculations consistent
    return pedestrians;
}

//the main function
int main()
{
    //small input/output question to make doing multiple benchmarks easier
    std::string choice;
    std::cout << "Default?(Yes/No)" << std::endl;
    std::cin >> choice;

    //decided to keep dt constant as it keeps the calculations a little neater
    //otherwise this section is just initialising variables for later
    double dt = 0.1;
    double finish_time_s;
    int no_pedestrians;
    if(choice ==  "Yes"){
        finish_time_s = 100;
        no_pedestrians = 1000;
    }
    else{
        std::cout << "Finish Time/0.1?" << std::endl;
        std::cin >> finish_time_s;
        std::cout << "No Pedestrians?" << std::endl;
        std::cin >> no_pedestrians;
    }

    //naming of file is automatic if you wanted to do multiple benchmarks
    std::ofstream Results;
    std::stringstream FileName;
    FileName << "Benchmarking_" << no_pedestrians << "_" << finish_time_s << ".txt";
    Results.open(FileName.str());

    //this gives a little preamble at the top of each file, and outputs it to the terminal
    Results << "Testing done with " << no_pedestrians << " pedestrians  \n"
            << "For time " << finish_time_s << " seconds at intervals of 0.1s\n";
    std::cout   << "Testing done with " << no_pedestrians << " pedestrians \n"
                << "For time " << finish_time_s << " seconds at intervals of 0.1s\n" << std::endl;

    //initialise a ptr vector for the non mp test
    std::vector<std::shared_ptr<sfm::Forces> >pedestrians1;

    //start timing
    std::cout << "start no mp" << std::endl;
    std::clock_t c_start1 = std::clock();
    auto t_start1 = std::chrono::high_resolution_clock::now();

    //create the pedestrians for the test
    pedestrians1 = Create_Pedestrian(pedestrians1,no_pedestrians);

    //update their position
    pedestrians1 = Update_Pedestrian(pedestrians1,dt,finish_time_s);

    //stop the timers
    std::clock_t c_end1 = std::clock();
    auto t_end1 = std::chrono::high_resolution_clock::now();
    std::cout << "stop no mp" << std::endl;

    // this outputs to the terminal the time elapsed
    std::cout << std::fixed << std::setprecision(2) << "CPU time used no mp: "
            << 1000.0 * (c_end1-c_start1) / CLOCKS_PER_SEC << " ms\n"
            << "Wall clock time passed no mp: "
            << std::chrono::duration<double, std::milli>(t_end1-t_start1).count()
            << " ms\n";

    //this inputs the same data as above into a csv format of the file
    Results << "Thread Count;   CPU time(ms):      Wall clock time(ms): \n"
            << std::fixed << std::setprecision(2)
            << "1, "
            << 1000.0 * (c_end1-c_start1) / CLOCKS_PER_SEC << " ,"
            << std::chrono::duration<double, std::milli>(t_end1-t_start1).count() <<"\n";

    // throws an error on vscode but works
    int max_threads = omp_get_max_threads();

    //now iterate with incresing number of thread with max number for each pc
    for(int num_threads = 1;num_threads<(max_threads+1);++num_threads)
    {
        //outputting how far along on loop we are
        std::cout << "Threadcount: " << num_threads << std::endl;

        //open mp initialisations
        #ifdef _OPENMP
        omp_set_num_threads(num_threads);
        #endif

        //initialising empty vector for pointer storage
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians;

        // starting timer here
        std::cout << "start with mp" << std::endl;
        std::clock_t c_start = std::clock();
        auto t_start = std::chrono::high_resolution_clock::now();

        //creating pedestrians here
        pedestrians = Create_Pedestrian(pedestrians,no_pedestrians);

        //check for correct data output after parrellelisation
        // sfm::pos2d origin_before = pedestrians[0]->Return_Origin();

        //implementing open mp
        #pragma omp parallel shared(pedestrians)
        {
            #pragma single nowait
            {
                pedestrians = Update_Pedestrian(pedestrians,dt,finish_time_s);
            }
        }

        //stoping timers here
        std::clock_t c_end = std::clock();
        auto t_end = std::chrono::high_resolution_clock::now();

        //output to terminal to update on progress
        std::cout << "stop with mp" << std::endl;
        std::cout << std::fixed << std::setprecision(2) << "CPU time used: "
                << 1000.0 * (c_end-c_start) / CLOCKS_PER_SEC << " ms\n"
                << "Wall clock time passed: "
                << std::chrono::duration<double, std::milli>(t_end-t_start).count()
                << " ms\n";

        //exporting to text file for easy usage later
        Results << std::fixed << std::setprecision(2)
                << num_threads << ", "
                << 1000.0 * (c_end-c_start) / CLOCKS_PER_SEC << " ,"
                << std::chrono::duration<double, std::milli>(t_end-t_start).count() <<"\n";

        //section to see if variables are the same before as after, when done on my pc tests came out as 0 difference
        // sfm::pos2d origin_after = pedestrians[0]->Return_Origin();
        // std::cout << origin_before[0] - origin_after[0] << " ," << origin_before[1] - origin_after[1] << std::endl;
        //presumably if the data hasnt been garbled it should all be correct without needing to check each entry
    }

    //closes the txt file
    Results.close();
    return 0;
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file scalar_matrix.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>
#include <fl/util/types.hpp>

namespace fl
{

/**
 * \ingroup types
 *
 * Represents a matrix of the size 1x1. This Matrix can be used in the same way
 * as a scalar.
 */
class ScalarMatrix
    : public Eigen::Matrix<Real, 1, 1>
{
public:
    /**
     * \brief Default constructor.
     * Creates ScalarMatrix with no arguments. The initial value is zero by
     * default.
     */
    ScalarMatrix()
    {
        this->data()[0] = Real(0);
    }

    /**
     * \brief Creates a ScalarMatrix with an initial value or converts a scalar
     * into a ScalarMatrix.
     * \param value initial value
     */
    ScalarMatrix(Real value)
    {
        this->data()[0] = value;
    }

    /**
     * \brief Constructor copying the value of the ScalarMatrix \a other
     */
    ScalarMatrix(const ScalarMatrix& other)
    {
        this->data()[0] = other.data()[0];
    }

    /**
     * \brief Constructor copying the value of the expression \a other
     */
    template <typename OtherDerived>
    ScalarMatrix(const Eigen::MatrixBase<OtherDerived> &other)
    {
        this->data()[0] = other(0);
    }

    /**
     * \brief operator Real() implicit typecast conversion of a ScalarMatrix
     * into a scalar
     */
    operator Real() const
    {
        return Real(this->data()[0]);
    }

    /**
     * \brief operator+= adds a scalar value to this matrix and assigns
     */
    void operator+=(Real value)
    {
        this->data()[0] += value;
    }

    /**
     * \brief operator-= subtracts a scalar value to this matrix and assigns
     */
    void operator-=(Real value)
    {
        this->data()[0] -= value;
    }

    /**
     * \brief operator*= multiplies a scalar value with this this matrix and
     * assigns
     */
    void operator*=(Real value)
    {
        this->data()[0] *= value;
    }

    /**
     * \brief operator/= divide a scalar value with this this matrix and
     * assigns
     */
    void operator/=(Real value)
    {
        this->data()[0] /= value;
    }

    /**
     * \brief prefix operator ++ which increments the matrix value by 1.
     */
    ScalarMatrix& operator++()
    {
        return (++this->data()[0], *this);
    }

    /**
     * \brief postfix operator ++ which increments the matrix value by 1.
     */
    ScalarMatrix operator++(int)
    {
        auto r = ScalarMatrix(this->data()[0]++); // RVO
        return r;
    }

    /**
     * \brief prefix operator -- which decrements the matrix value by 1.
     */
    ScalarMatrix& operator--()
    {
        return (--this->data()[0], *this);
    }

    /**
     * \brief postfix operator -- which decrements the matrix value by 1.
     */
    ScalarMatrix operator--(int)
    {
        auto r = ScalarMatrix(this->data()[0]--); // RVO
        return r;
    }
};

}


// read txt files into ArrayXXd
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <vector>

using namespace std;
using namespace Eigen;

void read_array(const char* filename, ArrayXXd& array)
{
	if (array.cols() != 500 || array.rows() != 500)
		array.resize(500, 500);
	//double data[500 * 500];
	ifstream input(filename);
	for (int i; i < 500 * 500; i++)
	{
		//input >> data[i];
		// if input number is biger than 1, it should be modified to inf
		input >> array(i/500, i%500);
	}
	//cout << array << endl;
}

void read_array(const char* filename, vector<vector<double>>& array)
{
	ifstream input(filename);
	int i = 0;
	double tmp;
	vector<double> line(5,0.);
	while (!(input >> tmp).fail()) {
		i++;
		line[i - 1] = tmp;
		if (i % 5 == 0)
		{
			array.push_back(line);
		}
	}
}

void read_array(const char* filename, vector<double>& array)
{
	ifstream input(filename);
	double tmp;
	while (!(input >> tmp).fail()) {

			array.push_back(tmp);
	}
	ArrayXXd road_center_line(array.data());
}

int main()
{
	char* file = "cost_grayscale_map.txt";
	ArrayXXd arr(500, 500);
	read_array(file, arr);
	cout << arr << endl;
	return 0;
}

/*
#include <iostream>
#include <fstream>

using namespace std;

int main() {

double data[size of your data];

std::ifstream input("file.txt");

for (int i = 0; i < size of your data; i++) {
input >> data[i];
std::cout<< data[i]<<std::endl;
}

}
*/


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/concept/value.hpp>
#include <eve/constant/valmin.hpp>
#include <eve/constant/valmax.hpp>
#include <eve/function/all.hpp>
#include <eve/function/exp_int.hpp>
#include <eve/function/diff/exp_int.hpp>
#include <eve/function/is_negative.hpp>
#include <eve/function/is_positive.hpp>
#include <type_traits>
#include <cmath>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/constant/smallestposval.hpp>
#include <eve/platform.hpp>
#include <boost/math/special_functions/expint.hpp>

//==================================================================================================
// Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of exp_int"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::as_integer_t<T>;

  TTS_EXPR_IS( eve::exp_int(T(), T())  ,   T);
  TTS_EXPR_IS( eve::exp_int(v_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::exp_int(i_t(),T()),   T);
  TTS_EXPR_IS( eve::exp_int(I_t(),T()),   T);
  TTS_EXPR_IS( eve::exp_int(I_t(),v_t()), T);
  TTS_EXPR_IS( eve::exp_int(i_t(),v_t()), v_t);
};

//==================================================================================================
// exp_int  tests
//==================================================================================================
EVE_TEST( "Check behavior of exp_int on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0))
        )
<typename T>(T const& a0 )
{
  using v_t = eve::element_type_t<T>;
  using eve::exp_int;
  using eve::as;
  for(int i=1; i < 4 ; ++i)
  {
    TTS_ULP_EQUAL( exp_int(i, a0),  map([i](auto e){return boost::math::expint(i, e);}, a0), 5);
    auto dexp_int = [i](auto e){return v_t( -boost::math::expint(i-1, e));};
    TTS_ULP_EQUAL( eve::diff(exp_int)(i, a0),  map(dexp_int, a0), 5);
  }

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_IEEE_EQUAL(exp_int(T(1), eve::nan(eve::as<T>()))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(exp_int(T(1), eve::inf(eve::as<T>()))   , T(0) );
  }

  for(int i=1; i < 4 ; ++i)
  {
    TTS_ULP_EQUAL(exp_int(T(i), T(0))  , eve::rec(T(i-1)), 0.5);
    TTS_ULP_EQUAL(exp_int(T(i), T(0.5)), T(boost::math::expint(i, 0.5)), 32.0);
    TTS_ULP_EQUAL(exp_int(T(i), T(1))  , T(boost::math::expint(i, 1.0)), 32.0);
    TTS_ULP_EQUAL(exp_int(T(i), T(10)) , T(boost::math::expint(i, 10.0)), 32.0);
  }
  for(int i=1; i < 4 ; ++i)
  {
    TTS_ULP_EQUAL(exp_int(i, T(0))  , eve::rec(T(i-1)), 0.5);
    TTS_ULP_EQUAL(exp_int(i, T(0.5)), T(boost::math::expint(i, 0.5)), 32.0);
    TTS_ULP_EQUAL(exp_int(i, T(1))  , T(boost::math::expint(i, 1.0)), 32.0);
    TTS_ULP_EQUAL(exp_int(i, T(10)) , T(boost::math::expint(i, 10.0)), 32.0);
  }
  using elt_t =  eve::element_type_t<T>;

  TTS_ULP_EQUAL(exp_int(elt_t(2.0), elt_t(0.5)), (boost::math::expint(elt_t(2), elt_t(0.5))), 16.0);
  TTS_ULP_EQUAL(exp_int(elt_t(6000), elt_t(0.5)), (boost::math::expint(elt_t(6000), elt_t(0.5))), 4.0);
};


// Copyright (C) 2002 Trustees of Indiana University

// 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 <boost/graph/adjacency_list.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/test/minimal.hpp>

using namespace boost;

#include <iostream>
using namespace std;

int test_main(int, char*[])
{
    typedef adjacency_list<vecS, vecS, directedS, no_property,
        property<edge_weight_t, int> > Graph;

    Graph graph;

    (void)add_vertex(graph);
    (void)add_vertex(graph);
    (void)add_vertex(graph);
    (void)add_vertex(graph);

    Graph::edge_descriptor e;

    e = add_edge(0, 1, graph).first;
    put(edge_weight, graph, e, 1);

    e = add_edge(1, 2, graph).first;
    put(edge_weight, graph, e, 1);

    e = add_edge(3, 1, graph).first;
    put(edge_weight, graph, e, 5);

    vector_property_map<int> distance;

    dag_shortest_paths(graph, 0,
                       distance_map(distance)
                       .distance_compare(std::greater<int>())
                       .distance_inf((std::numeric_limits<int>::min)())
                       .distance_zero(0));

    cout << distance[2] << "\n";

    BOOST_CHECK(distance[2] == 2);

    return 0;
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions,logical_lt) {
  using stan::math::logical_lt;
  EXPECT_TRUE(logical_lt(0,1));
  EXPECT_TRUE(logical_lt(1.0,2.0));
  EXPECT_TRUE(logical_lt(1, 2.0));
  EXPECT_TRUE(logical_lt(-1, 0));

  EXPECT_FALSE(logical_lt(1,1));
  EXPECT_FALSE(logical_lt(5.7,5.7));
  EXPECT_FALSE(logical_lt(5.7,-9.0));
  EXPECT_FALSE(logical_lt(0,0.0));
}

TEST(MathFunctions, logical_lt_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_FALSE(stan::math::logical_lt(1.0, nan));
  EXPECT_FALSE(stan::math::logical_lt(nan, 2.0));
  EXPECT_FALSE(stan::math::logical_lt(nan, nan));
}