// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt

/*
    This example contains a detailed discussion of the template expression
    technique used to implement the matrix tools in the dlib C++ library.

    It also gives examples showing how a user can create their own custom
    matrix expressions.

    Note that you should be familiar with the dlib::matrix before reading
    this example.  So if you haven't done so already you should read the
    matrix_ex.cpp example program.
*/

#include <iostream>
#include <dlib/matrix.h>

using namespace dlib;
using namespace std;

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

void custom_matrix_expressions_example();

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

int main()
{

    // Declare some variables used below
    matrix<double,3,1> y;
    matrix<double,3,3> M;
    matrix<double> x;

    // set all elements to 1
    y = 1;
    M = 1;

    // -------------------------  Template Expressions -----------------------------
    // Now I will discuss the "template expressions" technique and how it is
    // used in the matrix object.  First consider the following expression:
    x = y + y;

    /*
        Normally this expression results in machine code that looks, at a high
        level, like the following:
            temp = y + y;
            x = temp

        Temp is a temporary matrix returned by the overloaded + operator.
        temp then contains the result of adding y to itself.  The assignment
        operator copies the value of temp into x and temp is then destroyed while
        the blissful C++ user never sees any of this.

        This is, however, totally inefficient.  In the process described above
        you have to pay for the cost of constructing a temporary matrix object
        and allocating its memory.  Then you pay the additional cost of copying
        it over to x.  It also gets worse when you have more complex expressions
        such as x = round(y + y + y + M*y) which would involve the creation and copying
        of 5 temporary matrices.

        All these inefficiencies are removed by using the template expressions
        technique.  The basic idea is as follows, instead of having operators and
        functions return temporary matrix objects you return a special object that
        represents the expression you wish to perform.

        So consider the expression x = y + y again.  With dlib::matrix what happens
        is the expression y+y returns a matrix_exp object instead of a temporary matrix.
        The construction of a matrix_exp does not allocate any memory or perform any
        computations.  The matrix_exp however has an interface that looks just like a
        dlib::matrix object and when you ask it for the value of one of its elements
        it computes that value on the spot.  Only in the assignment operator does
        someone ask the matrix_exp for these values so this avoids the use of any
        temporary matrices.  Thus the statement x = y + y is equivalent to the following
        code:
            // loop over all elements in y matrix
            for (long r = 0; r < y.nr(); ++r)
                for (long c = 0; c < y.nc(); ++c)
                    x(r,c) = y(r,c) + y(r,c);

        This technique works for expressions of arbitrary complexity.  So if you
        typed x = round(y + y + y + M*y) it would involve no temporary matrices being
        created at all.  Each operator takes and returns only matrix_exp objects.
        Thus, no computations are performed until the assignment operator requests
        the values from the matrix_exp it receives as input.

        There is, however, a slight complication in all of this.  It is for statements
        that involve the multiplication of a complex matrix_exp such as the following:
    */
        x = M*(M+M+M+M+M+M+M);
    /*
        According to the discussion above, this statement would compute the value of
        M*(M+M+M+M+M+M+M) totally without any temporary matrix objects.  This sounds
        good but we should take a closer look.  Consider that the + operator is
        invoked 6 times.  This means we have something like this:

        x = M * (matrix_exp representing M+M+M+M+M+M+M);

        M is being multiplied with a quite complex matrix_exp.  Now recall that when
        you ask a matrix_exp what the value of any of its elements are it computes
        their values *right then*.

        If you think on what is involved in performing a matrix multiply you will
        realize that each element of a matrix is accessed M.nr() times.  In the
        case of our above expression the cost of accessing an element of the
        matrix_exp on the right hand side is the cost of doing 6 addition operations.

        Thus, it would be faster to assign M+M+M+M+M+M+M to a temporary matrix and then
        multiply that by M.  This is exactly what the dlib::matrix does under the covers.
        This is because it is able to spot expressions where the introduction of a
        temporary is needed to speed up the computation and it will automatically do this
        for you.

        Another complication that is dealt with automatically is aliasing.  All matrix
        expressions are said to "alias" their contents.  For example, consider
        the following expressions:
           M + M
           M * M

        We say that the expressions (M + M) and (M * M) alias M.  Additionally, we say that
        the expression (M * M) destructively aliases M.

        To understand why we say (M * M) destructively aliases M consider what would happen
        if we attempted to evaluate it without first assigning (M * M) to a temporary matrix.
        That is, if we attempted to perform the following:
            for (long r = 0; r < M.nr(); ++r)
                for (long c = 0; c < M.nc(); ++c)
                    M(r,c) = rowm(M,r)*colm(M,c);

        It is clear that the result would be corrupted and M wouldn't end up with the right
        values in it.  So in this case we must perform the following:
            temp = M*M;
            M = temp;

        This sort of interaction is what defines destructive aliasing.  Whenever we are
        assigning a matrix expression to a destination that is destructively aliased by
        the expression we need to introduce a temporary.  The dlib::matrix is capable of
        recognizing the two forms of aliasing and introduces temporary matrices only when
        necessary.
    */

    // Next we discuss how to create custom matrix expressions.   In what follows we
    // will define three different matrix expressions and show their use.
    custom_matrix_expressions_example();
}

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

template <typename M>
struct example_op_trans
{
    /*!
        This object defines a matrix expression that represents a transposed matrix.
        As discussed above, constructing this object doesn't compute anything.  It just
        holds a reference to a matrix and presents an interface which defines
        matrix transposition.
    !*/

    // Here we simply hold a reference to the matrix we are supposed to be the transpose of.
    example_op_trans( const M& m_) : m(m_){}
    const M& m;

    // The cost field is used by matrix multiplication code to decide if a temporary needs to
    // be introduced.  It represents the computational cost of evaluating an element of the
    // matrix expression.  In this case we say that the cost of obtaining an element of the
    // transposed matrix is the same as obtaining an element of the original matrix (since
    // transpose doesn't really compute anything).
    const static long cost = M::cost;

    // Here we define the matrix expression's compile-time known dimensions.  Since this
    // is a transpose we define them to be the reverse of M's dimensions.
    const static long NR = M::NC;
    const static long NC = M::NR;

    // Define the type of element in this matrix expression.  Also define the
    // memory manager type used and the layout.  Usually we use the same types as the
    // input matrix.
    typedef typename M::type type;
    typedef typename M::mem_manager_type mem_manager_type;
    typedef typename M::layout_type layout_type;

    // This is where the action is.  This function is what defines the value of an element of
    // this matrix expression.  Here we are saying that m(c,r) == trans(m)(r,c) which is just
    // the definition of transposition.  Note also that we must define the return type from this
    // function as a typedef.  This typedef lets us either return our argument by value or by
    // reference.  In this case we use the same type as the underlying m matrix.  Later in this
    // example program you will see two other options.
    typedef typename M::const_ret_type const_ret_type;
    const_ret_type apply (long r, long c) const { return m(c,r); }

    // Define the run-time defined dimensions of this matrix.
    long nr () const { return m.nc(); }
    long nc () const { return m.nr(); }

    // Recall the discussion of aliasing.  Each matrix expression needs to define what
    // kind of aliasing it introduces so that we know when to introduce temporaries.  The
    // aliases() function indicates that the matrix transpose expression aliases item if
    // and only if the m matrix aliases item.
    template <typename U> bool aliases               ( const matrix_exp<U>& item) const { return m.aliases(item); }
    // This next function indicates that the matrix transpose expression also destructively
    // aliases anything m aliases.  I.e. transpose has destructive aliasing.
    template <typename U> bool destructively_aliases ( const matrix_exp<U>& item) const { return m.aliases(item); }

};

// Here we define a simple function that creates and returns transpose expressions.  Note that the
// matrix_op<> template is a matrix_exp object and exists solely to reduce the amount of boilerplate
// you have to write to create a matrix expression.
template < typename M >
const matrix_op<example_op_trans<M> > example_trans (
    const matrix_exp<M>& m
)
{
    typedef example_op_trans<M> op;
    // m.ref() returns a reference to the object of type M contained in the matrix expression m.
    return matrix_op<op>(op(m.ref()));
}

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

template <typename T>
struct example_op_vector_to_matrix
{
    /*!
        This object defines a matrix expression that holds a reference to a std::vector<T>
        and makes it look like a column vector.  Thus it enables you to use a std::vector
        as if it was a dlib::matrix.

    !*/
    example_op_vector_to_matrix( const std::vector<T>& vect_) : vect(vect_){}

    const std::vector<T>& vect;

    // This expression wraps direct memory accesses so we use the lowest possible cost.
    const static long cost = 1;

    const static long NR = 0; // We don't know the length of the vector until runtime.  So we put 0 here.
    const static long NC = 1; // We do know that it only has one column (since it's a vector)
    typedef T type;
    // Since the std::vector doesn't use a dlib memory manager we list the default one here.
    typedef default_memory_manager mem_manager_type;
    // The layout type also doesn't really matter in this case.  So we list row_major_layout
    // since it is a good default.
    typedef row_major_layout layout_type;

    // Note that we define const_ret_type to be a reference type.  This way we can
    // return the contents of the std::vector by reference.
    typedef const T& const_ret_type;
    const_ret_type apply (long r, long ) const { return vect[r]; }

    long nr () const { return vect.size(); }
    long nc () const { return 1; }

    // This expression never aliases anything since it doesn't contain any matrix expression (it
    // contains only a std::vector which doesn't count since you can't assign a matrix expression
    // to a std::vector object).
    template <typename U> bool aliases               ( const matrix_exp<U>& ) const { return false; }
    template <typename U> bool destructively_aliases ( const matrix_exp<U>& ) const { return false; }
};

template < typename T >
const matrix_op<example_op_vector_to_matrix<T> > example_vector_to_matrix (
    const std::vector<T>& vector
)
{
    typedef example_op_vector_to_matrix<T> op;
    return matrix_op<op>(op(vector));
}

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

template <typename M, typename T>
struct example_op_add_scalar
{
    /*!
        This object defines a matrix expression that represents a matrix with a single
        scalar value added to all its elements.
    !*/

    example_op_add_scalar( const M& m_, const T& val_) : m(m_), val(val_){}

    // A reference to the matrix
    const M& m;
    // A copy of the scalar value that should be added to each element of m
    const T val;

    // This time we add 1 to the cost since evaluating an element of this
    // expression means performing 1 addition operation.
    const static long cost = M::cost + 1;
    const static long NR = M::NR;
    const static long NC = M::NC;
    typedef typename M::type type;
    typedef typename M::mem_manager_type mem_manager_type;
    typedef typename M::layout_type layout_type;

    // Note that we declare const_ret_type to be a non-reference type.  This is important
    // since apply() computes a new temporary value and thus we can't return a reference
    // to it.
    typedef type const_ret_type;
    const_ret_type apply (long r, long c) const { return m(r,c) + val; }

    long nr () const { return m.nr(); }
    long nc () const { return m.nc(); }

    // This expression aliases anything m aliases.
    template <typename U> bool aliases               ( const matrix_exp<U>& item) const { return m.aliases(item); }
    // Unlike the transpose expression.  This expression only destructively aliases something if m does.
    // So this expression has the regular non-destructive kind of aliasing.
    template <typename U> bool destructively_aliases ( const matrix_exp<U>& item) const { return m.destructively_aliases(item); }

};

template < typename M, typename T >
const matrix_op<example_op_add_scalar<M,T> > add_scalar (
    const matrix_exp<M>& m,
    T val
)
{
    typedef example_op_add_scalar<M,T> op;
    return matrix_op<op>(op(m.ref(), val));
}

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

void custom_matrix_expressions_example(
)
{
    matrix<double> x(2,3);
    x = 1, 1, 1,
        2, 2, 2;

    cout << x << endl;

    // Finally, lets use the matrix expressions we defined above.

    // prints the transpose of x
    cout << example_trans(x) << endl;

    // prints this:
    //   11 11 11
    //   12 12 12
    cout << add_scalar(x, 10) << endl;

    // matrix expressions can be nested, even the user defined ones.
    // the following statement prints this:
    //   11 12
    //   11 12
    //   11 12
    cout << example_trans(add_scalar(x, 10)) << endl;

    // Since we setup the alias detection correctly we can even do this:
    x = example_trans(add_scalar(x, 10));
    cout << "new x:\n" << x << endl;

    cout << "Do some testing with the example_vector_to_matrix() function: " << endl;
    std::vector<float> vect;
    vect.push_back(1);
    vect.push_back(3);
    vect.push_back(5);

    // Now lets treat our std::vector like a matrix and print some things.
    cout << example_vector_to_matrix(vect) << endl;
    cout << add_scalar(example_vector_to_matrix(vect), 10) << endl;

    /*
        As an aside, note that dlib contains functions equivalent to the ones we
        defined above.  They are:
            - dlib::trans()
            - dlib::mat() (converts things into matrices)
            - operator+ (e.g. you can say my_mat + 1)

        Also, if you are going to be creating your own matrix expression you should also
        look through the matrix code in the dlib/matrix folder.  There you will find
        many other examples of matrix expressions.
    */
}

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


#include <iostream>
#include <Eigen\Dense>

using namespace Eigen;
using namespace std;

void main() {
	{
		// TODO: http://eigen.tuxfamily.org/dox/group__Sparse__chapter.html
	}
	system("pause");
}


//==============================================================================
//         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_CONSTANT_CONSTANTS_MTHREE_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MTHREE_HPP_INCLUDED

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

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Mthree generic tag

     Represents the Mthree constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Mthree, double, -3
                                , 0xc0400000UL, 0xc008000000000000ULL
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Mthree, Site> dispatching_Mthree(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Mthree, Site>();
   }
   template<class... Args>
   struct impl_Mthree;
  }
  /*!
    Generates value -3

    @par Semantic:

    @code
    T r = Mthree<T>();
    @endcode

    is similar to:

    @code
    T r = T(-3);
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Mthree, Mthree)
} }
#include <boost/simd/constant/common.hpp>

#endif


#include <boost/ratio/mpl/lcm.hpp>


//  (C) Copyright John Maddock 2006.
//  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)

#ifndef BOOST_MATH_TOOLS_RATIONAL_HPP
#define BOOST_MATH_TOOLS_RATIONAL_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/array.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/mpl/int.hpp>

#if BOOST_MATH_POLY_METHOD == 1
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner1_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_POLY_METHOD == 2
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner2_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_POLY_METHOD == 3
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner3_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#endif
#if BOOST_MATH_RATIONAL_METHOD == 1
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner1_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_RATIONAL_METHOD == 2
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner2_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_RATIONAL_METHOD == 3
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner3_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#endif

#if 0
//
// This just allows dependency trackers to find the headers
// used in the above PP-magic.
//
#include <boost/math/tools/detail/polynomial_horner1_2.hpp>
#include <boost/math/tools/detail/polynomial_horner1_3.hpp>
#include <boost/math/tools/detail/polynomial_horner1_4.hpp>
#include <boost/math/tools/detail/polynomial_horner1_5.hpp>
#include <boost/math/tools/detail/polynomial_horner1_6.hpp>
#include <boost/math/tools/detail/polynomial_horner1_7.hpp>
#include <boost/math/tools/detail/polynomial_horner1_8.hpp>
#include <boost/math/tools/detail/polynomial_horner1_9.hpp>
#include <boost/math/tools/detail/polynomial_horner1_10.hpp>
#include <boost/math/tools/detail/polynomial_horner1_11.hpp>
#include <boost/math/tools/detail/polynomial_horner1_12.hpp>
#include <boost/math/tools/detail/polynomial_horner1_13.hpp>
#include <boost/math/tools/detail/polynomial_horner1_14.hpp>
#include <boost/math/tools/detail/polynomial_horner1_15.hpp>
#include <boost/math/tools/detail/polynomial_horner1_16.hpp>
#include <boost/math/tools/detail/polynomial_horner1_17.hpp>
#include <boost/math/tools/detail/polynomial_horner1_18.hpp>
#include <boost/math/tools/detail/polynomial_horner1_19.hpp>
#include <boost/math/tools/detail/polynomial_horner1_20.hpp>
#include <boost/math/tools/detail/polynomial_horner2_2.hpp>
#include <boost/math/tools/detail/polynomial_horner2_3.hpp>
#include <boost/math/tools/detail/polynomial_horner2_4.hpp>
#include <boost/math/tools/detail/polynomial_horner2_5.hpp>
#include <boost/math/tools/detail/polynomial_horner2_6.hpp>
#include <boost/math/tools/detail/polynomial_horner2_7.hpp>
#include <boost/math/tools/detail/polynomial_horner2_8.hpp>
#include <boost/math/tools/detail/polynomial_horner2_9.hpp>
#include <boost/math/tools/detail/polynomial_horner2_10.hpp>
#include <boost/math/tools/detail/polynomial_horner2_11.hpp>
#include <boost/math/tools/detail/polynomial_horner2_12.hpp>
#include <boost/math/tools/detail/polynomial_horner2_13.hpp>
#include <boost/math/tools/detail/polynomial_horner2_14.hpp>
#include <boost/math/tools/detail/polynomial_horner2_15.hpp>
#include <boost/math/tools/detail/polynomial_horner2_16.hpp>
#include <boost/math/tools/detail/polynomial_horner2_17.hpp>
#include <boost/math/tools/detail/polynomial_horner2_18.hpp>
#include <boost/math/tools/detail/polynomial_horner2_19.hpp>
#include <boost/math/tools/detail/polynomial_horner2_20.hpp>
#include <boost/math/tools/detail/polynomial_horner3_2.hpp>
#include <boost/math/tools/detail/polynomial_horner3_3.hpp>
#include <boost/math/tools/detail/polynomial_horner3_4.hpp>
#include <boost/math/tools/detail/polynomial_horner3_5.hpp>
#include <boost/math/tools/detail/polynomial_horner3_6.hpp>
#include <boost/math/tools/detail/polynomial_horner3_7.hpp>
#include <boost/math/tools/detail/polynomial_horner3_8.hpp>
#include <boost/math/tools/detail/polynomial_horner3_9.hpp>
#include <boost/math/tools/detail/polynomial_horner3_10.hpp>
#include <boost/math/tools/detail/polynomial_horner3_11.hpp>
#include <boost/math/tools/detail/polynomial_horner3_12.hpp>
#include <boost/math/tools/detail/polynomial_horner3_13.hpp>
#include <boost/math/tools/detail/polynomial_horner3_14.hpp>
#include <boost/math/tools/detail/polynomial_horner3_15.hpp>
#include <boost/math/tools/detail/polynomial_horner3_16.hpp>
#include <boost/math/tools/detail/polynomial_horner3_17.hpp>
#include <boost/math/tools/detail/polynomial_horner3_18.hpp>
#include <boost/math/tools/detail/polynomial_horner3_19.hpp>
#include <boost/math/tools/detail/polynomial_horner3_20.hpp>
#include <boost/math/tools/detail/rational_horner1_2.hpp>
#include <boost/math/tools/detail/rational_horner1_3.hpp>
#include <boost/math/tools/detail/rational_horner1_4.hpp>
#include <boost/math/tools/detail/rational_horner1_5.hpp>
#include <boost/math/tools/detail/rational_horner1_6.hpp>
#include <boost/math/tools/detail/rational_horner1_7.hpp>
#include <boost/math/tools/detail/rational_horner1_8.hpp>
#include <boost/math/tools/detail/rational_horner1_9.hpp>
#include <boost/math/tools/detail/rational_horner1_10.hpp>
#include <boost/math/tools/detail/rational_horner1_11.hpp>
#include <boost/math/tools/detail/rational_horner1_12.hpp>
#include <boost/math/tools/detail/rational_horner1_13.hpp>
#include <boost/math/tools/detail/rational_horner1_14.hpp>
#include <boost/math/tools/detail/rational_horner1_15.hpp>
#include <boost/math/tools/detail/rational_horner1_16.hpp>
#include <boost/math/tools/detail/rational_horner1_17.hpp>
#include <boost/math/tools/detail/rational_horner1_18.hpp>
#include <boost/math/tools/detail/rational_horner1_19.hpp>
#include <boost/math/tools/detail/rational_horner1_20.hpp>
#include <boost/math/tools/detail/rational_horner2_2.hpp>
#include <boost/math/tools/detail/rational_horner2_3.hpp>
#include <boost/math/tools/detail/rational_horner2_4.hpp>
#include <boost/math/tools/detail/rational_horner2_5.hpp>
#include <boost/math/tools/detail/rational_horner2_6.hpp>
#include <boost/math/tools/detail/rational_horner2_7.hpp>
#include <boost/math/tools/detail/rational_horner2_8.hpp>
#include <boost/math/tools/detail/rational_horner2_9.hpp>
#include <boost/math/tools/detail/rational_horner2_10.hpp>
#include <boost/math/tools/detail/rational_horner2_11.hpp>
#include <boost/math/tools/detail/rational_horner2_12.hpp>
#include <boost/math/tools/detail/rational_horner2_13.hpp>
#include <boost/math/tools/detail/rational_horner2_14.hpp>
#include <boost/math/tools/detail/rational_horner2_15.hpp>
#include <boost/math/tools/detail/rational_horner2_16.hpp>
#include <boost/math/tools/detail/rational_horner2_17.hpp>
#include <boost/math/tools/detail/rational_horner2_18.hpp>
#include <boost/math/tools/detail/rational_horner2_19.hpp>
#include <boost/math/tools/detail/rational_horner2_20.hpp>
#include <boost/math/tools/detail/rational_horner3_2.hpp>
#include <boost/math/tools/detail/rational_horner3_3.hpp>
#include <boost/math/tools/detail/rational_horner3_4.hpp>
#include <boost/math/tools/detail/rational_horner3_5.hpp>
#include <boost/math/tools/detail/rational_horner3_6.hpp>
#include <boost/math/tools/detail/rational_horner3_7.hpp>
#include <boost/math/tools/detail/rational_horner3_8.hpp>
#include <boost/math/tools/detail/rational_horner3_9.hpp>
#include <boost/math/tools/detail/rational_horner3_10.hpp>
#include <boost/math/tools/detail/rational_horner3_11.hpp>
#include <boost/math/tools/detail/rational_horner3_12.hpp>
#include <boost/math/tools/detail/rational_horner3_13.hpp>
#include <boost/math/tools/detail/rational_horner3_14.hpp>
#include <boost/math/tools/detail/rational_horner3_15.hpp>
#include <boost/math/tools/detail/rational_horner3_16.hpp>
#include <boost/math/tools/detail/rational_horner3_17.hpp>
#include <boost/math/tools/detail/rational_horner3_18.hpp>
#include <boost/math/tools/detail/rational_horner3_19.hpp>
#include <boost/math/tools/detail/rational_horner3_20.hpp>
#endif

namespace boost{ namespace math{ namespace tools{

//
// Forward declaration to keep two phase lookup happy:
//
template <class T, class U>
U evaluate_polynomial(const T* poly, U const& z, std::size_t count);

namespace detail{

template <class T, class V, class Tag>
inline V evaluate_polynomial_c_imp(const T* a, const V& val, const Tag*)
{
   return evaluate_polynomial(a, val, Tag::value);
}

} // namespace detail

//
// Polynomial evaluation with runtime size.
// This requires a for-loop which may be more expensive than
// the loop expanded versions above:
//
template <class T, class U>
inline U evaluate_polynomial(const T* poly, U const& z, std::size_t count)
{
   BOOST_ASSERT(count > 0);
   U sum = static_cast<U>(poly[count - 1]);
   for(int i = static_cast<int>(count) - 2; i >= 0; --i)
   {
      sum *= z;
      sum += static_cast<U>(poly[i]);
   }
   return sum;
}
//
// Compile time sized polynomials, just inline forwarders to the
// implementations above:
//
template <std::size_t N, class T, class V>
inline V evaluate_polynomial(const T(&a)[N], const V& val)
{
   typedef mpl::int_<N> tag_type;
   return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a), val, static_cast<tag_type const*>(0));
}

template <std::size_t N, class T, class V>
inline V evaluate_polynomial(const boost::array<T,N>& a, const V& val)
{
   typedef mpl::int_<N> tag_type;
   return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()), val, static_cast<tag_type const*>(0));
}
//
// Even polynomials are trivial: just square the argument!
//
template <class T, class U>
inline U evaluate_even_polynomial(const T* poly, U z, std::size_t count)
{
   return evaluate_polynomial(poly, z*z, count);
}

template <std::size_t N, class T, class V>
inline V evaluate_even_polynomial(const T(&a)[N], const V& z)
{
   return evaluate_polynomial(a, z*z);
}

template <std::size_t N, class T, class V>
inline V evaluate_even_polynomial(const boost::array<T,N>& a, const V& z)
{
   return evaluate_polynomial(a, z*z);
}
//
// Odd polynomials come next:
//
template <class T, class U>
inline U evaluate_odd_polynomial(const T* poly, U z, std::size_t count)
{
   return poly[0] + z * evaluate_polynomial(poly+1, z*z, count-1);
}

template <std::size_t N, class T, class V>
inline V evaluate_odd_polynomial(const T(&a)[N], const V& z)
{
   typedef mpl::int_<N-1> tag_type;
   return a[0] + z * detail::evaluate_polynomial_c_imp(static_cast<const T*>(a) + 1, z*z, static_cast<tag_type const*>(0));
}

template <std::size_t N, class T, class V>
inline V evaluate_odd_polynomial(const boost::array<T,N>& a, const V& z)
{
   typedef mpl::int_<N-1> tag_type;
   return a[0] + z * detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()) + 1, z*z, static_cast<tag_type const*>(0));
}

template <class T, class U, class V>
V evaluate_rational(const T* num, const U* denom, const V& z_, std::size_t count);

namespace detail{

template <class T, class U, class V, class Tag>
inline V evaluate_rational_c_imp(const T* num, const U* denom, const V& z, const Tag*)
{
   return boost::math::tools::evaluate_rational(num, denom, z, Tag::value);
}

}
//
// Rational functions: numerator and denominator must be
// equal in size.  These always have a for-loop and so may be less
// efficient than evaluating a pair of polynomials. However, there
// are some tricks we can use to prevent overflow that might otherwise
// occur in polynomial evaluation, if z is large.  This is important
// in our Lanczos code for example.
//
template <class T, class U, class V>
V evaluate_rational(const T* num, const U* denom, const V& z_, std::size_t count)
{
   V z(z_);
   V s1, s2;
   if(z <= 1)
   {
      s1 = static_cast<V>(num[count-1]);
      s2 = static_cast<V>(denom[count-1]);
      for(int i = (int)count - 2; i >= 0; --i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   else
   {
      z = 1 / z;
      s1 = static_cast<V>(num[0]);
      s2 = static_cast<V>(denom[0]);
      for(unsigned i = 1; i < count; ++i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   return s1 / s2;
}

template <std::size_t N, class T, class U, class V>
inline V evaluate_rational(const T(&a)[N], const U(&b)[N], const V& z)
{
   return detail::evaluate_rational_c_imp(a, b, z, static_cast<const mpl::int_<N>*>(0));
}

template <std::size_t N, class T, class U, class V>
inline V evaluate_rational(const boost::array<T,N>& a, const boost::array<U,N>& b, const V& z)
{
   return detail::evaluate_rational_c_imp(a.data(), b.data(), z, static_cast<mpl::int_<N>*>(0));
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_RATIONAL_HPP


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// 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.

#ifndef SMOOTH__FEEDBACK__MESH_FUNCTION_HPP_
#define SMOOTH__FEEDBACK__MESH_FUNCTION_HPP_

/**
 * @file
 * @brief Evaluate transform-like functions and derivatives on collocation points.
 */

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <smooth/diff.hpp>

#include "mesh.hpp"
#include "smooth/feedback/utils/sparse.hpp"

namespace smooth::feedback {

template<uint8_t Deriv>
struct MeshValue;

/**
 * @brief Result of Mesh function.
 *
 * A generic function is a mapping (t0, tf, q, X, U) -> R^M
 *
 * The function variables are
 * NAME     SIZE
 *   t0        1
 *   tf        1
 *    X nx*(N+1)
 *    U     nu*N
 *
 * Let the total number of variables be K = 2 + nq + nx*(N+1) + nu*N.
 */
template<>
struct MeshValue<0>
{
  /// @brief Function value (size M)
  Eigen::VectorXd F;

  /// @brief If set to true correct allocation is assumed, and no allocation is performed.
  bool allocated{false};
};

/**
 * @brief Result and first derivative of Mesh function.
 *
 * @see MeshValue<0>
 */
template<>
struct MeshValue<1> : public MeshValue<0>
{
  /// @brief Size M x numVar
  Eigen::SparseMatrix<double> dF;
};

/**
 * @brief Result, first, and second derivatives w.r.t. collocation variables
 *
 * @see MeshValue<0>, MeshValue<1>
 */
template<>
struct MeshValue<2> : public MeshValue<1>
{
  /// @brief Multipliers (must be set before)
  Eigen::VectorXd lambda;

  /// @brief Size numVar x numVar
  Eigen::SparseMatrix<double> d2F;
};

/**
 * @brief Reset MeshValue to zeros.
 *
 * If sparse matrices are compressed the coefficients are set to zero, otherwise coefficients are
 * removed but memory is kept.
 */
template<uint8_t Deriv>
void set_zero(MeshValue<Deriv> & mv)
{
  mv.F.setZero();
  if constexpr (Deriv >= 1) { set_zero(mv.dF); }
  if constexpr (Deriv >= 2) { set_zero(mv.d2F); }
}

/**
 * @brief Evaluate function over a mesh.
 *
 * @tparam Deriv differentiation order (0, 1, or 2)
 * @tparam DT inner differentiation method (used to differentiate f)
 *
 * Computes the expression
 * \f[
 *  \begin{bmatrix}
 *    f(t_0 + (t_f - t_0) \tau_0, x_0, u_0) \\
 *    f(t_0 + (t_f - t_0) \tau_1, x_1, u_1) \\
 *    \vdots \\
 *    f(t_{N-1} + (t_f - t_0) \tau_{N-1}, x_{N-1}, u_{N-1})
 *  \end{bmatrix}.
 * \f]
 *
 * If DT > 0 derivatives w.r.t. t0, tf, {xi} and {ui} are returned.
 *
 * @param out result structure
 * @param m mesh
 * @param f integrand
 * @param t0 initial time parameter
 * @param tf final time parameter
 * @param xs state parameters {xi}
 * @param us input parameters {ui}
 * @param scale scale values with quadrature weights
 */
template<uint8_t Deriv, diff::Type DT = diff::Type::Default>
  requires(Deriv <= 2)
void mesh_eval(
  MeshValue<Deriv> & out,
  const MeshType auto & m,
  auto & f,
  const double t0,
  const double tf,
  std::ranges::range auto && xs,
  std::ranges::range auto && us,
  bool scale = false)
{
  using utils::zip, std::views::iota;
  using X = PlainObject<std::decay_t<std::ranges::range_value_t<decltype(xs)>>>;
  using U = PlainObject<std::decay_t<std::ranges::range_value_t<decltype(us)>>>;

  static constexpr auto nx = Dof<X>;
  static constexpr auto nu = Dof<U>;
  static constexpr auto nf = Dof<std::invoke_result_t<decltype(f), double, X, U>>;

  static_assert(nx > -1, "State dimension must be static");
  static_assert(nu > -1, "Input dimension must be static");
  static_assert(nf > -1, "Output size must be static");

  const auto N = m.N_colloc();

  const Eigen::Index numOuts = nf * N;
  const Eigen::Index numVars = 2 + nx * (N + 1) + nu * N;

  // ALLOCATION

  if (!out.allocated) {
    out.F.resize(numOuts);

    if constexpr (Deriv >= 1) {
      Eigen::VectorXi pattern = Eigen::VectorXi::Zero(numVars);
      pattern.segment(0, 1).setConstant(numOuts);                 // t0 is dense
      pattern.segment(1, 1).setConstant(numOuts);                 // tf is dense
      pattern.segment(2, nx * N).setConstant(nf);                 // block diagonal, last x not used
      pattern.segment(2 + nx * (N + 1), nu * N).setConstant(nf);  // block diagonal

      out.dF.resize(numOuts, numVars);
      out.dF.reserve(pattern);
    }

    if constexpr (Deriv >= 2u) {
      Eigen::VectorXi pattern = Eigen::VectorXi::Zero(numVars);
      pattern.segment(0, 1).setConstant(1);  // t0
      pattern.segment(1, 1).setConstant(2);  // t0, tf
      for (auto i = 0u; i < N; ++i) {
        for (auto j = 0u; j < nx; ++j) {
          pattern(2 + nx * i + j) = 2 + (j + 1);  // t0, tf, x upper diag
        }
      }
      for (auto i = 0u; i < N; ++i) {
        for (auto j = 0u; j < nu; ++j) {
          pattern(2 + nx * (N + 1) + nu * i + j) = 2 + nx + (j + 1);  // t0, tf, x, u upper diag
        }
      }
      out.d2F.resize(numVars, numVars);
      out.d2F.reserve(pattern);
    }

    out.allocated = true;
  }

  set_zero(out);

  for (const auto & [i, tau, w_quad, x, u] :
       zip(iota(0u, N), m.all_nodes(), m.all_weights(), xs, us)) {
    const double ti = t0 + (tf - t0) * tau;
    const X xi      = x;
    const U ui      = u;

    const double mtau = 1. - tau;
    const double w    = scale ? w_quad : 1.;

    const auto fvals = diff::dr<Deriv, DT>(f, wrt(ti, xi, ui));

    out.F.segment(i * nf, nf) = w * std::get<0>(fvals);

    if constexpr (Deriv >= 1u) {
      const auto & df = std::get<1>(fvals);
      const auto row0 = nf * i;

      block_add(out.dF, row0, 0, df.middleCols(0, 1), w * mtau);
      block_add(out.dF, row0, 1, df.middleCols(0, 1), w * tau);
      block_add(out.dF, row0, 2 + i * nx, df.middleCols(1, nx), w);
      block_add(out.dF, row0, 2 + nx * (N + 1) + nu * i, df.middleCols(1 + nx, nu), w);

      if constexpr (Deriv >= 2u) {
        assert(out.lambda.size() == numOuts);

        const auto & d2f = std::get<2>(fvals);

        for (auto j = 0u; j < nf; ++j) {
          const double wl = w * out.lambda(row0 + j);

          // destination block locations
          const auto t0_d = 0;
          const auto tf_d = 1;
          const auto x_d  = 2 + i * nx;
          const auto u_d  = 2 + (N + 1) * nx + i * nu;

          // source block locations
          const auto b_s = (1 + nx + nu) * j;
          const auto t_s = 0;
          const auto x_s = 1;
          const auto u_s = 1 + nx;

          // clang-format off
          // t0 row
          block_add(out.d2F, t0_d, t0_d, d2f.block(t_s, b_s + t_s, 1, 1 ), wl * mtau * mtau, true);
          block_add(out.d2F, t0_d, tf_d, d2f.block(t_s, b_s + t_s, 1, 1 ), wl * mtau * tau, true);
          block_add(out.d2F, t0_d,  x_d, d2f.block(t_s, b_s + x_s, 1, nx), wl * mtau, true);
          block_add(out.d2F, t0_d,  u_d, d2f.block(t_s, b_s + u_s, 1, nu), wl * mtau, true);

          // tf row
          block_add(out.d2F, tf_d, tf_d, d2f.block(t_s, b_s + t_s, 1, 1 ), wl * tau * tau, true);
          block_add(out.d2F, tf_d,  x_d, d2f.block(t_s, b_s + x_s, 1, nx), wl * tau, true);
          block_add(out.d2F, tf_d,  u_d, d2f.block(t_s, b_s + u_s, 1, nu), wl * tau, true);

          // x row
          block_add(out.d2F, x_d,  x_d, d2f.block(x_s, b_s + x_s, nx, nx), wl, true);
          block_add(out.d2F, x_d,  u_d, d2f.block(x_s, b_s + u_s, nx, nu), wl, true);

          // u row
          block_add(out.d2F, u_d,  u_d, d2f.block(u_s, b_s + u_s, nu, nu), wl, true);
          // clang-format on
        }
      }
    }
  }
}

/**
 * @brief Evaluate integral over a mesh.
 *
 * @tparam Deriv differentiation order (0, 1, or 2)
 * @tparam DT inner differentiation method (used to differentiate f)

 * This function approximates the integral
 * \f[
 *   \int_{t_0}^{t_f} f(s, x(s), u(s)) \mathrm{d} s.
 * \f]
 * by computing the quadrature
 * \f[
 *    (t_f - t_0) \sum_{i = 0}^N w_i f(t_0 + (t_f - t_0) \tau_i, x_i, u_i).
 * \f]
 *
 * If DT > 0 derivatives w.r.t. t0, tf, {xi} and {ui} are returned.
 *
 * @param out result structure
 * @param m mesh
 * @param f integrand
 * @param t0 initial time parameter
 * @param tf final time parameter
 * @param xs state parameters {xi}
 * @param us input parameters {ui}
 */
template<uint8_t Deriv, diff::Type DT = diff::Type::Default>
  requires(Deriv <= 2)
void mesh_integrate(
  MeshValue<Deriv> & out,
  const MeshType auto & m,
  auto & f,
  const double t0,
  const double tf,
  std::ranges::range auto && xs,
  std::ranges::range auto && us)
{
  using utils::zip, std::views::iota;
  using X = PlainObject<std::decay_t<std::ranges::range_value_t<decltype(xs)>>>;
  using U = PlainObject<std::decay_t<std::ranges::range_value_t<decltype(us)>>>;

  static constexpr auto nx = Dof<X>;
  static constexpr auto nu = Dof<U>;
  static constexpr auto nf = Dof<std::invoke_result_t<decltype(f), double, X, U>>;

  static_assert(nx > -1, "State dimension must be static");
  static_assert(nu > -1, "Input dimension must be static");
  static_assert(nf > -1, "Output size must be static");

  const auto N = m.N_colloc();

  const Eigen::Index numOuts = nf;
  const Eigen::Index numVars = 2 + nx * (N + 1) + nu * N;

  // ALLOCATION

  if (!out.allocated) {
    out.F.resize(numOuts);

    if constexpr (Deriv >= 1) {
      Eigen::VectorXi pattern = Eigen::VectorXi::Constant(numVars, numOuts);  // dense
      pattern.segment(2 + nx * N, nx).setZero();

      out.dF.resize(numOuts, numVars);
      out.dF.reserve(pattern);
    }

    if constexpr (Deriv >= 2) {
      Eigen::VectorXi pattern = Eigen::VectorXi::Zero(numVars);  // dense
      pattern(0)              = 1;                               // t0 dense
      pattern(1)              = 2;                               // tf dense
      for (auto i = 0u; i < N; ++i) {
        for (auto j = 0u; j < nx; ++j) {
          pattern(2 + nx * i + j) = 2 + (j + 1);  // t0, tf, x upper diag
        }
      }
      for (auto i = 0u; i < N; ++i) {
        for (auto j = 0u; j < nu; ++j) {
          pattern(2 + nx * (N + 1) + nu * i + j) = 2 + nx + (j + 1);  // t0, tf, x, u upper diag
        }
      }

      out.d2F.resize(numVars, numVars);
      out.d2F.reserve(pattern.replicate(numOuts, 1).reshaped());
    }

    out.allocated = true;
  }

  set_zero(out);

  for (const auto & [i, tau, w, x, u] : zip(iota(0u, N), m.all_nodes(), m.all_weights(), xs, us)) {
    const double ti = t0 + (tf - t0) * tau;
    const X xi      = x;
    const U ui      = u;

    const double mtau = 1. - tau;

    const auto fvals = diff::dr<Deriv, DT>(f, wrt(ti, xi, ui));

    const auto & fval = std::get<0>(fvals);
    out.F.noalias() += w * (tf - t0) * fval;

    if constexpr (Deriv >= 1u) {
      const auto & df = std::get<1>(fvals);
      // t0
      block_add(out.dF, 0, 0, df.middleCols(0, 1), w * (tf - t0) * mtau);
      block_add(out.dF, 0, 0, fval, -w);
      // tf
      block_add(out.dF, 0, 1, df.middleCols(0, 1), w * (tf - t0) * tau);
      block_add(out.dF, 0, 1, fval, w);
      // x
      block_add(out.dF, 0, 2 + i * nx, df.middleCols(1, nx), w * (tf - t0));
      // u
      block_add(out.dF, 0, 2 + nx * (N + 1) + nu * i, df.middleCols(1 + nx, nu), w * (tf - t0));

      if constexpr (Deriv >= 2u) {
        assert(out.lambda.size() == numOuts);

        const auto & d2f = std::get<2>(fvals);

        for (auto j = 0u; j < nf; ++j) {
          const double wl = w * out.lambda(j);

          // source block locations
          const auto b_s = (1 + nx + nu) * j;  // horizontal block
          const auto t_s = 0;                  // t
          const auto x_s = 1;                  // x
          const auto u_s = 1 + nx;             // u

          // destination block locations
          const auto t0_d = 0;                          // t0
          const auto tf_d = 1;                          // tf
          const auto x_d  = 2 + nx * i;                 // x[i]
          const auto u_d  = 2 + nx * (N + 1) + nu * i;  // u[i]

          // clang-format off
          // t0t0
          block_add(out.d2F, t0_d, t0_d, d2f.block(t_s, b_s + t_s, 1,  1), wl * (tf - t0) * mtau * mtau, true);
          block_add(out.d2F, t0_d, t0_d,  df.block(j,         t_s, 1,  1), -wl * 2 * mtau, true);
          // t0tf
          block_add(out.d2F, t0_d, tf_d, d2f.block(t_s, b_s + t_s, 1,  1), wl * (tf - t0) * mtau * tau, true);
          block_add(out.d2F, t0_d, tf_d,  df.block(j,         t_s, 1,  1), wl * (1 - 2 * tau), true);
          // t0x
          block_add(out.d2F, t0_d, x_d,  d2f.block(t_s, b_s + x_s, 1, nx), wl * (tf - t0) * mtau, true);
          block_add(out.d2F, t0_d, x_d,   df.block(j,         x_s, 1, nx), -wl, true);
          // t0u
          block_add(out.d2F, t0_d, u_d,  d2f.block(t_s, b_s + u_s, 1, nu), wl * (tf - t0) * mtau, true);
          block_add(out.d2F, t0_d, u_d,   df.block(j,         u_s, 1, nu), -wl, true);

          // tftf
          block_add(out.d2F, tf_d, tf_d, d2f.block(t_s, b_s + t_s, 1,  1), wl * (tf - t0) * tau * tau, true);
          block_add(out.d2F, tf_d, tf_d,  df.block(j,         t_s, 1,  1), wl * 2 * tau, true);
          // tfx
          block_add(out.d2F, tf_d, x_d,  d2f.block(t_s, b_s + x_s, 1, nx), wl * (tf - t0) * tau, true);
          block_add(out.d2F, tf_d, x_d,   df.block(j,         x_s, 1, nx), wl, true);
          // tfu
          block_add(out.d2F, tf_d, u_d,  d2f.block(t_s, b_s + u_s, 1, nu), wl * (tf - t0) * tau, true);
          block_add(out.d2F, tf_d, u_d,   df.block(j,         u_s, 1, nu), wl, true);

          // xx
          block_add(out.d2F, x_d, x_d,  d2f.block(x_s, b_s + x_s, nx, nx), wl * (tf - t0), true);
          // xu
          block_add(out.d2F, x_d, u_d,  d2f.block(x_s, b_s + u_s, nx, nu), wl * (tf - t0), true);

          // uu
          block_add(out.d2F, u_d, u_d,  d2f.block(u_s, b_s + u_s, nu, nu), wl * (tf - t0), true);
          // clang-format on
        }
      }
    }
  }
}

/**
 * @brief Evaluate dynamic constraints over a mesh (differentiation version).
 *
 * @tparam Deriv differentiation order (0, 1, or 2)
 * @tparam DT inner differentiation method (used to differentiate f)
 *
 * For interval \f$j\f$ with nodes \f$\tau_i\f$ and weights \f$w_i\f$ the corresponding dynamic
 * constraints are a block of the form
 * \f[
 *   \begin{bmatrix}
 *     w_0 \left( f\left(t_0 + (t_f - t_0) \tau_0, x_0, u_0\right) - \sum_{k=0}^{N_j} D_{k, 0} x_k
 * \right) \\
 *     \vdots  \\
 *     w_{N_j-1} \left( f\left(t_0 + (t_f - t_0) \tau_{N_j-1}, x_{N_j - 1}, u_{N_j - 1}\right) -
 * \sum_{k=0}^{N_j} D_{k, N_j-1} x_k \right) \end{bmatrix}, \f] where \f$D\f$ is the interval
 * differentiation matrix (see interval_diffmat() in Mesh).
 *
 * This function returnes all such blocks stacked into a 1D vector.
 *
 * If DT > 0 derivatives w.r.t. t0, tf, {xi} and {ui} are returned.
 *
 * @param out result structure
 * @param m mesh
 * @param f right-hand side of dynamics
 * @param t0 initial time parameter
 * @param tf final time parameter
 * @param xs state parameters {xi}
 * @param us input parameters {ui}
 */
template<uint8_t Deriv, diff::Type DT = diff::Type::Default>
  requires(Deriv <= 2)
void mesh_dyn(
  MeshValue<Deriv> & out,
  const MeshType auto & m,
  auto & f,
  const double t0,
  const double tf,
  std::ranges::range auto && xs,
  std::ranges::range auto && us)
{
  using utils::zip, std::views::iota;
  using X = PlainObject<std::decay_t<std::ranges::range_value_t<decltype(xs)>>>;
  using U = PlainObject<std::decay_t<std::ranges::range_value_t<decltype(us)>>>;

  static constexpr auto nx = Dof<X>;
  static constexpr auto nu = Dof<U>;
  static constexpr auto nf = Dof<std::invoke_result_t<decltype(f), double, X, U>>;

  static_assert(nx > -1, "State dimension must be static");
  static_assert(nu > -1, "Input dimension must be static");
  static_assert(nf > -1, "Output size must be static");
  static_assert(nx == nf, "Output dimension must be same as state dimension");

  const auto N               = m.N_colloc();
  const Eigen::Index numOuts = nx * N;
  const Eigen::Index numVars = 2 + nx * (N + 1) + nu * N;

  // ALLOCATION

  if (!out.allocated) {
    out.F.resize(numOuts);

    if constexpr (Deriv >= 1) {
      Eigen::VectorXi pattern = Eigen::VectorXi::Zero(numVars);
      pattern(0)              = numOuts;  // t0 dense
      pattern(1)              = numOuts;  // tf dense

      // x has blocks depending on mesh size
      auto idx0 = 2u;
      for (auto ival = 0u; ival < m.N_ivals(); ++ival) {
        const std::size_t K = m.N_colloc_ival(ival);
        pattern.segment(idx0, K * nx) += Eigen::VectorXi::Constant(K * nx, nx + K - 1);
        pattern.segment(idx0 + K * nx, nx) += Eigen::VectorXi::Constant(nx, K);
        idx0 += K * nx;
      }

      // u is block diagonal with small blocks
      pattern.segment(2 + nx * (N + 1), nu * N).setConstant(nx);

      out.dF.resize(numOuts, numVars);
      out.dF.reserve(pattern);
    }

    if constexpr (Deriv >= 2) {
      Eigen::VectorXi pattern = Eigen::VectorXi::Zero(numVars);
      pattern(0)              = 1;
      pattern(1)              = 2;
      for (auto i = 0u; i < N; ++i) {
        for (auto j = 0u; j < nx; ++j) {
          pattern(2 + nx * i + j) = 2 + (j + 1);  // t0, tf, x upper diag
        }
      }
      for (auto i = 0u; i < N; ++i) {
        for (auto j = 0u; j < nu; ++j) {
          pattern(2 + nx * (N + 1) + nu * i + j) = 2 + nx + (j + 1);  // t0, tf, x, u upper diag
        }
      }

      out.d2F.resize(numVars, numVars);
      out.d2F.reserve(pattern);
    }

    out.allocated = true;
  }

  set_zero(out);

  // We build the constraint through two loops over xi:
  //   - the first loop adds wk * f(tk, xk, uk)
  //   - the second loop adds -wk * [x0 ... Xni] * dk

  // ADD FIRST PART

  for (const auto & [i, tau, w, x, u] : zip(iota(0u, N), m.all_nodes(), m.all_weights(), xs, us)) {
    const double ti = t0 + (tf - t0) * tau;
    const X xi      = x;
    const U ui      = u;

    const auto row0   = nx * i;
    const double mtau = 1. - tau;

    const auto fvals = diff::dr<Deriv, DT>(f, wrt(ti, xi, ui));
    const auto fval  = std::get<0>(fvals);

    out.F.segment(row0, nx) += w * (tf - t0) * fval;

    if constexpr (Deriv >= 1) {
      const auto & df = std::get<1>(fvals);

      // clang-format off
      // dF/dt0 = -f + (tf - t0) * df/dti * (1-tau)
      block_add(out.dF, row0, 0, fval, -w);
      block_add(out.dF, row0, 0, df.col(0), w * (tf - t0) * mtau);
      // dF/dtf = f + (tf - t0) * df/dti * tau
      block_add(out.dF, row0, 1, fval, w);
      block_add(out.dF, row0, 1, df.col(0), w * (tf - t0) * tau);
      // dF/dx
      block_add(out.dF, row0, 2 + nx * i, df.middleCols(1, nx), w * (tf - t0));
      // dF/du
      block_add(out.dF, row0, 2 + nx * (N + 1) + nu * i, df.middleCols(1 + nx, nu), w * (tf - t0));
      // clang-format on

      if constexpr (Deriv >= 2) {
        assert(out.lambda.size() == numOuts);

        const auto & d2f = std::get<2>(fvals);

        for (auto j = 0u; j < nx; ++j) {
          const double wl = w * out.lambda(row0 + j);

          // destination block locations
          const auto t0_d = 0;
          const auto tf_d = 1;
          const auto x_d  = 2 + i * nx;
          const auto u_d  = 2 + (N + 1) * nx + i * nu;

          // source block locations
          const auto b_s = (1 + nx + nu) * j;
          const auto t_s = 0;
          const auto x_s = 1;
          const auto u_s = 1 + nx;

          // clang-format off
          // t0t0
          block_add(out.d2F, t0_d, t0_d, d2f.block(t_s, b_s + t_s, 1, 1 ), wl * (tf - t0) * mtau * mtau, true);
          block_add(out.d2F, t0_d, t0_d,  df.block(j,         t_s, 1, 1 ), -wl * 2 * mtau, true);
          // t0tf
          block_add(out.d2F, t0_d, tf_d, d2f.block(t_s, b_s + t_s, 1, 1 ), wl * (tf - t0) * mtau * tau, true);
          block_add(out.d2F, t0_d, tf_d,  df.block(j,         t_s, 1, 1 ), wl * (1. - 2 * tau), true);
          // t0x
          block_add(out.d2F, t0_d, x_d, d2f.block(t_s, b_s + x_s, 1, nx), wl * (tf - t0) * mtau, true);
          block_add(out.d2F, t0_d, x_d,  df.block(j,         x_s, 1, nx), -wl, true);
          // t0u
          block_add(out.d2F, t0_d, u_d, d2f.block(t_s, b_s + u_s, 1, nu), wl * (tf - t0) * mtau, true);
          block_add(out.d2F, t0_d, u_d,  df.block(j,         u_s, 1, nu), -wl, true);

          // tftf
          block_add(out.d2F, tf_d, tf_d, d2f.block(t_s, b_s + t_s, 1,  1), wl * (tf - t0) * tau * tau, true);
          block_add(out.d2F, tf_d, tf_d,  df.block(j,         t_s, 1,  1), wl * 2 * tau, true);
          // tfx
          block_add(out.d2F, tf_d, x_d,  d2f.block(t_s, b_s + x_s, 1, nx), wl * (tf - t0) * tau, true);
          block_add(out.d2F, tf_d, x_d,   df.block(j,         x_s, 1, nx), wl, true);
          // tfu
          block_add(out.d2F, tf_d, u_d,  d2f.block(t_s, b_s + u_s, 1, nu), wl * (tf - t0) * tau, true);
          block_add(out.d2F, tf_d, u_d,   df.block(j,         u_s, 1, nu), wl, true);

          // xx
          block_add(out.d2F, x_d, x_d,  d2f.block(x_s, b_s + x_s, nx, nx), wl * (tf - t0), true);
          // xu
          block_add(out.d2F, x_d, u_d,  d2f.block(x_s, b_s + u_s, nx, nu), wl * (tf - t0), true);

          // uu
          block_add(out.d2F, u_d, u_d,  d2f.block(u_s, b_s + u_s, nu, nu), wl * (tf - t0), true);
          // clang-format on
        }
      }
    }
  }

  // ADD SECOND PART (LINEAR IN X, NO SECOND DERIVATIVE..)

  auto ival      = 0u;  // current interval index
  auto ival_idx0 = 0u;  // node start index of current interval
  auto Nival     = m.N_colloc_ival(ival);

  for (const auto & [i, x] : zip(iota(0u, N + 1), xs)) {
    if (i == ival_idx0 + Nival) {
      // jumping to new interval --- add overlap to current interval before switching
      const auto [alpha, Dus] = m.interval_diffmat_unscaled(ival);
      for (const auto & [j, w] : zip(iota(0u, Nival), m.interval_weights(ival))) {
        const auto row0   = (ival_idx0 + j) * nx;
        const double coef = -w * alpha * Dus(i - ival_idx0, j);

        out.F.segment(row0, nx) += coef * x;

        if constexpr (Deriv >= 1) {
          // add diagonal matrix
          for (auto k = 0u; k < nx; ++k) { out.dF.coeffRef(row0 + k, 2 + nx * i + k) += coef; }
        }
      }

      // update interval
      ++ival;
      if (ival < m.N_ivals()) {
        ival_idx0 += Nival;
        Nival = m.N_colloc_ival(ival);
      }
    }

    if (i < N) {
      const auto [alpha, Dus] = m.interval_diffmat_unscaled(ival);
      for (const auto & [j, w] : zip(iota(0u, Nival), m.interval_weights(ival))) {
        const auto row0   = (ival_idx0 + j) * nx;
        const double coef = -w * alpha * Dus(i - ival_idx0, j);

        out.F.segment(row0, nx) += coef * x;

        if constexpr (Deriv >= 1) {
          // add diagonal matrix
          for (auto k = 0u; k < nx; ++k) { out.dF.coeffRef(row0 + k, 2 + nx * i + k) += coef; }
        }
      }
    }
  }
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__MESH_FUNCTION_HPP_


/***********************************************************************************************************************
 *  OpenStudio(R), Copyright (c) 2008-2018, Alliance for Sustainable Energy, LLC. All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 *  following conditions are met:
 *
 *  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *  disclaimer.
 *
 *  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *  following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 *  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote
 *  products derived from this software without specific prior written permission from the respective party.
 *
 *  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative
 *  works may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without
 *  specific prior written permission from Alliance for Sustainable Energy, LLC.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER, THE UNITED STATES GOVERNMENT, OR ANY CONTRIBUTORS BE LIABLE FOR
 *  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **********************************************************************************************************************/

#ifndef UTILITIES_GEOMETRY_INTERSECTION_HPP
#define UTILITIES_GEOMETRY_INTERSECTION_HPP

#include "../UtilitiesAPI.hpp"

#include "Point3d.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  /** IntersectionResult contains detailed information about an intersection. */
  class UTILITIES_API IntersectionResult {
  public:
    IntersectionResult(const std::vector<Point3d>& polygon1,
                       const std::vector<Point3d>& polygon2,
                       const std::vector< std::vector<Point3d> >& newPolygons1,
                       const std::vector< std::vector<Point3d> >& newPolygons2);

    // vertices of first polygon after intersection
    std::vector<Point3d> polygon1() const;

    // vertices of second polygon after intersection
    std::vector<Point3d> polygon2() const;

    // new polygons generated from the first surface
    std::vector< std::vector<Point3d> > newPolygons1() const;

    // new polygons generated from the second surface
    std::vector< std::vector<Point3d> > newPolygons2() const;

  private:
    std::vector<Point3d> m_polygon1;
    std::vector<Point3d> m_polygon2;
    std::vector< std::vector<Point3d> > m_newPolygons1;
    std::vector< std::vector<Point3d> > m_newPolygons2;
  };

  /// removes spikes from a polygon, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<Point3d> removeSpikes(const std::vector<Point3d>& polygon, double tol);

  /// returns true if point is inside polygon, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API bool pointInPolygon(const Point3d& point, const std::vector<Point3d>& polygon, double tol);

  /// compute the union of two overlapping polygons, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API boost::optional<std::vector<Point3d> > join(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol);

  /// compute the union of many polygons, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<std::vector<Point3d> > joinAll(const std::vector<std::vector<Point3d> >& polygons, double tol);

  /// intersect two polygons, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API boost::optional<IntersectionResult> intersect(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol);

  /// subtract all holes from polygon, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<std::vector<Point3d> > subtract(const std::vector<Point3d>& polygon, const std::vector<std::vector<Point3d> >& holes, double tol);

  /// returns true polygon intersects iteself, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  /// returns false if polygon has less than three vertices
  UTILITIES_API bool selfIntersects(const std::vector<Point3d>& polygon, double tol);

  /// returns true if polygon1 intersects polygon2, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  /// returns false if either polygon has less than three vertices
  UTILITIES_API bool intersects(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol);

  /// returns true if geometry1 is completely within polygon2, requires that all vertices are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  /// geometry1 can be a point or a polygon
  /// currently only tests that all points of geometry1 are within polygon2, better support when upgrade to boost 1.57
  UTILITIES_API bool within(const Point3d& point1, const std::vector<Point3d>& polygon2, double tol);
  UTILITIES_API bool within(const std::vector<Point3d>& geometry1, const std::vector<Point3d>& polygon2, double tol);

  /// simplify a list of vertices
  UTILITIES_API std::vector<Point3d> simplify(const std::vector<Point3d>& vertices, bool removeCollinear, double tol);

} // openstudio

#endif //UTILITIES_GEOMETRY_INTERSECTION_HPP


#if 0
#include <algorithm>
#include <cassert>
#include <climits>
#include <functional>
#include <iomanip>
#include <memory>
#include <random>
#include <sstream>
#include <string>
#include <utility>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/random_spanning_tree.hpp>

#include "dbg.hpp"
#include "dump.hpp"
#include "eemp.hpp"
#include "partial_perm.hpp"
#include "perm_group.hpp"
#include "perm_set.hpp"
#include "util.hpp"

namespace mpsym
{

namespace internal
{

namespace eemp
{

std::vector<std::vector<unsigned>> action_component(
  std::vector<unsigned> const &alpha,
  std::vector<PartialPerm> const &generators, unsigned dom_max,
  SchreierTree &schreier_tree, OrbitGraph &orbit_graph)
{
#ifndef NDEBUG
  DBG(DEBUG) << "Computing component of action of generators:";
  for (PartialPerm const &gen : generators)
    DBG(TRACE) << gen;

  DBG(TRACE) << "on:";
  DBG(TRACE) << alpha;
#endif

  // compute domain range over all generators
  unsigned dom_min = UINT_MAX;
  for (PartialPerm const &gen : generators)
    dom_min = std::min(dom_min, gen.dom_min());

  // data structures for efficient component membership testing
  std::vector<int> elem_size_present(dom_max + 1u, 0);
  elem_size_present[alpha.size()] = true;

  struct ComponentElemId {
    ComponentElemId(unsigned id, std::size_t hash) : id(id), hash(hash) {}

    unsigned id;
    std::size_t hash;
  };

  std::vector<std::vector<ComponentElemId>> elem_ids(dom_max + 1u);

  std::size_t hash = util::container_hash(alpha.begin(), alpha.end());
  elem_ids[alpha.size()].push_back(ComponentElemId(0u, hash));

  // if beta is a component element, set 'id' to it's component index,
  // otherwise add add it to the component
  auto in_component = [&](std::vector<unsigned> const &beta, unsigned &id) {
    bool contained = true;

    std::size_t beta_hash = util::container_hash(beta.begin(), beta.end());

    auto const size_idx = beta.size();
    if (!elem_size_present[size_idx]) {
      contained = false;

    } else {
      auto it = std::find_if(
        elem_ids[size_idx].begin(), elem_ids[size_idx].end(),
        [&](ComponentElemId const &cei) { return cei.hash == beta_hash; });

      if (it == elem_ids[size_idx].end())
        contained = false;
      else
        id = it->id;
    }

    if (contained)
      return true;

    DBG(TRACE) << "Adjoining " << beta;
    elem_size_present[size_idx] = true;
    elem_ids[size_idx].push_back(ComponentElemId(id, beta_hash));

    return false;
  };

  // main loop
  std::vector<std::vector<unsigned>> component {alpha};
  decltype(schreier_tree.data) schreier_tree_data;
  decltype(orbit_graph.data) orbit_graph_data(generators.size());

  unsigned i = 0u, n = 0u;
  while (i < component.size()) {
    std::vector<unsigned> beta = component[i];
    DBG(TRACE) << "Looking at Component element " << beta
               << " (i = " << i  << ')';

    for (auto j = 0u; j < generators.size(); ++j) {
      DBG(TRACE) << "Considering generator " << generators[j];

      auto beta_prime(generators[j].image<std::vector>(beta.begin(),
                                                       beta.end()));

      unsigned id = static_cast<unsigned>(component.size());

      if (!in_component(beta_prime, id)) {
        component.push_back(beta_prime);

        ++n;

        schreier_tree_data.push_back(std::make_pair(i, j));
        DBG(TRACE) << "v_" << n + 1u << " = " << i + 1u;
        DBG(TRACE) << "w_" << n  + 1u<< " = " << j + 1u;

        orbit_graph_data[j].push_back(n);
        DBG(TRACE) << "g_" << i + 1u << "," << j + 1u << " = " << n + 1u;

      } else {
        DBG(TRACE) << beta_prime << " already processed";

        orbit_graph_data[j].push_back(id);
        DBG(TRACE) << "g_" << i + 1u << "," << j + 1u << " = " << id + 1u;
      }
    }

    ++i;
  }

  schreier_tree.data = schreier_tree_data;

#ifndef NDEBUG
  DBG(TRACE) << "Resulting action component";
  for (auto i = 0u; i < component.size(); ++i)
    DBG(TRACE) << i + 1u << ": " << component[i];
#endif

  DBG(TRACE) << "Resulting schreier tree:\n" << schreier_tree;

  orbit_graph.data = orbit_graph_data;

  DBG(TRACE) << "Resulting orbit graph:\n" << orbit_graph;

  return component;
}

std::pair<unsigned, std::vector<unsigned>> strongly_connected_components(
  OrbitGraph const &orbit_graph)
{
  boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS> g;

  for (auto i = 0u; i < orbit_graph.data[0].size(); ++i)
    boost::add_vertex(g);

  for (auto i = 0u; i < orbit_graph.data[0].size(); ++i) {
    for (auto const &row : orbit_graph.data) {
      auto j = row[i];
      if (j != i)
        boost::add_edge(i, j, g);
    }
  }

  std::vector<unsigned> component(boost::num_vertices(g));
  unsigned num = boost::strong_components(g, &component[0]);

  return std::make_pair(num, component);
}

SchreierTree scc_spanning_tree(
  unsigned i, OrbitGraph const &orbit_graph, std::vector<unsigned> const &scc)
{
  static auto re(util::random_engine());

  DBG(TRACE) << "Finding spanning tree for s.c.c rooted at node " << i + 1u
             << " in orbit graph:\n" << orbit_graph;

  // construct s.c.c. subgraph
  struct VertexProperty { unsigned i; };
  struct EdgeProperty { unsigned gen; };

  boost::adjacency_list<
    boost::vecS, boost::vecS, boost::directedS, VertexProperty, EdgeProperty> g;

  std::vector<unsigned> scc_i;
  std::vector<unsigned> vertex_map(scc.size(), 0u);

  DBG(TRACE) << "Constructing temporary subgraph";

  for (unsigned j = 0u; j < scc.size(); ++j) {
    if (scc[j] == scc[i]) {
      scc_i.push_back(j);

      unsigned v = boost::add_vertex({j}, g);
      vertex_map[j] = v + 1u;

      DBG(TRACE) << "Added vertex " << v << " (" << j + 1u << ')';
    }
  }

  assert(scc_i[0] == i && "provided representative is first node in s.c.c");

  for (unsigned source : scc_i) {
    for (unsigned row = 0; row < orbit_graph.data.size(); ++row) {
      unsigned dest = orbit_graph.data[row][source];

      unsigned source_vertex = vertex_map[source];
      unsigned dest_vertex = vertex_map[dest];

      if (dest_vertex > 0u && dest_vertex != source_vertex) {
        boost::add_edge(dest_vertex - 1u, source_vertex - 1u, {row}, g);

        DBG(TRACE) << "Added edge "
                   << source_vertex - 1u << " => " << dest_vertex - 1u
                   << " (" << source + 1u << " => " << dest + 1u
                   << " (" << row + 1u << "))";
      }
    }
  }

  // find random spanning tree
  SchreierTree spanning_tree;

  std::vector<unsigned> pred(boost::num_vertices(g));

  DBG(TRACE) << "Finding random spanning tree...";

  boost::random_spanning_tree(
    g, re, boost::root_vertex(vertex_map[i] - 1u).predecessor_map(&pred[0]));

  DBG(TRACE) << "Found spanning tree with predecessor relationships: "
             << std::vector<unsigned>(pred.begin() + 1, pred.end());

  // convert into Schreier tree format
  spanning_tree.data =
    std::vector<std::pair<unsigned, unsigned>>(scc.size() - 1u);

  for (auto j = 1u; j < pred.size(); ++j) {
    auto child(boost::vertex(j, g));
    auto parent(boost::vertex(pred[j], g));
    auto edge(boost::edge(child, parent, g));

    assert(edge.second && "edge exists");

    spanning_tree.data[g[child].i - 1u] =
      std::make_pair(g[parent].i, g[edge.first].gen);
  }

  DBG(TRACE) << "Resulting spanning Schreier tree is:\n" << spanning_tree;

  return spanning_tree;
}

PartialPerm schreier_trace(
  unsigned x, SchreierTree const &schreier_tree,
  std::vector<PartialPerm> const &generators, unsigned dom_max, unsigned target)
{
  PartialPerm res(dom_max);

  while (x != target) {
    unsigned v = std::get<0>(schreier_tree.data[x - 1u]);
    unsigned w = std::get<1>(schreier_tree.data[x - 1u]);

    res = generators[w] * res;
    x = v;
  }

  return res;
}

PermGroup schreier_generators(unsigned i,
  std::vector<PartialPerm> const &generators, unsigned dom_max,
  std::vector<std::vector<unsigned>> const &action_component,
  SchreierTree const &schreier_tree, OrbitGraph const &orbit_graph,
  std::vector<unsigned> const &sccs)
{
  auto im(action_component[i]);

  DBG(TRACE) << "Finding schreier generators for Sx for: " << im;

  if (im.empty()) {
    DBG(TRACE) << "=> Returning empty permutation group";
    return PermGroup();
  }

  unsigned im_max = im.back();

  std::vector<unsigned> scc;
  for (unsigned j = 0u; j < sccs.size(); ++j) {
    if (sccs[j] == sccs[i])
      scc.push_back(j);
  }

#ifndef NDEBUG
  std::vector<std::vector<unsigned>> _scc(scc.size());
  for (auto j = 0u; j < scc.size(); ++j)
    _scc[j] = action_component[scc[j]];

  DBG(TRACE) << "Strongly connected component of x is: " << _scc;
#endif

  PermSet sg_gens;

  for (auto j = 0u; j < scc.size(); ++j) {
    for (auto k = 0u; k < generators.size(); ++k) {

      unsigned l = orbit_graph.data[k][scc[j]];
      if (sccs[l] != sccs[i])
        continue;

      DBG(TRACE) << "Found generator for j/i(j)/k/l = "
                 << j + 1u << '/'
                 << scc[j] + 1u << '/'
                 << k + 1u << '/'
                 << l + 1u;

      PartialPerm ui(schreier_trace(
        scc[j], schreier_tree, generators, dom_max, i));
      PartialPerm ul(schreier_trace(
        l, schreier_tree, generators, dom_max, i));

      DBG(TRACE) << "where:";
      DBG(TRACE) << "u_i(j) = " << "u_" << scc[j] + 1u << " = " << ui;
      DBG(TRACE) << "x_k = " << "x_" << k + 1u << " = " << generators[k];
      DBG(TRACE) << "~u_l = " << "u_" << l + 1u << " = " << ~ul;

      PartialPerm sg(ui * generators[k] * ~ul);
      sg = sg.restricted(im.begin(), im.end());

      DBG(TRACE) << "Schreier generator is: " << sg;

      if (!sg.id())
        sg_gens.emplace(sg.to_perm(im_max));
    }
  }

  PermGroup res(im_max, sg_gens);

  DBG(TRACE) << "=> Returning:";
  DBG(TRACE) << res;

  return res;
}

std::vector<PartialPerm> r_class_representatives(
  SchreierTree const &schreier_tree, std::vector<PartialPerm> const &generators)
{
  std::vector<std::vector<unsigned>> st_adj(schreier_tree.data.size() + 1u);
  for (auto i = 0u; i < schreier_tree.data.size(); ++i)
    st_adj[std::get<0>(schreier_tree.data[i])].push_back(i + 1u);

  std::vector<PartialPerm> res;

  std::function<void(unsigned, PartialPerm const &)>
  backtrack = [&](unsigned node, PartialPerm const &pperm) {
    res.push_back(pperm);

    for (unsigned child : st_adj[node]) {
      unsigned gen_idx = schreier_tree.data[child - 1u].second;
      PartialPerm next_pperm(pperm * generators[gen_idx]);

      backtrack(child, next_pperm);
    }
  };

  backtrack(0, generators[0]);

  return res;
}

std::vector<std::vector<unsigned>> expand_partition(
  std::vector<unsigned> partition)
{
  auto mm = std::minmax_element(partition.begin(), partition.end());

  auto first = *mm.first;
  auto last = *mm.second;

  std::vector<std::vector<unsigned>> res(last - first + 1);
  for (unsigned x = 0u; x < partition.size(); ++x)
    res[partition[x] - first].push_back(x);

  std::sort(res.begin(), res.end(),
           [](std::vector<unsigned> const &a,
              std::vector<unsigned> const &b){ return a[0] < b[0]; });

  return res;
}

std::ostream& operator<<(
  std::ostream& stream, eemp::SchreierTree const &schreier_tree)
{
  if (schreier_tree.data.empty()) {
    stream << "empty schreier tree";
    return stream;
  }

  auto size = schreier_tree.data.size();
  auto cellwidth = std::to_string(size).length();

  stream << "i   |";
  stream << std::setw(cellwidth) << 2u;
  for (auto i = 1u; i < size; ++i)
    stream << ' ' << std::setw(cellwidth) << i + 2u;
  stream << '\n';

  stream << std::string(4u, '-');
  for (auto i = 0u; i < size; ++i)
    stream << std::string(cellwidth + 1u, '-');
  stream << '\n';

  stream << "v_i |";
  stream << std::setw(cellwidth) << std::get<0>(schreier_tree.data[0]) + 1u;
  for (auto i = 1u; i < size; ++i) {
    stream << ' ' << std::setw(cellwidth)
           << std::get<0>(schreier_tree.data[i]) + 1u;
  }
  stream << '\n';

  stream << "w_i |";
  stream << std::setw(cellwidth) << std::get<1>(schreier_tree.data[0]) + 1u;
  for (auto i = 1u; i < size; ++i) {
    stream << ' ' << std::setw(cellwidth)
           << std::get<1>(schreier_tree.data[i]) + 1u;
  }

  return stream;
}

std::ostream &operator<<(std::ostream &os, eemp::OrbitGraph const &orbit_graph)
{
  if (orbit_graph.data.empty()) {
    os << "empty orbit graph";
    return os;
  }

  auto size = orbit_graph.data[0].size();

  auto cellwidth = std::to_string(size).length();
  auto pad = std::to_string(orbit_graph.data.size()).length();

  os << "i   " << std::string(pad, ' ') << "  |";
  os << std::setw(cellwidth) << 1u;
  for (auto i = 1u; i < size; ++i)
    os << ' ' << std::setw(cellwidth) << i + 1u;
  os << '\n';

  os << std::string(6u + pad, '-');
  for (auto i = 0u; i < size; ++i)
    os << std::string(cellwidth + 1, '-');
  os << '\n';

  for (auto j = 0u; j < orbit_graph.data.size(); ++j) {
    os << "g_i," << std::setw(pad) << j + 1u << "  |";

    os << std::setw(cellwidth) << orbit_graph.data[j][0] + 1u;
    for (auto k = 1u; k < size; ++k)
      os << ' ' << std::setw(cellwidth) << orbit_graph.data[j][k] + 1u;
    os << '\n';
  }

  auto tmp(strongly_connected_components(orbit_graph));
  auto scc(expand_partition(tmp.second));

  os << "s.c.c." << std::string(pad - 1u, ' ') << " | "
     << DUMP_CUSTOM(scc, "{}", "{}");

  return os;
}

} // namespace eemp

} // namespace internal

} // namespace mpsym
#endif


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include <cstdlib>

#include <boost/math/special_functions/fpclassify.hpp>

#include <vtkMinimalStandardRandomSequence.h>
#include <vtkSmartPointer.h>
#include <vtkTransform.h>

#include <mitkDataStorage.h>
#include <mitkIOUtil.h>
#include <mitkTestingMacros.h>

#include <niftkAffineTransformDataNodeProperty.h>
#include <niftkCoordinateAxesData.h>
#include <niftkICPBasedRegistration.h>
#include <niftkVTKFunctions.h>

bool MatrixOK ( vtkMatrix4x4 * matrix )
{
  for (int i = 0 ; i < 4 ; i++ )
  {
    for ( int j = 0 ; j < 4 ; j++ )
    {
      if ( boost::math::isnan(matrix->GetElement(i,j)) )
      {
        return false;
      }
    }
  }
  return true;
}

int niftkICPBasedRegistrationTestRealData(int argc, char* argv[])
{
  if (argc != 5)
  {
    std::cerr << "Usage: niftkICPBasedRegistrationTestRealData points.vtp/.mps surface.vtp maxiterations numberOfPoints" << std::endl;
    std::cerr << " argc=" << argc << std::endl;
    for (int i = 0; i < argc; ++i)
    {
      std::cerr << " argv[" << i << "]=" << argv[i] << std::endl;
    }
    return EXIT_FAILURE;
  }

  int MaxIterations = atoi(argv[3]);
  int MaxLandmarks = atoi(argv[4]);

  // Read Fixed Points
  mitk::DataNode::Pointer fixednode = mitk::DataNode::New();
  mitk::PointSet::Pointer fixedPoints = mitk::PointSet::New();
  mitk::Surface::Pointer fixedSurface = mitk::Surface::New();

  try
  {
    fixedPoints = mitk::IOUtil::LoadPointSet(argv[1]);
    fixednode->SetData(fixedPoints);
  }
  catch (const mitk::Exception& e)
  {
    // try again, maybe its a surface
    int numberOfPoints = fixedPoints->GetSize();
    if ( numberOfPoints == 0  )
    {
      fixedSurface = mitk::IOUtil::LoadSurface(argv[1]);
      fixednode->SetData(fixedSurface);
    }
  }

  // Read Moving Surface
  mitk::DataNode::Pointer movingnode = mitk::DataNode::New();
  mitk::Surface::Pointer movingSurface = mitk::Surface::New();
  movingSurface = mitk::IOUtil::LoadSurface(argv[2]);
  movingnode->SetData(movingSurface);

  vtkSmartPointer<vtkMatrix4x4> resultMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
  niftk::ICPBasedRegistration::Pointer registerer = niftk::ICPBasedRegistration::New();
  registerer->SetMaximumIterations(MaxIterations);
  registerer->SetMaximumNumberOfLandmarkPointsToUse(MaxLandmarks);
  registerer->Update(fixednode, movingnode, *resultMatrix);
  std::cerr << *resultMatrix;
  MITK_TEST_CONDITION_REQUIRED(MatrixOK(resultMatrix), ".. Testing result matrix is a number");

  // Test TLS.
  std::vector<mitk::DataNode::Pointer> movingNodes;
  movingNodes.push_back(movingnode);
  std::vector<mitk::DataNode::Pointer> fixedNodes;
  fixedNodes.push_back(fixednode);
  registerer->SetTLSIterations(2);
  registerer->SetTLSPercentage(50);
  registerer->Update(fixedNodes, movingNodes, *resultMatrix);

  std::cerr << *resultMatrix;
  MITK_TEST_CONDITION_REQUIRED(MatrixOK(resultMatrix), ".. Testing result matrix is a number");

  return EXIT_SUCCESS;
}


#include <algorithm>
#include <cmath>
#include <iostream>
#include <sstream>
#include <string>
#include <random>
#include <fstream>
#include <thread>

#include "forest/map.hpp"
#include "forest/area.hpp"
#include "forest/pattern.hpp"

#include "common.hpp"
#include "location_finder_master.hpp"
#include "location_finder_worker.hpp"
#include "orientee.hpp"
#include "path_finder.hpp"

#include <boost/program_options.hpp>

std::string ppc::g_worldID;

void init_logger(const int rank);
bool parse_args(int argc, char* argv[], ppc::Map& map,
	boost::optional<ppc::index_pair>& startingPosition, boost::optional<ppc::Direction>& startingDirection,
	bool& randomize, bool& pathFinding, float& splitFactor, bool& statistics,
	ppc::PathFinder::EntranceSearching& searchMethod, int& numOfSearchLocations,
	const bool log);
bool init_mpi(ppc::mpi::communicator& world, ppc::mpi::communicator& workersComm, ppc::mpi::communicator& orienteeComm);
void write_path(const ppc::path& path, const std::string& filename);

int main(int argc, char *argv[])
{
	const auto startTime = std::chrono::steady_clock::now();

	ppc::Map map;
	boost::optional<ppc::index_pair> startingPosition;
	boost::optional<ppc::Direction> startingDirection;
	bool randomize = false;
	bool pathFiding = false;
	float splitFactor = 0.0f;
	bool statistics = true;
	ppc::PathFinder::EntranceSearching searchMethod = ppc::PathFinder::ANY_ENTRANCE;
	int numOfSearchLocations = 5;

	ppc::mpi::environment environment;
	ppc::mpi::communicator world;
	ppc::mpi::communicator workersComm;
	ppc::mpi::communicator orienteeComm;

	init_logger(world.rank());

	if (!init_mpi(world, workersComm, orienteeComm))
	{
		PPC_LOG(info) << "Program is shutting down...";
		return 1;
	}
	orienteeComm = world;

	try
	{
		if (!parse_args(argc, argv, map,
			startingPosition, startingDirection,
			randomize, pathFiding, splitFactor, statistics,
			searchMethod, numOfSearchLocations,
			world.rank() == 0))
		{
			PPC_LOG(info) << "Program is shutting down...";
			return 1;
		}
	}
	catch (const std::exception& e)
	{
		PPC_LOG(fatal) << "Parsing error: " << e.what();
		return 1;
	}

	//Statistics initializations
	boost::optional<ppc::Statistics> stats;
	if (statistics)
	{
		stats = ppc::Statistics{};
		const auto initializationEndTime = std::chrono::steady_clock::now();
		stats->initializationTime = std::chrono::duration_cast<std::chrono::milliseconds>(initializationEndTime - startTime);
	}

	//Find the current location.
	ppc::LocationOrientationPair locationSolution;
	if (world.rank() == 0)
	{
		ppc::LocationFinderMaster locationMaster{ workersComm, orienteeComm };
		locationSolution = locationMaster.run(map, randomize, stats);

		if (statistics)
		{
			const auto locationFindingEndTime = std::chrono::steady_clock::now();
			stats->locationFindingTime = std::chrono::duration_cast<std::chrono::milliseconds>(locationFindingEndTime - startTime);

			orienteeComm.send(1, ppc::tags::STATS, ppc::dummy<ppc::Direction>);
			orienteeComm.recv(ppc::mpi::any_source, ppc::mpi::any_tag, stats->startingLocation);
			orienteeComm.recv(ppc::mpi::any_source, ppc::mpi::any_tag, stats->startingOrientation);
			orienteeComm.recv(ppc::mpi::any_source, ppc::mpi::any_tag, stats->totalNumberOfMoves);
			orienteeComm.recv(ppc::mpi::any_source, ppc::mpi::any_tag, stats->totalNumberOfQueries);
		}

		const bool shouldNotStop = !pathFiding && statistics;
		if (!shouldNotStop)
		{
			orienteeComm.send(1, ppc::tags::STOP, ppc::dummy<ppc::Direction>);
			orienteeComm.send(1, 0, locationSolution);
		}
	}
	else if (world.rank() == 1)
	{
		ppc::Orientee orientee{ orienteeComm };
		ppc::path path = orientee.run(map, startingPosition, startingDirection);
		orienteeComm.recv(ppc::mpi::any_source, ppc::mpi::any_tag, locationSolution);

		write_path(path, "location_finding.path");
	}
	else
	{
		ppc::LocationFinderWorker locationWorker{ workersComm };
		locationSolution = locationWorker.run(map);
	}

	const auto location = locationSolution.first;
	const auto orientation = locationSolution.second;
	if (world.rank() != 1 && location.first == 0 && location.second == 0)
	{
		return 1;
	}

	decltype(std::chrono::steady_clock::now()) pathFindingStartTime;
	if (pathFiding)
	{
		//Path finding
		world.barrier();
		pathFindingStartTime = std::chrono::steady_clock::now();
		const auto startingY = location.second + 1;
		const auto requiredY = map.height() - 3;	//-2 for the border, - 1 for 0 indexing
		const auto pathFindingRows = requiredY - startingY + 2;	//TODO: come up with a better name

		if (world.rank() == 1)
		{
			//Recreate the orientee to use the world communicator and to start from
			//the previous state.
			ppc::Orientee orientee{ world };
			auto path = orientee.run(map, location, orientation);

			write_path(path, "path_finding.path");
		}

		ppc::index_type numOfPathfindingWorkers = 0;
		if (world.rank() != 1 && location.second < requiredY)
		{
			const auto startX = 0;
			const auto width = map.width();	//+2 for the border
			const auto numOfWorkers = static_cast<ppc::index_type>(workersComm.size());
			const auto workerID = static_cast<ppc::index_type>(workersComm.rank());

			ppc::Area mainArea{ startX, startingY - 1, pathFindingRows, width };

			if (mainArea.height > 3 * numOfWorkers)
			{
				PPC_LOG(info) << "Starting pathfinding with " << numOfWorkers << " workers";
				numOfPathfindingWorkers = numOfWorkers;
				std::vector<ppc::Area> areas;
				if (splitFactor != 0.0f)
				{
					areas = ppc::factor_split(mainArea, numOfWorkers, splitFactor);
				}
				else
				{
					areas = ppc::split(mainArea, numOfWorkers);
				}

				ppc::PathFinder pathFinder{ workersComm, world, workerID, numOfWorkers };
				if (workerID == 0u)
				{
					pathFinder.run(map, areas[workerID], locationSolution, searchMethod, numOfSearchLocations);
				}
				else
				{
					pathFinder.run(map, areas[workerID], {}, searchMethod, numOfSearchLocations);
				}
			}
			else if (world.rank() == 0)
			{
				PPC_LOG(info) << "Path finding area too small. Will only use 1 worker.";
				numOfPathfindingWorkers = 1;
				ppc::PathFinder pathFinder{ workersComm, world, workerID, 1 };
				pathFinder.run(map, mainArea, locationSolution, searchMethod, numOfSearchLocations);
			}
		}
		else if (world.rank() == 0)
		{
			PPC_LOG(info) << "No need for path finding.";
		}

		const bool isLastWorker = (numOfPathfindingWorkers > 1 && world.rank() == world.size() - 1)
			|| (numOfPathfindingWorkers == 1 && world.rank() == 0);
		if (!statistics && isLastWorker)
		{
			world.send(1, ppc::tags::STOP, ppc::dummy<ppc::Direction>);
		}
	}

	if (statistics && world.rank() != 1)
	{
		workersComm.barrier();

		if (world.rank() == 0)
		{
			PPC_LOG(info) << "Gathering statistical data...";

			//Run time stats.
			const auto endTime = std::chrono::steady_clock::now();
			stats->totalTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);

			//Num of moves + queries stats
			ppc::index_type additionalNumOfMoves;
			ppc::index_type additionalNumOfQueries;

			world.send(1, ppc::tags::STATS, ppc::dummy<ppc::Direction>);
			world.recv(ppc::mpi::any_source, ppc::mpi::any_tag, ppc::dummy<ppc::index_pair>);
			world.recv(ppc::mpi::any_source, ppc::mpi::any_tag, ppc::dummy<ppc::index_pair>);
			world.recv(ppc::mpi::any_source, ppc::mpi::any_tag, additionalNumOfMoves);
			world.recv(ppc::mpi::any_source, ppc::mpi::any_tag, additionalNumOfQueries);

			stats->totalNumberOfMoves += additionalNumOfMoves;
			stats->totalNumberOfQueries += additionalNumOfQueries;

			world.send(1, ppc::tags::STOP, ppc::dummy<ppc::Direction>);
			if (!pathFiding)
			{
				world.send(1, 0, locationSolution);
			}
			PPC_LOG(info) << "Stop signal sent to orientee";

			//Path finding stats
			stats->pathFinding = pathFiding;
			if (pathFiding)
			{
				const auto pathFindingEndTime = std::chrono::steady_clock::now();

				stats->numOfPathFindingMoves = additionalNumOfMoves;
				stats->pathFindingTime = std::chrono::duration_cast<std::chrono::milliseconds>(pathFindingEndTime - pathFindingStartTime);
			}

			//Other stats
			stats->numOfProcessors = world.size();
			stats->mapHeight = map.height();
			stats->mapWidth = map.width();
			for (auto i = 1; i < argc; ++i)
			{
				stats->startupOptions += argv[i];
				stats->startupOptions += " ";
			}

			std::ofstream statsFile{ "statistics.txt" };
			statsFile << *stats;
		}
	}

	PPC_LOG(info) << "Shutting down...";
	return 0;
}

void init_logger(const int rank)
{
	//http://www.boost.org/doc/libs/1_54_0/libs/log/doc/html/boost/log/add_console_lo_idp21543664.html
	namespace logging = boost::log;
	namespace keywords = logging::keywords;
	namespace expr = logging::expressions;
	namespace sinks = logging::sinks;

	auto format = expr::stream << "["
		<< expr::format_date_time<boost::posix_time::ptime>("TimeStamp", "%Y-%m-%d %H:%M:%S")
		<< "] <" << logging::trivial::severity
		<< "> - " << expr::smessage;

	//TODO: stop using g_worldID and insert it here
	logging::add_console_log(
		std::cout,
		keywords::format = format,
		keywords::auto_flush = true
	);
	logging::add_file_log(
		keywords::file_name = "process_" + std::to_string(rank) + "_log.txt",
		keywords::rotation_size = 10 * 1024 * 1024,
		keywords::time_based_rotation = sinks::file::rotation_at_time_point(0, 0, 0),
		keywords::format = format
	);
	logging::add_common_attributes();

#ifdef NDEBUG
	logging::core::get()->set_filter
	(
		logging::trivial::severity >= logging::trivial::info
	);
#endif
}

bool parse_args(int argc, char* argv[], ppc::Map& map, boost::optional<ppc::index_pair>& startingPosition,
	boost::optional<ppc::Direction>& startingDirection,
	bool& randomize, bool& pathFinding, float& splitFactor, bool& statistics,
	ppc::PathFinder::EntranceSearching& searchMethod, int& numOfSearchLocations,
	const bool log)
{
	namespace po = boost::program_options;

	po::options_description desc{ "Orientation In A Forest program options" };
	desc.add_options()
		("help,h", "Prints instructions on how to use the program")
		("map,m", po::value<std::string>(), "The name of the file containing the map. This is a required argument")
		("path_finding,p", "After finding the location, find the way to the bottom side of the map")
		("statistics,s", "Gathers statistics and benchmarks")
		("startingX,x", po::value<ppc::index_type>(), "Starting x position of the orientee")
		("startingY,y", po::value<ppc::index_type>(), "Starting y position of the orientee")
		("direction,d", po::value<int>(), "Starting orientation of the orientee(FORWARD=0, RIGHT=1, BACKWARDS=2, LEFT=3)")
		("random,r", "Indicates that the algorithm is allowed to use randomization in some areas, to guarantee a solution(if it exists)")
		("split_factor,f", po::value<float>(), "Factor used to when splitting the area for path finding(has to be 0 < factor < 1)")
		("entry_method,e", po::value<int>(), "0 = Any entrance(Default value), 1 = Min table cost entrance, 2 = Search n locations")
		("n_locations,n", po::value<int>(), "Number of entrances to be searched when using -e 2");

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

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

	if (vm.count("map"))
	{
		auto mapFilename = vm["map"].as<std::string>();
		std::ifstream inputFile{ mapFilename };
		if (inputFile)
		{
			if (log)
			{
				PPC_LOG(info) << "Reading the map...";
			}
			inputFile >> map;

			if (log)
			{
				PPC_LOG(info) << "Map successfully read. Map height: " << map.height() << ", Map width: " << map.width() << ".";
			}
		}
		else
		{
			if (log)
			{
				PPC_LOG(fatal) << "Invalid map file(" << mapFilename << ").";
			}
			return false;
		}
	}
	else
	{
		if (log)
		{
			PPC_LOG(fatal) << "Map filename is required! Use --help for more information.";
		}
		return false;
	}

	if (vm.count("startingX") ^ vm.count("startingY"))
	{
		PPC_LOG(fatal) << "You have to specify both x and y if you want to set the starting position.";
		return false;
	}

	if (vm.count("startingX"))
	{
		const auto x = vm["startingX"].as<ppc::index_type>();
		const auto y = vm["startingY"].as<ppc::index_type>();

		ppc::index_pair pos{ x, y };
		startingPosition = pos;
		if (log)
		{
			PPC_LOG(info) << "Starting position overriden: " << pos << ".";
		}
	}

	if (vm.count("direction"))
	{
		const auto asInteger = vm["direction"].as<int>();
		if (asInteger >= 0 && asInteger <= 3)
		{
			startingDirection = static_cast<ppc::Direction>(asInteger);
			if (log)
			{
				PPC_LOG(info) << "Starting direction overriden: " << *startingDirection;
			}
		}
		else
		{
			if (log)
			{
				PPC_LOG(fatal) << "Invalid direction";
			}
			return false;
		}
	}

	if (vm.count("random"))
	{
		randomize = true;
		if (log)
		{
			PPC_LOG(info) << "Randomization is on";
		}
	}
	else
	{
		randomize = false;
	}

	if (vm.count("path_finding"))
	{
		pathFinding = true;
		if (log)
		{
			PPC_LOG(info) << "Pathfinding is on";
		}
	}
	else
	{
		pathFinding = false;
	}

	if (vm.count("split_factor"))
	{
		const auto factor = vm["split_factor"].as<float>();
		if (factor <= 0 || factor >= 1)
		{
			if (log)
			{
				PPC_LOG(fatal) << "Invalid split factor! Use --help for more information.";
			}
			return false;
		}
		splitFactor = factor;
		if (log)
		{
			PPC_LOG(info) << "Split factor used: " << splitFactor;
		}
	}
	else
	{
		splitFactor = 0.0f;
	}

	if (vm.count("statistics"))
	{
		statistics = true;
		if (log)
		{
			PPC_LOG(info) << "Statistics are on";
		}
	}
	else
	{
		statistics = false;
	}

	if (vm.count("entry_method"))
	{
		const auto asInteger = vm["entry_method"].as<int>();
		if (asInteger >= 0 && asInteger <= 2)
		{
			searchMethod = static_cast<ppc::PathFinder::EntranceSearching>(asInteger);
			if (log)
			{
				PPC_LOG(info) << "Entry method: " << searchMethod;
			}
		}
		else
		{
			if (log)
			{
				PPC_LOG(fatal) << "Invalid entrance method";
			}
			return false;
		}
	}
	else
	{
		searchMethod = ppc::PathFinder::ANY_ENTRANCE;
	}

	if (vm.count("n_locations"))
	{
		numOfSearchLocations = vm["n_locations"].as<int>();
		if (numOfSearchLocations <= 0)
		{
			if (log)
			{
				PPC_LOG(fatal) << "n_locations taks as input a positive integer > 1";
			}
			return false;
		}
	}
	else if (searchMethod == ppc::PathFinder::N_SEARCH)
	{
		if (log)
		{
			PPC_LOG(fatal) << "To use N_SEARCH you also need to give the n_locations options";
		}
		return false;
	}

	return true;
}

bool init_mpi(ppc::mpi::communicator& world, ppc::mpi::communicator& workersComm, ppc::mpi::communicator& orienteeComm)
{
	PPC_LOG(info) << "Initializing mpi...";

	ppc::g_worldID = "World#" + std::to_string(world.rank()) + ": ";
	PPC_LOG(info) << "World initialized! World size: " << world.size();
	if (world.size() < 3)
	{
		PPC_LOG(fatal) << "The program requires at least 3 processes.";
		return false;
	}

	workersComm = world.split(world.rank() != 1);
	PPC_LOG(info) << "Workers communicator initialized!";

	orienteeComm = world.split(world.rank() <= 1);
	PPC_LOG(info) << "Orientee communicator initialized.";

	world.barrier();
	PPC_LOG(info) << "Program starting!";

	return true;
}

void write_path(const ppc::path& path, const std::string& filename)
{
	std::ofstream pathFile{ filename };
	if (!pathFile)
	{
		PPC_LOG(fatal) << "Cannot write path file(" << filename << ")";
		return;
	}

	for (const auto& point : path)
	{
		pathFile << point.first << " " << point.second << "\n";
	}
}