#ifndef STAN_MATH_PRIM_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP
#define STAN_MATH_PRIM_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/inv.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {
/**
 * Return the log of the binomial coefficient for the specified
 * arguments.
 *
 * The binomial coefficient, \f${N \choose n}\f$, read "N choose n", is
 * defined for \f$0 \leq n \leq N\f$ by
 *
 * \f${N \choose n} = \frac{N!}{n! (N-n)!}\f$.
 *
 * This function uses Gamma functions to define the log
 * and generalize the arguments to continuous N and n.
 *
 * \f$ \log {N \choose n}
 * = \log \ \Gamma(N+1) - \log \Gamma(n+1) - \log \Gamma(N-n+1)\f$.
 *
   \f[
   \mbox{binomial\_coefficient\_log}(x, y) =
   \begin{cases}
     \textrm{error} & \mbox{if } y > x \textrm{ or } y < 0\\
     \ln\Gamma(x+1) & \mbox{if } 0\leq y \leq x \\
     \quad -\ln\Gamma(y+1)& \\
     \quad -\ln\Gamma(x-y+1)& \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{binomial\_coefficient\_log}(x, y)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } y > x \textrm{ or } y < 0\\
     \Psi(x+1) & \mbox{if } 0\leq y \leq x \\
     \quad -\Psi(x-y+1)& \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{binomial\_coefficient\_log}(x, y)}{\partial y} =
   \begin{cases}
     \textrm{error} & \mbox{if } y > x \textrm{ or } y < 0\\
     -\Psi(y+1) & \mbox{if } 0\leq y \leq x \\
     \quad +\Psi(x-y+1)& \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]
 *
 * @param N total number of objects.
 * @param n number of objects chosen.
 * @return log (N choose n).
 */
template <typename T_N, typename T_n>
inline return_type_t<T_N, T_n> binomial_coefficient_log(const T_N N,
                                                        const T_n n) {
  using std::log;
  const double CUTOFF = 1000;
  if (N - n < CUTOFF) {
    const T_N N_plus_1 = N + 1;
    return lgamma(N_plus_1) - lgamma(n + 1) - lgamma(N_plus_1 - n);
  } else {
    return_type_t<T_N, T_n> N_minus_n = N - n;
    const double one_twelfth = inv(12);
    return multiply_log(n, N_minus_n) + multiply_log((N + 0.5), N / N_minus_n)
           + one_twelfth / N - n - one_twelfth / N_minus_n - lgamma(n + 1);
  }
}

}  // namespace math
}  // namespace stan
#endif


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

 /*
 * \author Christian Bouchard
 */

#ifndef HULLOVERLAP_HPP
#define HULLOVERLAP_HPP

#include <iostream>
#include <cstdint>

#include <vector>

#include <utility>      // std::pair, std::make_pair

#include <limits>       // std::numeric_limits

#include <pcl/common/common_headers.h>

#include <pcl/point_types.h>
#include <pcl/surface/concave_hull.h>

#include <pcl/PointIndices.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>

#include <pcl/segmentation/extract_polygonal_prism_data.h>

#include <Eigen/Dense>
#include <Eigen/Geometry> // For cross product

//-----------------------------------------------------------------------------------
// Andrew's monotone chain convex hull algorithm
// Adapted from
// https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain#C++

typedef float coord_t;      // coordinate type (Use float because pcl::PointXYZ's coordinates are float)
typedef double coord2_t;    // must be big enough to hold 2*max(|coordinate|)^2

struct PointAndrews
{
	coord_t x;
    coord_t y;
    uint64_t index;

	bool operator <( const PointAndrews &p ) const
    {
		return x < p.x || (x == p.x && y < p.y);
	}
};

// 3D cross product of OA and OB vectors, (i.e z-component of their "2D" cross product,
// but remember that it is not defined in "2D").
// Returns a positive value, if OAB makes a counter-clockwise turn,
// negative for clockwise turn, and zero if the points are collinear.
coord2_t cross( const PointAndrews &O, const PointAndrews &A, const PointAndrews &B )
{
	return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x);
}

// Returns a list of points on the convex hull in counter-clockwise order.
// Note: the last point in the returned list is the same as the first one.
// CB: vector points is modified by getting sorted.
void AndrewsConvex_hull( std::vector<PointAndrews> & hull, std::vector<PointAndrews> & points )
{
	size_t n = points.size(), k = 0;

    hull.clear();

    if ( n <= 3 )
    {
        hull.reserve( n );

        for ( int count = 0; count < n; count++ )
            hull.push_back( points[ count ] );

        return;
    }

    hull.resize( 2 * n );

	// Sort points lexicographically
	sort(points.begin(), points.end());

	// Build lower hull
	for (size_t i = 0; i < n; ++i)
    {
		while (k >= 2 && cross(hull[k-2], hull[k-1], points[i]) <= 0)
            k--;

		hull[k++] = points[i];
	}

	// Build upper hull
	for (size_t i = n-1, t = k+1; i > 0; --i)
    {
		while (k >= t && cross(hull[k-2], hull[k-1], points[i-1]) <= 0)
            k--;

		hull[k++] = points[i-1];
	}

	hull.resize(k-1);

}

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

class HullOverlap
{

public:

	/**
	* Creates a HullOverlap
	*
	* @param line1In Point cloud for line #1
	* @param line2In Point cloud for line #2
    * @param a projection plane coefficient 'a' in ax + by + cz + d = 0
    * @param b projection plane coefficient 'b' in ax + by + cz + d = 0
    * @param c projection plane coefficient 'c' in ax + by + cz + d = 0
    * @param d projection plane coefficient 'd' in ax + by + cz + d = 0
    * @param hullMethod Method to find the hulls, possible values: "PCL ConcaveHull", "Andrew's"
    * @param alpha1 Concave hull computation parameter to use with line #1
    * @param alpha2 Concave hull computation parameter to use with line #2
	*/
    HullOverlap( pcl::PointCloud<pcl::PointXYZ>::ConstPtr line1In,
                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr line2In,
                    double a, double b, double c, double d, std::string hullMethod = "Andrew's",
                    double alphaLine1 = 1.0, double alphaLine2 = 1.0 )
                    :   line1( line1In ), line2( line2In ),
                        a( a ), b( b ), c( c ), d( d ),
                        hullMethod( hullMethod ),

                        alphaLine1( alphaLine1 ), alphaLine2( alphaLine2 ),

                        coefficients ( new pcl::ModelCoefficients() ),

                        line1InPlane (new pcl::PointCloud<pcl::PointXYZ>),
                        line2InPlane (new pcl::PointCloud<pcl::PointXYZ>),

                        line1InPlane2D (new pcl::PointCloud<pcl::PointXYZ>),
                        line2InPlane2D (new pcl::PointCloud<pcl::PointXYZ>),

                        hull1Vertices (new pcl::PointCloud<pcl::PointXYZ>),
                        hull2Vertices (new pcl::PointCloud<pcl::PointXYZ>),

                       // Initialize to dummy values
                       vector1( 1, 0, 0 ), vector2( 0, 1, 0 ), refPoint( 0.0, 0.0, 0.0 )

    {
        coefficients->values.resize(4);
        coefficients->values[0] = a;
        coefficients->values[1] = b;
        coefficients->values[2] = c;
        coefficients->values[3] = d;

        if ( hullMethod != "PCL ConcaveHull" && hullMethod != "Andrew's" )
        {
            std::cerr << "\n\nHullOverlap::HullOverlap(), method \""<<  hullMethod
                << "\" is not a valid method to find the hull.\n\n" << std::endl;
            exit( 1 );
        }

    }

	/**
	* Returns a pair with the number of points in line #1 and in line #2 that are in the overlap area of the two lines.
	* The points are place in the point clouds pointed to by line1InBothHull and line2InBothHull
    *
	* @param[out] line1InBothHull Point cloud of points in line #1 in the overlap area of the two lines
	* @param[out] line2InBothHull Point cloud of points in line #2 in the overlap area of the two lines
	*/
    std::pair< uint64_t, uint64_t > computePointsInBothHulls( pcl::PointCloud<pcl::PointXYZ>::Ptr line1InBothHull,
                                                              pcl::PointCloud<pcl::PointXYZ>::Ptr line2InBothHull )
    {
        return computeHullsAndPointsInBothHulls( line1InBothHull, line2InBothHull, true );
    }

	/**
	* Returns a pair with the number of points in line #1 and in line #2 that are in the overlap area of the two lines,
	* the points are place in the point clouds pointed to by line1InBothHull and line2InBothHull
    *
	* @param[out] line1InBothHull Point cloud of points in line #1 in the overlap area of the two lines
	* @param[out] line2InBothHull Point cloud of points in line #2 in the overlap area of the two lines
    * @param[in] line2InBothHull minimalMemory bool variable, true to specify to try and minimize the memory usage
	*/
    // Put back to public to be able to get indices to the points
    std::pair< uint64_t, uint64_t > computeHullsAndPointsInBothHulls(
                                                pcl::PointCloud<pcl::PointXYZ>::Ptr line1InBothHull = nullptr,
                                                pcl::PointCloud<pcl::PointXYZ>::Ptr line2InBothHull = nullptr,
                                                const bool minimalMemory = false )
    {

        if ( line1InBothHull != nullptr )
            line1InBothHull->clear();

        if ( line2InBothHull != nullptr )
            line2InBothHull->clear();

        std::cout << "\nProjecting line 1 in plane\n" << std::endl;

        // Project line 1 in plane
        createCloudFromProjectionInPlane( line1, line1InPlane );

        std::cout << "line1InPlane->points.size(): " << line1InPlane->points.size() << "\n" << std::endl;

        computeTwoVectorsAndRefPoint();

        std::cout << "\nExpressing points of line 1 in the projection plane using a 2D coordinate system\n" << std::endl;

        createCloudInPlane2D( line1InPlane, line1InPlane2D );

        if ( minimalMemory )
        {
            // Delete the dynamically allocated memory
            line1InPlane.reset();
            line1InPlane = nullptr;
        }

        std::cout << "line1InPlane2D->points.size(): " << line1InPlane2D->points.size() << "\n" << std::endl;

        std::cout << "\nProjecting line 2 in plane\n" << std::endl;

        // Project line 2 in plane
        createCloudFromProjectionInPlane( line2, line2InPlane );

        std::cout << "line2InPlane->points.size(): " << line2InPlane->points.size() << "\n" << std::endl;

        std::cout << "\nExpressing points of line 2 in the projection plane using a 2D coordinate system\n" << std::endl;

        createCloudInPlane2D( line2InPlane, line2InPlane2D );

        if ( minimalMemory )
        {
            // Delete the dynamically allocated memory
            line2InPlane.reset();
            line2InPlane = nullptr;
        }

        std::cout << "line2InPlane2D->points.size(): " << line2InPlane2D->points.size() << "\n" << std::endl;

        // const std::string method = "Andrew's";

        if ( hullMethod == "PCL ConcaveHull" )
        {
            //http://www.pointclouds.org/documentation/tutorials/hull_2d.php

            std::cout << "\nFinding Hull 1\n" << std::endl;

            // Create a Concave Hull for line 1
            computeVerticesOfConcaveHull( line1InPlane2D, alphaLine1, hull1Vertices, hull1PointIndices, ! minimalMemory );

            std::cout << "Finding Hull 2\n" << std::endl;

            // Create a Concave Hull for line 2
            computeVerticesOfConcaveHull( line2InPlane2D, alphaLine2, hull2Vertices, hull2PointIndices, ! minimalMemory );
        }
        else if ( hullMethod == "Andrew's" )
        {
            std::cout << "\nFinding Hull 1\n" << std::endl;

            // Create a Hull for line 1
            computeVerticesOfHullAndrews( line1InPlane2D, hull1Vertices, hull1PointIndices, ! minimalMemory );

            std::cout << "Finding Hull 2\n" << std::endl;

            // Create a Concave Hull for line 2
            computeVerticesOfHullAndrews( line2InPlane2D, hull2Vertices, hull2PointIndices, ! minimalMemory );
        }
        else
        {
            std::cerr << "\n\nHullOverlap::computeHullsAndPointsInBothHulls(), method \""<<  hullMethod
                << "\" is not a valid method to find the hull.\n\n" << std::endl;
            exit( 1 );
        }

        std::cout << "hull1Vertices->points.size(): " << hull1Vertices->points.size() << "\n"
            << "hull2Vertices->points.size(): " << hull2Vertices->points.size() << "\n" << std::endl;

        // If the hulls of the lines where found correctly, points of line 1 are within the hull of line 1.
        // So only need to check that a point of line 1 is part of hull 2 to know that it is part of both hulls.
        // Same idea for points of line 2.

        if ( line1InBothHull != nullptr && line2InBothHull != nullptr )
        {
            if ( minimalMemory )
            {
                std::cout << "Finding points of Line 1 inside Hull 2\n\n" << std::endl;

                findPointsInHullOnlyPoints( line1, line1InPlane2D, line1InBothHull, hull2Vertices );

                // Delete the dynamically allocated memory
                line1InPlane2D.reset();
                line1InPlane2D = nullptr;

                hull2Vertices.reset();
                hull2Vertices = nullptr;

                std::cout << "Finding points of Line 2 inside Hull 1\n\n" << std::endl;

                findPointsInHullOnlyPoints( line2, line2InPlane2D, line2InBothHull, hull1Vertices );

                // Delete the dynamically allocated memory
                line2InPlane2D.reset();
                line2InPlane2D = nullptr;

                hull1Vertices.reset();
                hull1Vertices = nullptr;

                std::cout << "line1InBothHull->points.size(): " << line1InBothHull->points.size() << "\n"
                    << "line2InBothHull->points.size(): " << line2InBothHull->points.size() << "\n" << std::endl;

                return std::make_pair( line1InBothHull->size(), line2InBothHull->size() );

            }
            else
            {

                std::cout << "Finding points of Line 1 inside Hull 2 (and the indices)\n\n" << std::endl;

                findPointsInHull( line1, line1InPlane2D, line1InBothHull, line1InBothHullPointIndices, hull2Vertices );

                std::cout << "Finding points of Line 2 inside Hull 1 (and the indices)\n\n" << std::endl;

                findPointsInHull( line2, line2InPlane2D, line2InBothHull, line2InBothHullPointIndices, hull1Vertices );

                std::cout << "line1InBothHull->points.size(): " << line1InBothHull->points.size() << "\n"
                    << "line2InBothHull->points.size(): " << line2InBothHull->points.size() << "\n" << std::endl;

                return std::make_pair( line1InBothHull->size(), line2InBothHull->size() );

            }

        }
        else
        {
            std::cout << "Finding indices of points of Line 1 inside Hull 2\n\n" << std::endl;

            findPointsInHullOnlyPointIndices( line1InPlane2D, line1InBothHullPointIndices, hull2Vertices );

            std::cout << "Finding indices of points of Line 2 inside Hull 1\n\n" << std::endl;

            findPointsInHullOnlyPointIndices( line2InPlane2D, line2InBothHullPointIndices, hull1Vertices );

            std::cout << "line1InBothHullPointIndices.size(): " << line1InBothHullPointIndices.size() << "\n"
                << "line2InBothHullPointIndices.size(): " << line2InBothHullPointIndices.size() << "\n" << std::endl;

            return std::make_pair( line1InBothHullPointIndices.size(), line2InBothHullPointIndices.size() );
        }

    }

    // Uncommented to get access to the point indices
    // const std::vector< uint64_t > * getConstPtrlineInBothHullPointIndices( const bool isLine1 )
    const std::vector< uint64_t > * getConstPtrlineInBothHullPointIndices( const int lineNumber ) const
    {

        if ( lineNumber < 0 || lineNumber > 1 )
        {
            std::cout << "\n\n----- Function HullOverlap::getConstPtrlineInBothHullPointIndices(): lineNumber parameter: "
                << lineNumber << ".\nIt must be either 0 or 1. Returning nullptr\n" << std::endl;
            return nullptr;
        }

        if ( lineNumber == 0 )
            return & ( line1InBothHullPointIndices );
        else
            return & ( line2InBothHullPointIndices );

        // if ( isLine1 )
        //     return & ( line1InBothHullPointIndices );
        // else
        //     return & ( line2InBothHullPointIndices );
    }

    // pcl::PointCloud< pcl::PointXYZ >::ConstPtr getConstPtrlineInPlane2D( const bool isLine1 )
    pcl::PointCloud< pcl::PointXYZ >::ConstPtr getConstPtrlineInPlane2D( const int lineNumber  ) const
    {

        if ( lineNumber < 0 || lineNumber > 1 )
        {
            std::cout << "\n\n----- Function HullOverlap::getConstPtrlineInPlane2D(): lineNumber parameter: "
                << lineNumber << ".\nIt must be either 0 or 1. Returning nullptr\n" << std::endl;
            return nullptr;
        }

        if ( lineNumber == 0 )
            return line1InPlane2D;
        else
            return line2InPlane2D;

        // if ( isLine1 )
        //     return line1InPlane2D;
        // else
        //     return line2InPlane2D;
    }

    pcl::PointCloud< pcl::PointXYZ >::ConstPtr getConstPtrlineInPlane3D( const int lineNumber  ) const
    {

        if ( lineNumber < 0 || lineNumber > 1 )
        {
            std::cout << "\n\n----- Function HullOverlap::getConstPtrlineInPlane3D(): lineNumber parameter: "
                << lineNumber << ".\nIt must be either 0 or 1. Returning nullptr\n" << std::endl;
            return nullptr;
        }

        if ( lineNumber == 0 )
            return line1InPlane;
        else
            return line2InPlane;
    }

    const std::vector< int > * getConstPtrVerticesIndices( const int lineNumber ) const
    {
        if ( lineNumber < 0 || lineNumber > 1 )
        {
            std::cout << "\n\n----- Function HullOverlap::getConstPtrVerticesIndices(): lineNumber parameter: "
                << lineNumber << ".\nIt must be either 0 or 1. Returning nullptr\n" << std::endl;
            return nullptr;
        }

        if ( lineNumber == 0 )
            return & ( hull1PointIndices.indices );
        else
            return & ( hull2PointIndices.indices );
    }

    bool getMinMaxPointsInOverlapPlane2D( pcl::PointXYZ & minPt, pcl::PointXYZ &maxPt )
    {
        bool OK = false;

        // if there are points in both lines in the overlap
        if ( line1InBothHullPointIndices.size() > 0 && line2InBothHullPointIndices.size() > 0 )
        {

            double xMin = std::numeric_limits<double>::max();
            double xMax = std::numeric_limits<double>::min();

            double yMin = std::numeric_limits<double>::max();
            double yMax = std::numeric_limits<double>::min();

            for ( uint64_t count = 0; count < line1InBothHullPointIndices.size(); count++ )
            {
                if ( line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].x < xMin )
                    xMin = line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].x;

                if ( line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].x > xMax )
                    xMax = line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].x;

                if ( line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].y < yMin )
                    yMin = line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].y;

                if ( line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].y > yMax )
                    yMax = line1InPlane2D->points[ line1InBothHullPointIndices[ count ] ].y;
            }

            for ( uint64_t count = 0; count < line2InBothHullPointIndices.size(); count++ )
            {
                if ( line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].x < xMin )
                    xMin = line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].x;

                if ( line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].x > xMax )
                    xMax = line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].x;

                if ( line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].y < yMin )
                    yMin = line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].y;

                if ( line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].y > yMax )
                    yMax = line2InPlane2D->points[ line2InBothHullPointIndices[ count ] ].y;
            }

            minPt.x = xMin;
            minPt.y = yMin;
            minPt.z = 0;

            maxPt.x = xMax;
            maxPt.y = yMax;
            maxPt.z = 0;

            OK = true;

        }

        return OK;
    }

    bool getMinMaxPointsInOverlapPlane3D( pcl::PointXYZ & minPt, pcl::PointXYZ &maxPt )
    {
        bool OK = false;

        // if there are points in both lines in the overlap
        if ( line1InBothHullPointIndices.size() > 0 && line2InBothHullPointIndices.size() > 0 )
        {

            double xMin = std::numeric_limits<double>::max();
            double xMax = std::numeric_limits<double>::min();

            double yMin = std::numeric_limits<double>::max();
            double yMax = std::numeric_limits<double>::min();

            double zMin = std::numeric_limits<double>::max();
            double zMax = std::numeric_limits<double>::min();

            for ( uint64_t count = 0; count < line1InBothHullPointIndices.size(); count++ )
            {
                if ( line1InPlane->points[ line1InBothHullPointIndices[ count ] ].x < xMin )
                    xMin = line1InPlane->points[ line1InBothHullPointIndices[ count ] ].x;

                if ( line1InPlane->points[ line1InBothHullPointIndices[ count ] ].x > xMax )
                    xMax = line1InPlane->points[ line1InBothHullPointIndices[ count ] ].x;

                if ( line1InPlane->points[ line1InBothHullPointIndices[ count ] ].y < yMin )
                    yMin = line1InPlane->points[ line1InBothHullPointIndices[ count ] ].y;

                if ( line1InPlane->points[ line1InBothHullPointIndices[ count ] ].y > yMax )
                    yMax = line1InPlane->points[ line1InBothHullPointIndices[ count ] ].y;

                if ( line1InPlane->points[ line1InBothHullPointIndices[ count ] ].z < zMin )
                    zMin = line1InPlane->points[ line1InBothHullPointIndices[ count ] ].z;

                if ( line1InPlane->points[ line1InBothHullPointIndices[ count ] ].z > zMax )
                    zMax = line1InPlane->points[ line1InBothHullPointIndices[ count ] ].z;

            }

            for ( uint64_t count = 0; count < line2InBothHullPointIndices.size(); count++ )
            {
                if ( line2InPlane->points[ line2InBothHullPointIndices[ count ] ].x < xMin )
                    xMin = line2InPlane->points[ line2InBothHullPointIndices[ count ] ].x;

                if ( line2InPlane->points[ line2InBothHullPointIndices[ count ] ].x > xMax )
                    xMax = line2InPlane->points[ line2InBothHullPointIndices[ count ] ].x;

                if ( line2InPlane->points[ line2InBothHullPointIndices[ count ] ].y < yMin )
                    yMin = line2InPlane->points[ line2InBothHullPointIndices[ count ] ].y;

                if ( line2InPlane->points[ line2InBothHullPointIndices[ count ] ].y > yMax )
                    yMax = line2InPlane->points[ line2InBothHullPointIndices[ count ] ].y;

                if ( line2InPlane->points[ line2InBothHullPointIndices[ count ] ].z < zMin )
                    zMin = line2InPlane->points[ line2InBothHullPointIndices[ count ] ].z;

                if ( line2InPlane->points[ line2InBothHullPointIndices[ count ] ].z > zMax )
                    zMax = line2InPlane->points[ line2InBothHullPointIndices[ count ] ].z;

            }

            minPt.x = xMin;
            minPt.y = yMin;
            minPt.z = zMin;

            maxPt.x = xMax;
            maxPt.y = yMax;
            maxPt.z = zMax;

            OK = true;

        }

        return OK;
    }

    uint64_t getNbPointsInOverlap( const int lineNumber ) const
    {
        if ( lineNumber < 0 || lineNumber > 1 )
        {
            std::cout << "\n\n----- Function HullOverlap::getNbPointsInOverlap(): lineNumber parameter: "
                << lineNumber << ".\nIt must be either 0 or 1. Returning 0\n" << std::endl;
            return 0;
        }

        if ( lineNumber == 0 )
            return line1InBothHullPointIndices.size();
        else
            return line2InBothHullPointIndices.size();
    }

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

private:

    // These functions were public when wanted to look at details of projection, etc
    // pcl::PointCloud<pcl::PointXYZ>::ConstPtr getConstPtrLineInPlane( const bool isLine1 )
    // {
    //     if ( isLine1 )
    //         return line1InPlane;
    //     else
    //         return line2InPlane;
    // }

	/**
	* Computes the projection of a point cloud onto a plane
    *
    * @param[in] cloudIn Point cloud to project on the plane
    * @param[out] cloudOut Point cloud resulting from the projection
	*/
    void createCloudFromProjectionInPlane( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut )
    {
        cloudOut->clear();
        cloudOut->reserve( cloudIn->points.size() );

        // Create the filtering object
        pcl::ProjectInliers<pcl::PointXYZ> proj;
        proj.setModelType( pcl::SACMODEL_PLANE );

        proj.setInputCloud( cloudIn );
        proj.setModelCoefficients( coefficients );

        proj.filter( *cloudOut );
    }

	/**
	* Computes two vectors and sets a reference point used to express point positions on the
    * projection plane using only two dimensions
	*/
    void computeTwoVectorsAndRefPoint()
    {

        // Two vectors and a reference point to span the projection plane
        // so that points in the projection plane can be expressed in
        // a coordinate system with vector1, vector2, and refPoint.

        refPoint = line1InPlane->points[ 0 ];

        // Vector #1: from first point in line to last point in line, normalized

        const uint64_t nbPointLine1 = line1InPlane->points.size();

        vector1 << line1InPlane->points[ nbPointLine1 - 1 ].x - line1InPlane->points[ 0 ].x,
                    line1InPlane->points[ nbPointLine1 - 1 ].y - line1InPlane->points[ 0 ].y,
                    line1InPlane->points[ nbPointLine1 - 1 ].z - line1InPlane->points[ 0 ].z;

        std::cout << "vector1 before normalization:\n" << vector1 << "\n\n";

        vector1 = vector1 / vector1.norm();

        std::cout << "vector1 after normalization:\n" << vector1 << "\n\n";

        Eigen::Vector3d normalToPlane;

        normalToPlane <<  a, b, c;

        // Vector #2: perpendicular to the normal to the plane and to vector #1
        vector2 = normalToPlane.cross( vector1 );

        std::cout << "vector2 before normalization:\n" << vector2 << "\n\n";

        vector2 = vector2 / vector2.norm();

        std::cout << "vector2 after normalization:\n" << vector2 << "\n\n";

        // Sanity check
        std::cout << "vector1 dot vector2: " << vector1.dot( vector2 ) << "    (should be 0)\n\n";
    }

	/**
	* Computes a 2D representation of points on the projection plane
    *
    * @param[in] cloudIn Point cloud on the projection plane expressed in 3D
    * @param[out] cloudOut Point cloud on the projection plane expressed in 2D
	*/
    void createCloudInPlane2D( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut )
    {
        // Build a point cloud where points in the projection plane are expressed in
        // the coordinate system with vector1, vector2, and refPoint.

        cloudOut->clear();
        cloudOut->reserve( cloudIn->points.size() );

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            pcl::PointXYZ point;

            // projection along vector 1
            point.x = ( cloudIn->points[ count ].x - refPoint.x ) * vector1( 0 )
                        + ( cloudIn->points[ count ].y - refPoint.y ) * vector1( 1 )
                        + ( cloudIn->points[ count ].z - refPoint.z ) * vector1( 2 );

            // projection along vector 2
            point.y = ( cloudIn->points[ count ].x - refPoint.x ) * vector2( 0 )
                        + ( cloudIn->points[ count ].y - refPoint.y ) * vector2( 1 )
                        + ( cloudIn->points[ count ].z - refPoint.z ) * vector2( 2 );

            point.z = 0;

            cloudOut->push_back( point );

        }
    }

	/**
	* Computes the vertices of a concave hull for points on the projection plane
    *
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[in] alpha Concave hull computation parameter to use
    * @param[out] hullVertices Computed vertices of the concave hull
    * @param[out] hullPointIndices Indices of the points in cloudIn making up the hull
    * @param[in] keepInformation bool variable, true to specify to put the indices of the points in cloudIn in hullPointIndices
	*/
    void computeVerticesOfConcaveHull( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                        const double alpha,
                                        pcl::PointCloud<pcl::PointXYZ>::Ptr hullVertices,
                                        pcl::PointIndices & hullPointIndices, const bool keepInformation = true )
    {
        hullVertices->clear();

        pcl::ConcaveHull<pcl::PointXYZ> concaveHull;

        if ( keepInformation )
            concaveHull.setKeepInformation( true ); // To be able to use function getHullPointIndices()

        concaveHull.setInputCloud( cloudIn );
        concaveHull.setAlpha( alpha );
        concaveHull.reconstruct( * hullVertices );

        if ( keepInformation )
            // Get indices of points making the hull
            concaveHull.getHullPointIndices( hullPointIndices );
    }

	/**
	* Computes the vertices of a hull for points on the projection plane, using Andrew's monotone chain
    *
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[out] hullVertices Computed vertices of the concave hull
    * @param[out] hullPointIndices Indices of the points in cloudIn making up the hull
    * @param[in] keepInformation bool variable, true to specify to put the indices of the points in cloudIn in hullPointIndices
	*/
    void computeVerticesOfHullAndrews( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                        pcl::PointCloud<pcl::PointXYZ>::Ptr hullVertices,
                                        pcl::PointIndices & hullPointIndices, const bool keepInformation = true )
    {

        std::cout << "\nBefore building vector for Andrews\n" << std::endl;

        std::vector< PointAndrews > points;
        points.reserve( cloudIn->size() );

        for ( uint64_t count = 0; count < cloudIn->size(); count++ )
        {
            PointAndrews point;

            point.x = cloudIn->points[ count ].x;
            point.y = cloudIn->points[ count ].y;
            point.index = count;

            points.push_back( point );
        }

        std::cout << "\nBefore calling Andrews\n" << std::endl;

        std::vector< PointAndrews > hullAndrews;
        AndrewsConvex_hull( hullAndrews, points );

        hullVertices->clear();
        hullVertices->reserve( hullAndrews.size() );

        std::cout << "\nAfter calling Andrews\n" << std::endl;

        for ( uint64_t count = 0; count < hullAndrews.size(); count++ )
        {
            pcl::PointXYZ point;

            point.x = hullAndrews[ count ].x;
            point.y = hullAndrews[ count ].y;
            point.z = 0;
            hullVertices->push_back( point );

        }

        if ( keepInformation )
        {
            // Get indices of points making the hull

            hullPointIndices.indices.clear();
            hullPointIndices.indices.reserve( hullAndrews.size() );

            for ( uint64_t count = 0; count < hullAndrews.size(); count++ )
                hullPointIndices.indices.push_back( static_cast< int >( hullAndrews[ count ].index ) );

        }

    }

	/**
	* Find points that are within a concave hull.
    * Provides the points and their indices within the original line
    *
    * @param[in] lineOriginal Point cloud of points on the line
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[out] cloudOut Point cloud of points on the line that are within the hull
    * @param[out] indexPointInHull Indices of the points on the line that are within the hull
    * @param[in] hullVertices Vertices of the concave hull
	*/
    void findPointsInHull( pcl::PointCloud<pcl::PointXYZ>::ConstPtr lineOriginal,
                                pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut,
                                std::vector< uint64_t > & indexPointInHull,
                                pcl::PointCloud<pcl::PointXYZ>::ConstPtr hullVertices )
    {
        cloudOut->clear();
        indexPointInHull.clear();

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            if ( pcl::isXYPointIn2DXYPolygon( cloudIn->points[ count ], *hullVertices ) )
            {
                cloudOut->push_back( lineOriginal->points[ count ] );
                indexPointInHull.push_back( count );
            }
        }

    }

	/**
	* Find indices of points that are within a concave hull.
    *
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[out] indexPointInHull Indices of the points on the line that are within the hull
    * @param[in] hullVertices Vertices of the concave hull
	*/
    void findPointsInHullOnlyPointIndices( pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                    std::vector< uint64_t > & indexPointInHull,
                                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr hullVertices )
    {
        indexPointInHull.clear();

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            if ( pcl::isXYPointIn2DXYPolygon( cloudIn->points[ count ], *hullVertices ) )
                indexPointInHull.push_back( count );
        }

    }

	/**
	* Find points that are within a concave hull.
    *
    * @param[in] lineOriginal Point cloud of points on the line
    * @param[in] cloudIn Point cloud on the projection plane expressed in 2D
    * @param[out] cloudOut Point cloud of points on the line that are within the hull
    * @param[in] hullVertices Vertices of the concave hull
	*/
    void findPointsInHullOnlyPoints( pcl::PointCloud<pcl::PointXYZ>::ConstPtr lineOriginal,
                                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudIn,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut,
                                    pcl::PointCloud<pcl::PointXYZ>::ConstPtr hullVertices )
    {
        cloudOut->clear();

        for ( uint64_t count = 0; count < cloudIn->points.size(); count++ )
        {
            if ( pcl::isXYPointIn2DXYPolygon( cloudIn->points[ count ], *hullVertices ) )
                cloudOut->push_back( lineOriginal->points[ count ] );
        }

    }

// ----------------------------- Variables ------------------------------------------------

    /**Point cloud for line #1*/
    const pcl::PointCloud<pcl::PointXYZ>::ConstPtr line1;

    /**Point cloud for line #2*/
    const pcl::PointCloud<pcl::PointXYZ>::ConstPtr line2;

    /**Projection plane coefficient 'a' in ax + by + cz + d = 0*/
    const double a;

    /**Projection plane coefficient 'b' in ax + by + cz + d = 0*/
    const double b;

    /**Projection plane coefficient 'c' in ax + by + cz + d = 0*/
    const double c;

    /**Projection plane coefficient 'd' in ax + by + cz + d = 0*/
    const double d;

    //** Method to find the hulls, possible values: "PCL ConcaveHull", "Andrew's"*/
    std::string hullMethod;

    /**Concave hull computation parameter to use with line #1*/
    double alphaLine1; // Alpha value to compute the concave hull for line #1

    /**Concave hull computation parameter to use with line #2*/
    double alphaLine2; // Alpha value to compute the concave hull for line #2

    /**Coefficients for the plane, ax + by + cz + d = 0 */
    pcl::ModelCoefficients::Ptr coefficients;

    /**Point cloud of the projection of line #1 on the plane, expressed in 3D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line1InPlane;

    /**Point cloud of the projection of line #2 on the plane, expressed in 3D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line2InPlane;

    /**Point cloud of the projection of line #1 on the plane, expressed in 2D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line1InPlane2D;

    /**Point cloud of the projection of line #2 on the plane, expressed in 2D*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr line2InPlane2D;

    /**Vertices of the concave hull for line #1*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull1Vertices;

    /**Vertices of the concave hull for line #2*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull2Vertices;

    /**Indices of the points in line #1 whose projection on the plane makes up its hull*/
    pcl::PointIndices hull1PointIndices;

    /**Indices of the points in line #2 whose projection on the plane makes up its hull*/
    pcl::PointIndices hull2PointIndices;

    /**Indices of the points in line #1 that are within both hulls*/
    std::vector< uint64_t > line1InBothHullPointIndices;

    /**Indices of the points in line #2 that are within both hulls*/
    std::vector< uint64_t > line2InBothHullPointIndices;

    /**First computed orthonormal vector used to express points on the projection plane in 2D*/
    Eigen::Vector3d vector1;

    /**Second computed orthonormal vector used to express points on the projection plane in 2D*/
    Eigen::Vector3d vector2;

    /**Referenced point used to express points on the projection plane in 2D*/
    pcl::PointXYZ refPoint;

};

#endif


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MATRIX_TRANSFORMERS_HPP
#define BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MATRIX_TRANSFORMERS_HPP

#include <cstddef>

// Remove the ublas checking, otherwise the inverse might fail
// (while nothing seems to be wrong)
#define BOOST_UBLAS_TYPE_CHECK 0

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

#if defined(__clang__)
// Avoid warning about unused UBLAS function: boost_numeric_ublas_abs
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-function"
#endif

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

#if defined(__clang__)
#pragma clang diagnostic pop
#endif

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace strategy { namespace transform
{

/*!
\brief Affine transformation strategy in Cartesian system.
\details The strategy serves as a generic definition of affine transformation matrix
         and procedure of application it to given point.
\see http://en.wikipedia.org/wiki/Affine_transformation
     and http://www.devmaster.net/wiki/Transformation_matrices
\ingroup strategies
\tparam Dimension1 number of dimensions to transform from
\tparam Dimension2 number of dimensions to transform to
 */
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class ublas_transformer
{
};

template <typename CalculationType>
class ublas_transformer<CalculationType, 2, 2>
{
protected :
    typedef CalculationType ct;
    typedef geofeatures_boost::numeric::ublas::matrix<ct> matrix_type;
    matrix_type m_matrix;

public :

    inline ublas_transformer(
                ct const& m_0_0, ct const& m_0_1, ct const& m_0_2,
                ct const& m_1_0, ct const& m_1_1, ct const& m_1_2,
                ct const& m_2_0, ct const& m_2_1, ct const& m_2_2)
        : m_matrix(3, 3)
    {
        m_matrix(0,0) = m_0_0;   m_matrix(0,1) = m_0_1;   m_matrix(0,2) = m_0_2;
        m_matrix(1,0) = m_1_0;   m_matrix(1,1) = m_1_1;   m_matrix(1,2) = m_1_2;
        m_matrix(2,0) = m_2_0;   m_matrix(2,1) = m_2_1;   m_matrix(2,2) = m_2_2;
    }

    inline ublas_transformer(matrix_type const& matrix)
        : m_matrix(matrix)
    {}

    inline ublas_transformer() : m_matrix(3, 3) {}

    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension_greater_equal<P1, 2>();
        assert_dimension_greater_equal<P2, 2>();

        ct const& c1 = get<0>(p1);
        ct const& c2 = get<1>(p1);

        ct p2x = c1 * m_matrix(0,0) + c2 * m_matrix(0,1) + m_matrix(0,2);
        ct p2y = c1 * m_matrix(1,0) + c2 * m_matrix(1,1) + m_matrix(1,2);

        typedef typename geometry::coordinate_type<P2>::type ct2;
        set<0>(p2, geofeatures_boost::numeric_cast<ct2>(p2x));
        set<1>(p2, geofeatures_boost::numeric_cast<ct2>(p2y));

        return true;
    }

    matrix_type const& matrix() const { return m_matrix; }
};

// It IS possible to go from 3 to 2 coordinates
template <typename CalculationType>
class ublas_transformer<CalculationType, 3, 2> : public ublas_transformer<CalculationType, 2, 2>
{
    typedef CalculationType ct;

public :
    inline ublas_transformer(
                ct const& m_0_0, ct const& m_0_1, ct const& m_0_2,
                ct const& m_1_0, ct const& m_1_1, ct const& m_1_2,
                ct const& m_2_0, ct const& m_2_1, ct const& m_2_2)
        : ublas_transformer<CalculationType, 2, 2>(
                    m_0_0, m_0_1, m_0_2,
                    m_1_0, m_1_1, m_1_2,
                    m_2_0, m_2_1, m_2_2)
    {}

    inline ublas_transformer()
        : ublas_transformer<CalculationType, 2, 2>()
    {}
};

template <typename CalculationType>
class ublas_transformer<CalculationType, 3, 3>
{
protected :
    typedef CalculationType ct;
    typedef geofeatures_boost::numeric::ublas::matrix<ct> matrix_type;
    matrix_type m_matrix;

public :
    inline ublas_transformer(
                ct const& m_0_0, ct const& m_0_1, ct const& m_0_2, ct const& m_0_3,
                ct const& m_1_0, ct const& m_1_1, ct const& m_1_2, ct const& m_1_3,
                ct const& m_2_0, ct const& m_2_1, ct const& m_2_2, ct const& m_2_3,
                ct const& m_3_0, ct const& m_3_1, ct const& m_3_2, ct const& m_3_3
                )
        : m_matrix(4, 4)
    {
        m_matrix(0,0) = m_0_0; m_matrix(0,1) = m_0_1; m_matrix(0,2) = m_0_2; m_matrix(0,3) = m_0_3;
        m_matrix(1,0) = m_1_0; m_matrix(1,1) = m_1_1; m_matrix(1,2) = m_1_2; m_matrix(1,3) = m_1_3;
        m_matrix(2,0) = m_2_0; m_matrix(2,1) = m_2_1; m_matrix(2,2) = m_2_2; m_matrix(2,3) = m_2_3;
        m_matrix(3,0) = m_3_0; m_matrix(3,1) = m_3_1; m_matrix(3,2) = m_3_2; m_matrix(3,3) = m_3_3;
    }

    inline ublas_transformer() : m_matrix(4, 4) {}

    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2& p2) const
    {
        ct const& c1 = get<0>(p1);
        ct const& c2 = get<1>(p1);
        ct const& c3 = get<2>(p1);

        typedef typename geometry::coordinate_type<P2>::type ct2;

        set<0>(p2, geofeatures_boost::numeric_cast<ct2>(
            c1 * m_matrix(0,0) + c2 * m_matrix(0,1) + c3 * m_matrix(0,2) + m_matrix(0,3)));
        set<1>(p2, geofeatures_boost::numeric_cast<ct2>(
            c1 * m_matrix(1,0) + c2 * m_matrix(1,1) + c3 * m_matrix(1,2) + m_matrix(1,3)));
        set<2>(p2, geofeatures_boost::numeric_cast<ct2>(
            c1 * m_matrix(2,0) + c2 * m_matrix(2,1) + c3 * m_matrix(2,2) + m_matrix(2,3)));

        return true;
    }

    matrix_type const& matrix() const { return m_matrix; }
};

/*!
\brief Strategy of translate transformation in Cartesian system.
\details Translate moves a geometry a fixed distance in 2 or 3 dimensions.
\see http://en.wikipedia.org/wiki/Translation_%28geometry%29
\ingroup strategies
\tparam Dimension1 number of dimensions to transform from
\tparam Dimension2 number of dimensions to transform to
 */
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class translate_transformer
{
};

template<typename CalculationType>
class translate_transformer<CalculationType, 2, 2> : public ublas_transformer<CalculationType, 2, 2>
{
public :
    // To have translate transformers compatible for 2/3 dimensions, the
    // constructor takes an optional third argument doing nothing.
    inline translate_transformer(CalculationType const& translate_x,
                CalculationType const& translate_y,
                CalculationType const& = 0)
        : ublas_transformer<CalculationType, 2, 2>(
                1, 0, translate_x,
                0, 1, translate_y,
                0, 0, 1)
    {}
};

template <typename CalculationType>
class translate_transformer<CalculationType, 3, 3> : public ublas_transformer<CalculationType, 3, 3>
{
public :
    inline translate_transformer(CalculationType const& translate_x,
                CalculationType const& translate_y,
                CalculationType const& translate_z)
        : ublas_transformer<CalculationType, 3, 3>(
                1, 0, 0, translate_x,
                0, 1, 0, translate_y,
                0, 0, 1, translate_z,
                0, 0, 0, 1)
    {}

};

/*!
\brief Strategy of scale transformation in Cartesian system.
\details Scale scales a geometry up or down in all its dimensions.
\see http://en.wikipedia.org/wiki/Scaling_%28geometry%29
\ingroup strategies
\tparam Dimension1 number of dimensions to transform from
\tparam Dimension2 number of dimensions to transform to
*/
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class scale_transformer
{
};

template <typename CalculationType>
class scale_transformer<CalculationType, 2, 2> : public ublas_transformer<CalculationType, 2, 2>
{

public :
    inline scale_transformer(CalculationType const& scale_x,
                CalculationType const& scale_y,
                CalculationType const& = 0)
        : ublas_transformer<CalculationType, 2, 2>(
                scale_x, 0,       0,
                0,       scale_y, 0,
                0,       0,       1)
    {}

    inline scale_transformer(CalculationType const& scale)
        : ublas_transformer<CalculationType, 2, 2>(
                scale, 0,     0,
                0,     scale, 0,
                0,     0,     1)
    {}
};

template <typename CalculationType>
class scale_transformer<CalculationType, 3, 3> : public ublas_transformer<CalculationType, 3, 3>
{
public :
    inline scale_transformer(CalculationType const& scale_x,
                CalculationType const& scale_y,
                CalculationType const& scale_z)
        : ublas_transformer<CalculationType, 3, 3>(
                scale_x, 0,       0,       0,
                0,       scale_y, 0,       0,
                0,       0,       scale_z, 0,
                0,       0,       0,       1)
    {}

    inline scale_transformer(CalculationType const& scale)
        : ublas_transformer<CalculationType, 3, 3>(
                scale, 0,     0,     0,
                0,     scale, 0,     0,
                0,     0,     scale, 0,
                0,     0,     0,     1)
    {}
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename DegreeOrRadian>
struct as_radian
{};

template <>
struct as_radian<radian>
{
    template <typename T>
    static inline T get(T const& value)
    {
        return value;
    }
};

template <>
struct as_radian<degree>
{
    template <typename T>
    static inline T get(T const& value)
    {
        typedef typename promote_floating_point<T>::type promoted_type;
        return value * math::d2r<promoted_type>();
    }

};

template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class rad_rotate_transformer
    : public ublas_transformer<CalculationType, Dimension1, Dimension2>
{
public :
    inline rad_rotate_transformer(CalculationType const& angle)
        : ublas_transformer<CalculationType, Dimension1, Dimension2>(
                 cos(angle), sin(angle), 0,
                -sin(angle), cos(angle), 0,
                 0,          0,          1)
    {}
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Strategy for rotate transformation in Cartesian coordinate system.
\details Rotate rotates a geometry of specified angle about a fixed point (e.g. origin).
\see http://en.wikipedia.org/wiki/Rotation_%28mathematics%29
\ingroup strategies
\tparam DegreeOrRadian degree/or/radian, type of rotation angle specification
\note A single angle is needed to specify a rotation in 2D.
      Not yet in 3D, the 3D version requires special things to allow
      for rotation around X, Y, Z or arbitrary axis.
\todo The 3D version will not compile.
 */
template
<
    typename DegreeOrRadian,
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class rotate_transformer : public detail::rad_rotate_transformer<CalculationType, Dimension1, Dimension2>
{

public :
    inline rotate_transformer(CalculationType const& angle)
        : detail::rad_rotate_transformer
            <
                CalculationType, Dimension1, Dimension2
            >(detail::as_radian<DegreeOrRadian>::get(angle))
    {}
};

}} // namespace strategy::transform

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MATRIX_TRANSFORMERS_HPP


/* Copyright (c) 2017, Waterloo Autonomous Vehicles Laboratory (WAVELab),
 * Waterloo Intelligent Systems Engineering Lab (WISELab),
 * University of Waterloo.
 *
 * Refer to the accompanying LICENSE file for license information.
 *
 * ############################################################################
 ******************************************************************************
 |                                                                            |
 |                         /\/\__/\_/\      /\_/\__/\/\                       |
 |                         \          \____/          /                       |
 |                          '----________________----'                        |
 |                              /                \                            |
 |                            O/_____/_______/____\O                          |
 |                            /____________________\                          |
 |                           /    (#UNIVERSITY#)    \                         |
 |                           |[**](#OFWATERLOO#)[**]|                         |
 |                           \______________________/                         |
 |                            |_""__|_,----,_|__""_|                          |
 |                            ! !                ! !                          |
 |                            '-'                '-'                          |
 |       __    _   _  _____  ___  __  _  ___  _    _  ___  ___   ____  ____   |
 |      /  \  | | | ||_   _|/ _ \|  \| |/ _ \| \  / |/ _ \/ _ \ /     |       |
 |     / /\ \ | |_| |  | |  ||_||| |\  |||_|||  \/  |||_||||_|| \===\ |====   |
 |    /_/  \_\|_____|  |_|  \___/|_| \_|\___/|_|\/|_|\___/\___/ ____/ |____   |
 |                                                                            |
 ******************************************************************************
 * ############################################################################
 *
 * File: pose_cov_comp.hpp
 * Desc: File containing the function for pose composition with uncertainty.
 *
 * References:
 *       [1] J. Blanco, “A tutorial on se (3) transformation parameterizations
 *           and on-manifold optimization,” Univ. Malaga, Tech. Rep, no. 3,
 *           pp. 1–56, 2014.
 * Auth: Chunshang Li and Jordan Hu
 *
 * ############################################################################
 */

#ifndef WAVE_UTILS_POSE_COV_COMP_HPP_
#define WAVE_UTILS_POSE_COV_COMP_HPP_

#include <Eigen/Dense>

namespace wave {

typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 4, 1> Vector4;
typedef Eigen::Matrix<double, 3, 1> Vector3;
typedef Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Matrix6x6;
typedef Eigen::Matrix<double, 6, 7, Eigen::RowMajor> Matrix6x7;
typedef Eigen::Matrix<double, 7, 6, Eigen::RowMajor> Matrix7x6;
typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Matrix4x4;
typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> Matrix3x4;
typedef Eigen::Matrix<double, 7, 7, Eigen::RowMajor> Matrix7x7;
typedef Eigen::Matrix<double, 3, 7, Eigen::RowMajor> Matrix3x7;
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Matrix3x3;

struct PoseWithCovariance {
    Vector3 position;
    Matrix3x3 rotation_matrix;
    Matrix6x6 covariance;

    PoseWithCovariance();
    PoseWithCovariance(Vector6 &p, Matrix6x6 &q);
    PoseWithCovariance(Vector3 &p, Matrix3x3 &r, Matrix6x6 &q);
    Vector3 getPosition() const;
    Vector3 getYPR() const;
    Eigen::Quaterniond getQuaternion() const;
    Vector7 getPoseQuaternion() const;
    Eigen::Affine3d getTransformMatrix() const;
};

/** Calculates the pose composition of two poses and the estimated covariance.
 *  Based on [1].
 *
 *  @param p1 first pose with covariance
 *  @param p2 second pose with covariance
 *
 *  @return composed pose with predicted covariance
 *
 */
PoseWithCovariance composePose(PoseWithCovariance &p1, PoseWithCovariance &p);

/** The Jacobian of quaternion normalization function. Quaternion in the form of
 *  [qr, qx, qt, qz]
 *  Equation (1.7)
 *
 *  @param q quaternion vector
 *
 *  @return quaternion normal wrt q Jacobian
 */
Matrix4x4 jacobian_Quat_Norm_wrt_q(const Vector4 &q);

/** The Jacobian of normalized quaternion to rpy function.
 *  Equation (2.9) to Equation (2.10)
 *
 *  @param q quaternion vector
 *
 *  @return quaternion normal to roll pitch yaw wrt q Jacobian
 */
Matrix3x4 jacobian_Quat_Norm_to_Rpy_wrt_q(const Vector4 &q);

/** The Jacobian of p7 to p6 conversion.
 *  Equation (2.12)
 *
 *  @param p pose with position and quaternion
 *
 *  @return p7 to p6 wrt p Jacobian
 */
Matrix6x7 jacobian_p7_to_p6_wrt_p(const Vector7 &p);

/** Jacobian of composing a point to a p7.
 *  Equation (3.8)
 *
 *  @param p pose with position and quaternion
 *  @param a position
 *  @return p7 point composition wrt p Jacobian
 */
Matrix3x7 jacobian_p7_Point_Composition_wrt_p(const Vector7 &p,
                                              const Vector3 &a);
/** Jacobian of the composition of p7 poses.
 *  Equation (5.8)
 *
 *  @param p1 pose with position and quaternion
 *  @param p2 pose with position and quaternion
 *  @return p7 p7 composition wrt p1 Jacobian
 */
Matrix7x7 jacobian_p7_p7_Composition_wrt_p1(const Vector7 &p1,
                                            const Vector7 &p2);

/** Jacobian of composing a point to a p7.
 *  Equation (3.10)
 *
 *  @param p pose with position and quaternion
 *  @param a position
 *  @return p7 point composition wrt a Jacobian
 */
Matrix3x3 jacobian_p7_Point_Composition_wrt_a(const Vector7 &p,
                                              const Vector3 &a);

/** Jacobian of the composition of p7 poses.
 *  Equation (5.9)
 *
 *  @param p1 pose with position and quaternion
 *  @param p2 pose with position and quaternion
 *
 *  @return p7 p7 composition wrt p2 Jacobian
 */
Matrix7x7 jacobian_p7_p7_Composition_wrt_p2(const Vector7 &p1,
                                            const Vector7 &p2);

/** Jacobian of converting a p6 to a p7.
 *  Equation (2.8)
 *
 *  @param p pose with position and ypr
 *  @return p6 to p7 wrt p Jacobian
 */
Matrix7x6 jacobian_p6_to_p7_wrt_p(const Vector6 &p);
}  // namespace wave

namespace wave {

/** NOTE: wave_geometry/ has a similar implementation of the following functions
 *  but these are made to match the methods proposed in [1]. These functions
 *  should ONLY be used for pose composition based on [1].
 *
 */
namespace pose_comp {

/** Converts Quaternion to Yaw Pitch Roll angles.
 *  Note v(0) is yaw, v(1) is pitch, v(2) is roll
 *  Equation(2.9) and Equation(2.10)
 *
 *  @param q quaternion vector
 *
 *  @return yaw pitch roll angles in rad
 */
Vector3 quatToYPR(const Eigen::Quaterniond &q);

/** Converts Yaw Pitch Roll angles to Quaternion.
 *  Equation(2.3), Equation(2.4), Equation(2.5), Equation(2.6)
 *
 *  @param ypr yaw pitch roll angles in rad
 *
 *  @return quaternion vector
 */
Eigen::Quaterniond yprToQuat(const Vector3 &ypr);

/** Converts Yaw Pitch Roll angles to Rotation Matrix.
 *  Equation(2.18)
 *
 *  @param ypr yaw pitch roll angles in rad
 *
 *  @return rotation matrix
 */
Matrix3x3 yprToRotMatrix(const Vector3 &ypr);

/** Converts Rotation Matrix to Yaw Pitch Roll angles.
 *  Equation(2.20), Equation(2.21), Equation(2.22), and Equation(2.23)
 *
 *  @param p rotation matrix
 *
 *  @return yaw pitch roll angles in rad
 */
Vector3 rotMatrixToYPR(const Matrix3x3 &p);

/** Converts Rotation Matrix to Quaternion. According to [1], fastest way to
 *  convert from Rotation Matrix to Quat is to convert
 *  Quaternion -> Euler Angles -> Rotation Matrix
 *
 *  @param p rotation matrix
 *
 *  @return: Quaternion vector
 */
Eigen::Quaterniond rotMatrixToQuat(const Matrix3x3 &p);
}  // namespace pose_comp
}  // namespace wave

#endif  // WAVE_UTILS_POSE_COV_COMP_HPP_


#include <iostream>
#include <functional>
#include <cstring>
#include <vector>
#include <utility>
#include <unordered_map>

#include <El.hpp>
#include <boost/mpi.hpp>

#define SKYLARK_NO_ANY
#include <skylark.hpp>

#include "utilities.hpp"
#include "parser.hpp"

/*******************************************/
namespace bmpi =  boost::mpi;
namespace skys =  skylark::sketch;
namespace skyb =  skylark::base;
/*******************************************/

/* These were declared as extern in utilities.hpp --- defining it here */
int int_params [NUM_INT_PARAMETERS];
char* chr_params[NUM_CHR_PARAMETERS];

/** Typedef DistMatrix and Matrix */
typedef std::vector<int> IntContainer;
typedef std::vector<double> DblContainer;
typedef El::DistMatrix<double, El::CIRC, El::CIRC> MatrixType;
typedef El::DistMatrix<double, El::VC, El::STAR> DistMatrixType;

int main (int argc, char** argv) {
    /* Initialize Elemental */
    El::Initialize (argc, argv);

    /* MPI sends argc and argv everywhere --- parse everywhere */
    parse_parameters (argc,argv);

    /* Initialize skylark */
    skyb::context_t context (int_params[RAND_SEED_INDEX]);

    /* Create matrices A and B */
    bmpi::communicator world;
    MPI_Comm mpi_world(world);
    El::Grid grid (mpi_world);
    El::DistMatrix<double, El::VR, El::STAR> A(grid);
    El::DistMatrix<double, El::VR, El::STAR> B(grid);

    /** Only randomization is supported for now */
    if (0==int_params[USE_RANDOM_INDEX]) {
        /** TODO: Read the entries! */
        std::cout << "We don't support reading --- yet --" << std::endl;
    } else {
        El::Uniform (A, int_params[M_INDEX], int_params[N_INDEX]);
        El::Uniform (B, int_params[M_INDEX], int_params[N_RHS_INDEX]);
    }

    /**
     * Depending on which sketch is requested, do the sketching.
     */
    if (0==strcmp("JLT", chr_params[TRANSFORM_INDEX]) ) {

        if (SKETCH_LEFT == int_params[SKETCH_DIRECTION_INDEX]) {

            /* 1. Create the sketching matrix */
            skys::JLT_t<DistMatrixType, MatrixType> JLT (int_params[M_INDEX],
                int_params[S_INDEX], context);

            /* 2. Create space for the sketched matrix */
            MatrixType sketch_A(int_params[S_INDEX], int_params[N_INDEX]);

            /* 3. Apply the transform */
            try {
                JLT.apply (A, sketch_A, skys::columnwise_tag());
            } catch (skylark::base::skylark_exception ex) {
                SKYLARK_PRINT_EXCEPTION_DETAILS(ex);
                SKYLARK_PRINT_EXCEPTION_TRACE(ex);
                errno = *(boost::get_error_info<skylark::base::error_code>(ex));
                std::cout << "Caught exception, exiting with error " << errno << std::endl;
                std::cout << skylark_strerror(errno) << std::endl;
                return errno;
            }

            /* 4. Print and see the result (if small enough) */
            if (int_params[S_INDEX] * int_params[N_INDEX] < 100 &&
                world.rank() == 0)
                El::Display(sketch_A);

            /** TODO: Do that same to B, and solve the system! */

        } else {
            std::cout << "We only have left sketching. Please retry" << std::endl;
        }
    } else if (0==strcmp("FJLT", chr_params[TRANSFORM_INDEX]) ) {
        if (SKETCH_LEFT == int_params[SKETCH_DIRECTION_INDEX]) {
            /* 1. Create the sketching matrix */
            skys::FJLT_t<DistMatrixType, MatrixType> FJLT (int_params[M_INDEX],
                int_params[S_INDEX], context);

            /* 2. Create space for the sketched matrix */
            MatrixType sketch_A(int_params[S_INDEX], int_params[N_INDEX]);

            /* 3. Apply the transform */
            FJLT.apply (A, sketch_A, skys::columnwise_tag());

            /* 4. Print and see the result */
            if (int_params[S_INDEX] * int_params[M_INDEX] < 100 &&
                world.rank() == 0)
                El::Display(sketch_A);

            /** TODO: Do that same to B, and solve the system! */

        }
    } else if (0==strcmp("CWT", chr_params[TRANSFORM_INDEX]) ) {
        if (SKETCH_LEFT == int_params[SKETCH_DIRECTION_INDEX]) {

            /* 1. Create the sketching matrix */
            skys::CWT_t<DistMatrixType, MatrixType>
                Sparse (int_params[M_INDEX], int_params[S_INDEX], context);

            /* 2. Create space for the sketched matrix */
            MatrixType sketch_A(int_params[S_INDEX], int_params[N_INDEX]);

            /* 3. Apply the transform */
            SKYLARK_BEGIN_TRY()
                Sparse.apply (A, sketch_A, skys::columnwise_tag());
            SKYLARK_END_TRY()
            SKYLARK_CATCH_AND_RETURN_ERROR_CODE();

            /* 4. Print and see the result */
            if (int_params[S_INDEX] * int_params[M_INDEX] < 100 &&
                world.rank() == 0)
                El::Display(sketch_A);

            /** TODO: Do that same to B, and solve the system! */

        }
    } else {
        std::cout << "We only have JLT/FJLT/Sparse sketching. Please retry" <<
            std::endl;
    }

    El::Finalize();

    return 0;

}


#include <armadillo>
#include "sargparse/Parameter.h"

namespace
{

arma::mat44 makeTransform(double dx, double dy, double dz, double alphaX, double alphaY, double alphaZ) {
	arma::mat44 translation = arma::eye(4, 4);
	translation(0, 3) = dx;
	translation(1, 3) = dy;
	translation(2, 3) = dz;

	/* align z axis to current joint this might incorporate two rotations (first the rotation around x-axis is performed, then around the y axis and around the z axis) */
	arma::mat44 rotX = arma::eye(4, 4);
	arma::mat44 rotY = arma::eye(4, 4);
	arma::mat44 rotZ = arma::eye(4, 4);

	if (std::abs(alphaX) >= std::numeric_limits<double>::epsilon())
	{
		const double cRotX = std::cos(alphaX);
		const double sRotX = std::sin(alphaX);
		rotX(1, 1) = cRotX;
		rotX(1, 2) = -sRotX;
		rotX(2, 1) = sRotX;
		rotX(2, 2) = cRotX;
	}

	if (std::abs(alphaY) >= std::numeric_limits<double>::epsilon())
	{
		const double cRotY = std::cos(alphaY);
		const double sRotY = std::sin(alphaY);
		rotY(0, 0) = cRotY;
		rotY(0, 2) = sRotY;
		rotY(2, 0) = -sRotY;
		rotY(2, 2) = cRotY;
	}

	if (std::abs(alphaZ) >= std::numeric_limits<double>::epsilon())
	{
		const double cRotZ = std::cos(alphaZ);
		const double sRotZ = std::sin(alphaZ);
		rotZ(0, 0) = cRotZ;
		rotZ(0, 1) = -sRotZ;
		rotZ(1, 0) = sRotZ;
		rotZ(1, 1) = cRotZ;
	}

	return translation * rotX * rotY * rotZ;
}

arma::mat44 transform = arma::eye(4,4);

sargp::Parameter<double> rotX {1., "rotX", "how much to rotate around the x axis (degrees)", []{ transform = makeTransform(0, 0, 0, *rotX * M_PI/180., 0, 0) * transform; }};
sargp::Parameter<double> rotY {1., "rotY", "how much to rotate around the y axis (degrees)", []{ transform = makeTransform(0, 0, 0, 0, *rotY * M_PI/180., 0) * transform; }};
sargp::Parameter<double> rotZ {1., "rotZ", "how much to rotate around the z axis (degrees)", []{ transform = makeTransform(0, 0, 0, 0, 0, *rotZ * M_PI/180.) * transform; }};

sargp::Parameter<double> movX {1., "movX", "how much to move along the x axis (degrees)", []{ transform = makeTransform(*movX, 0, 0, 0, 0, 0) * transform; }};
sargp::Parameter<double> movY {1., "movY", "how much to move along the y axis (degrees)", []{ transform = makeTransform(0, *movY, 0, 0, 0, 0) * transform; }};
sargp::Parameter<double> movZ {1., "movZ", "how much to move along the z axis (degrees)", []{ transform = makeTransform(0, 0, *movZ, 0, 0, 0) * transform; }};

sargp::Parameter<double> scale {1., "scale", "scale the STL", []{ transform = arma::mat44{{*scale, 0, 0, 0}, {0, *scale, 0, 0}, {0, 0, *scale, 0}, {0, 0, 0, 1}} * transform;}};

sargp::Parameter<double> scaleX {1., "scaleX", "scale the x components of the STL", []{ transform = arma::mat44{{*scaleX, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}} * transform;}};
sargp::Parameter<double> scaleY {1., "scaleY", "scale the y components of the STL", []{ transform = arma::mat44{{1, 0, 0, 0}, {0, *scaleY, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}} * transform;}};
sargp::Parameter<double> scaleZ {1., "scaleZ", "scale the z components of the STL", []{ transform = arma::mat44{{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, *scaleZ, 0}, {0, 0, 0, 1}} * transform;}};

}

arma::mat44 const& getTransform() {
    return transform;
}


#include <cmath>
#include <complex>
#include <vector>
#include <iostream>

#include <Eigen/Geometry>
#include <Magick++.h>

#include "Constants.h"
#include "ImageGenerator.h"
#include "FractalInstance.h"

using namespace std;
using namespace Eigen;
using namespace Magick;

vector<FractalInstance> fractals;

// user literal for millions (i.e. 1_m = 10000000)
constexpr unsigned long long operator "" _m(unsigned long long l) {
  return l * 1000 * 1000;
}

int main(int argc, char** argv)
{
  InitializeMagick(*argv);

  auto theta(0.0);
  auto delta(2 * M_PI / NUM_FRAMES);

  auto offset1(0.0);
  auto offset2(0.0);
  auto offset3(0.0);

  Vector3f axis1(1, 0, 0);
  Vector3f axis2(0, 1, 0);
  Vector3f axis3(0, 0, 1);

  axis1.normalize();
  axis2.normalize();
  axis3.normalize();

  Vector3f vec(-1/6.0, 1/120.0, -1/5040.0);
  auto vec1(vec);
  auto vec2(vec);
  auto vec3(vec);

  FractalInstance::num_points = 10_m;
  FractalInstance::complex_range = 6.0;
  FractalInstance::default_iterations = 1e2;

  /* FractalInstance fi( */
  /*   {1, vec1.x(), vec1.y(), vec1.z()}, */
  /*   {1, 2, 4, 6}, */
  /*   {0, 0, 3.0, 3.0} */
  /* ); */

  /* ImageGenerator::generate(fi); */

  for (auto frame(1); frame <= NUM_FRAMES; ++frame)
  {
    FractalInstance rfi(
      {1, vec1.x(), vec1.y(), vec1.z()},
      {1, 2, 4, 6},
      {0, 0, 4.0, 4.0}
    );

    FractalInstance gfi(
      {1, vec2.x(), vec2.y(), vec2.z()},
      {1, 2, 4, 6},
      {0, 0, 4.0, 4.0}
    );

    FractalInstance bfi(
      {1, vec3.x(), vec3.y(), vec3.z()},
      {1, 2, 4, 6},
      {0, 0, 4.0, 4.0}
    );

    ImageGenerator::generate({rfi, gfi, bfi}, frame);

    theta += delta;
    auto t1(AngleAxis<float>(theta + offset1, axis1));
    vec1 = t1 * vec;
    auto t2(AngleAxis<float>(theta + offset2, axis2));
    vec2 = t2 * vec;
    auto t3(AngleAxis<float>(theta + offset3, axis3));
    vec3 = t3 * vec;
  }

  return 0;
}


/*
 *  Created by Andrea Bedini on 21/Nov/2008.
 *  Copyright 2008-2014 Andrea Bedini <andrea@andreabedini.com>
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 */

#ifndef POLYNOMIAL_HPP_
#define POLYNOMIAL_HPP_

#include <boost/operators.hpp>

#include <iosfwd>
#include <vector>

template<typename T>
class polynomial
  : boost::ring_operators1< polynomial<T>
  , boost::ring_operators2< polynomial<T>, T
  , boost::equality_comparable< polynomial<T>
  , boost::left_shiftable<polynomial<T>, unsigned int
  > > > >
{
  /// Coefficients { c_0; ...; c_n } :
  std::vector<T> impl_;

  /// remove leading zero coefficients
  void normalize()
  {
    std::size_t i = order();
    // do not remove the constant term, even if it's zero
    while (impl_[i] == 0 and i > 1)
      i--;
    order(i);
  }

public:
  // default constructor
  explicit polynomial(const T& a = T(0))
    : impl_{a}
  {
  }

  polynomial(std::initializer_list<T> list)
    : impl_(list)
  {
  }

  // copy constructor
  polynomial(polynomial<T> const& rhs)
    : impl_(rhs.impl_)
  {
  }

  // move constructor
  polynomial(polynomial<T>&& rhs)
    : impl_(std::move(rhs.impl_))
  {
  }

  // assignemnt
  polynomial<T>& operator=(polynomial<T> const& rhs)
  {
    impl_ = rhs.impl_;
    return *this;
  }

  polynomial<T>& operator=(polynomial<T>&& rhs)
  {
    impl_ = std::move(rhs.impl_);
    return *this;
  }

  // conversions
  template<typename T2>
  friend class polynomial;

  template<class T2>
  explicit polynomial(T2 const& a)
    : impl_{T(a)}
  {
  }

  template<class T2>
  explicit polynomial(polynomial<T2> const& rhs)
  {
    for (auto const& c : rhs.impl_) {
      impl_.push_back(T(c));
    }
  }
  template<class T2>
  polynomial<T>& operator=(T2 const& rhs)
  {
    impl_ = {T(rhs)};
    return *this;
  }

  template<class T2>
  polynomial<T>& operator=(polynomial<T2> const& rhs)
  {
    impl_.clear();
    for (auto const& c : rhs.impl_) {
      impl_.push_back(T(c));
    }
    return *this;
  }

  std::size_t order() const
  {
    return impl_.size() - 1;
  }

  void order(std::size_t n)
  {
    impl_.resize(n + 1);
  }

  // iterators
  decltype(impl_.begin()) begin() { return impl_.begin(); }
  decltype(impl_.end())   end()   { return impl_.end(); }

  decltype(impl_.cbegin()) begin() const { return impl_.begin(); }
  decltype(impl_.cend())   end()   const { return impl_.end(); }

  // indexing
  T& operator[](std::size_t i)
  {
    return impl_[i];
  }

  const T& operator[](std::size_t i) const
  {
    return impl_[i];
  }

  // comparison
  bool operator==(const polynomial<T>& rhs) const
  {
    return impl_ == rhs.impl_;
  }

  /// arithmetic
  polynomial<T>& operator+=(const T& rhs)
  {
    impl_[0] += rhs;
    return *this;
  }

  polynomial<T>& operator-=(const T& rhs)
  {
    impl_[0] -= rhs;
    return *this;
  }

  polynomial<T>& operator*=(const T& rhs)
  {
    for (auto& c : impl_)
      c *= rhs;
    normalize();
    return *this;
  }

  polynomial<T>& operator+=(const polynomial<T>& rhs)
  {
    order(std::max(order(), rhs.order()));
    for (std::size_t i = 0; i <= rhs.order(); i++)
      impl_[i] += rhs.impl_[i];
    normalize();
    return *this;
  }

  polynomial<T>& operator-=(const polynomial<T>& rhs)
  {
    order(std::max(order(), rhs.order()));
    for (std::size_t i = 0; i <= rhs.order(); i++)
      impl_[i] -= rhs.impl_[i];
    normalize();
    return *this;
  }

  polynomial<T>& operator*=(const polynomial<T>& rhs)
  {
    polynomial<T> product;
    product.order(order() + rhs.order());
    for (std::size_t i = 0; i <= order(); i++) {
      for (std::size_t j = 0; j <= rhs.order(); j++)
        product.impl_[i + j] += impl_[i] * rhs.impl_[j];
    }
    impl_.swap(product.impl_);
    return *this;
  }

  // left_shiftable
  polynomial<T>& operator<<=(std::size_t rhs)
  {
    impl_.insert(impl_.begin(), rhs, T(0));
    return *this;
  }

  const polynomial<T> operator-() const
  {
    polynomial<T> y;
    y.order(order());
    for (std::size_t i = 0; i <= order(); i++)
      y.impl_[i] = -impl_[i];
    return y;
  }

};

template<class T>
std::ostream& operator<<(std::ostream& o, const polynomial<T>& p)
{
  for (auto i = 0u; i <= p.order(); i++) {
    if (p[i] == T(0))
      continue;
    auto c = p[i];
    if (c < 0) {
      o << "- ";
      c = -c;
    } else if (i > 0) {
      o << "+ ";
    }
    if (c != 1 or i == 0) {
      o << c << " ";
    }
    if (i > 0) {
      o << "x";
      if (i > 1)
        o << "^" << i;
      o << " ";
    }
  }
  return o;
}

#endif // POLYNOMIAL_HPP_


#include "image_processing.h"
#include <numeric>
#include <cfloat>
#include <Eigen/LU>
#include <QImage>

namespace ImageProcessing
{

template <typename Scalar>
Scalar crop(Scalar x, Scalar min_x, Scalar max_x)
{
    return std::max(std::min(x, max_x), min_x);
}

void Image::force_unity()
{
    const double sum = std::accumulate(pixels.begin(), pixels.end(), 0.0);
    assert(sum > 1e-16);
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        set_pixel(x, y, get_pixel(x, y) / sum);
    }
}

void Image::scale_to_unit()
{
    double max_value = - DBL_MAX;
    double min_value = + DBL_MAX;

    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const double value = get_pixel(x, y);
        max_value = std::max(max_value, value);
        min_value = std::min(min_value, value);
    }
    assert(max_value - min_value > 0.0);
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        set_pixel(x, y, (get_pixel(x, y) - min_value) / (max_value - min_value));
    }
}

void AbstractImage::save(const std::string &file_path) const
{
    QImage q_image(width(), height(), QImage::Format_ARGB32);
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const IntColor color = get_color(x, y);
        q_image.setPixelColor(x, y, QColor(color(0), color(1), color(2), color(3)));
    }
    q_image.save(QString::fromStdString(file_path));
}

AbstractImage::IntColor Image::get_color(int x, int y) const
{
    const double value = crop(get_pixel(x, y), 0.0, 1.0);
    return IntColor(value * 255, value * 255, value * 255, 255);
}

AbstractImage::IntColor ColorImage::get_color(int x, int y) const
{
    Eigen::Vector4d color = get_rgba(x, y);
    for (int i : { 0, 1, 2, 3 }) color(i) = crop(color(i), 0.0, 1.0);
    return IntColor(color(0) * 255, color(1) * 255, color(2) * 255, color(3) * 255);
}

ColorImage::ColorImage(const std::string &file_path)
{
    QImage q_image(QString::fromStdString(file_path));
    width_  = q_image.width();
    height_ = q_image.height();

    assert(width() > 0 && height() > 0);

    rgba_ = std::vector<Image>(4, Image(width(), height()));
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const QColor q_color = q_image.pixelColor(x, y);
        rgba_[0].set_pixel(x, y, q_color.redF());
        rgba_[1].set_pixel(x, y, q_color.greenF());
        rgba_[2].set_pixel(x, y, q_color.blueF());
        rgba_[3].set_pixel(x, y, q_color.alphaF());
    }
}

Image ColorImage::get_luminance() const
{
    Image new_image(width(), height());
    for (int x = 0; x < width(); ++ x) for (int y = 0; y < height(); ++ y)
    {
        const double r = rgba_[0].get_pixel(x, y);
        const double g = rgba_[1].get_pixel(x, y);
        const double b = rgba_[2].get_pixel(x, y);

        // https://en.wikipedia.org/wiki/Relative_luminance
        const double luminance = 0.2126 * r + 0.7152 * g + 0.0722 * b;

        new_image.set_pixel(x, y, luminance);
    }
    return new_image;
}

///////////////////////////////////////////////////////////////////////////////////////

Image apply_convolution(const Image& image, const Eigen::MatrixXd &kernel)
{
    const int w = image.width();
    const int h = image.height();

    const int kernel_size = kernel.rows();

    assert(kernel_size % 2 == 1);
    assert(kernel_size == kernel.cols());

    Image new_image(w, h);

    for (int x = 0; x < w; ++ x) for (int y = 0; y < h; ++ y)
    {
        double value = 0.0;
        for (int kernel_x = 0; kernel_x < kernel_size; ++ kernel_x)
        {
            for (int kernel_y = 0; kernel_y < kernel_size; ++ kernel_y)
            {
                const int original_image_x = crop(x + kernel_x - ((kernel_size - 1) / 2), 0, w - 1);
                const int original_image_y = crop(y + kernel_y - ((kernel_size - 1) / 2), 0, h - 1);

                value += kernel(kernel_x, kernel_y) * image.get_pixel(original_image_x, original_image_y);
            }
        }
        new_image.set_pixel(x, y, value);
    }
    return new_image;
}

Image calculate_guided_filter_kernel(const Image& image, int center_x, int center_y, int radius, double epsilon, bool force_positive)
{
    const int width  = image.width();
    const int height = image.height();

    const Image mean_I = ImageProcessing::apply_box_filter(image, radius);
    const Image corr_I = ImageProcessing::apply_box_filter(ImageProcessing::square(image), radius);
    const Image var_I  = ImageProcessing::subtract(corr_I, ImageProcessing::square(mean_I));
    const double I_seed = image.get_pixel(center_x, center_y);

    Image weight_map(width, height, 0.0);
    for (int x = center_x - radius; x <= center_x + radius; ++ x)
    {
        for (int y = center_y - radius; y <= center_y + radius; ++ y)
        {
            if (x < 0 || x >= width || y < 0 || y >= height) continue;

            const double I_j = image.get_pixel(x, y);
            double weight = 0.0;
            for (int k_x = 0; k_x < width; ++ k_x)
            {
                for (int k_y = 0; k_y < height; ++ k_y)
                {
                    bool range_j    = (k_x >= x - radius && k_x <= x + radius) && (k_y >= y - radius && k_y <= y + radius);
                    bool range_seed = (k_x >= center_x - radius && k_x <= center_x + radius) && (k_y >= center_y - radius && k_y <= center_y + radius);
                    if (!range_j || !range_seed) continue;

                    const double mu_k  = mean_I.get_pixel(k_x, k_y);
                    const double var_k = var_I.get_pixel(k_x, k_y);
                    weight += 1.0 + ((I_seed - mu_k) * (I_j - mu_k)) / (epsilon + var_k);
                }
            }
            if (force_positive) weight = std::max(0.0, weight);
            weight_map.set_pixel(x, y, weight);
        }
    }
    weight_map.force_unity();
    return weight_map;
}

Image calculate_gradient_magnitude(const Image& image)
{
    const int width  = image.width();
    const int height = image.height();
    const Image sobel_x = ImageProcessing::apply_sobel_filter_x(image);
    const Image sobel_y = ImageProcessing::apply_sobel_filter_y(image);

    Image gradient_magnitude = Image(width, height);
    for (int x = 0; x < width; ++ x)
    {
        for (int y = 0; y < height; ++ y)
        {
            const double g_x = sobel_x.get_pixel(x, y);
            const double g_y = sobel_y.get_pixel(x, y);
            const double value = std::sqrt(g_x * g_x + g_y * g_y);
            gradient_magnitude.set_pixel(x, y, value);
        }
    }
    return gradient_magnitude;
}

Image apply_guided_filter(const Image& input_image, const ColorImage& guidance_image, int radius, double epsilon)
{
    const int width  = input_image.width();
    const int height = input_image.height();

    assert(width == guidance_image.width());
    assert(height == guidance_image.height());

    const Image I_r = guidance_image.get_r();
    const Image I_g = guidance_image.get_g();
    const Image I_b = guidance_image.get_b();

    const Image mean_I_r = apply_box_filter(I_r, radius);
    const Image mean_I_g = apply_box_filter(I_g, radius);
    const Image mean_I_b = apply_box_filter(I_b, radius);

    const Image mean_p = apply_box_filter(input_image, radius);

    const Image mean_Ip_r = apply_box_filter(multiply(I_r, input_image), radius);
    const Image mean_Ip_g = apply_box_filter(multiply(I_g, input_image), radius);
    const Image mean_Ip_b = apply_box_filter(multiply(I_b, input_image), radius);

    const Image cov_Ip_r = subtract(mean_Ip_r, multiply(mean_I_r, mean_p));
    const Image cov_Ip_g = subtract(mean_Ip_g, multiply(mean_I_g, mean_p));
    const Image cov_Ip_b = subtract(mean_Ip_b, multiply(mean_I_b, mean_p));

    const Image var_I_rr = subtract(apply_box_filter(multiply(I_r, I_r), radius), multiply(mean_I_r, mean_I_r));
    const Image var_I_rg = subtract(apply_box_filter(multiply(I_r, I_g), radius), multiply(mean_I_r, mean_I_g));
    const Image var_I_rb = subtract(apply_box_filter(multiply(I_r, I_b), radius), multiply(mean_I_r, mean_I_b));
    const Image var_I_gg = subtract(apply_box_filter(multiply(I_g, I_g), radius), multiply(mean_I_g, mean_I_g));
    const Image var_I_gb = subtract(apply_box_filter(multiply(I_g, I_b), radius), multiply(mean_I_g, mean_I_b));
    const Image var_I_bb = subtract(apply_box_filter(multiply(I_b, I_b), radius), multiply(mean_I_b, mean_I_b));

    Image a_r(width, height);
    Image a_g(width, height);
    Image a_b(width, height);
    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
    {
        Eigen::Matrix3d sigma;
        sigma << var_I_rr.get_pixel(x, y), var_I_rg.get_pixel(x, y), var_I_rb.get_pixel(x, y),
                 var_I_rg.get_pixel(x, y), var_I_gg.get_pixel(x, y), var_I_gb.get_pixel(x, y),
                 var_I_rb.get_pixel(x, y), var_I_gb.get_pixel(x, y), var_I_bb.get_pixel(x, y);

        const Eigen::Vector3d cov_Ip = { cov_Ip_r.get_pixel(x, y), cov_Ip_g.get_pixel(x, y), cov_Ip_b.get_pixel(x, y) };
        const Eigen::Vector3d a_xy = (sigma + epsilon * Eigen::Matrix3d::Identity()).inverse() * cov_Ip;

        a_r.set_pixel(x, y, a_xy(0));
        a_g.set_pixel(x, y, a_xy(1));
        a_b.set_pixel(x, y, a_xy(2));
    }

    const Image b = subtract(subtract(subtract(mean_p, multiply(a_r, mean_I_r)), multiply(a_g, mean_I_g)), multiply(a_b, mean_I_b));

    Image q = apply_box_filter(b, radius);
    q = add(q, multiply(apply_box_filter(a_r, radius), I_r));
    q = add(q, multiply(apply_box_filter(a_g, radius), I_g));
    q = add(q, multiply(apply_box_filter(a_b, radius), I_b));

    return q;
}

}


// practice 5-8

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/size.hpp>

using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

struct fibo_tag {};
struct fibo_series
{
	typedef fibo_tag tag;
	typedef fibo_series type;
};

template <int Value, int Pos>
struct fibo_iterator
{
	typedef fibo_tag tag;
	typedef typename fibo_iterator type;
	static const int value = Value;
	static const int pos = Pos;
};

template<int N>
struct fibo_val
{
	static const int value =
		fibo_val<N-1>::value +
		fibo_val<N-2>::value;
};

template<>
struct fibo_val<0> : mpl::int_<0>
{};

template<>
struct fibo_val<1> : mpl::int_<1>
{};

namespace boost { namespace mpl {
	template<>
	struct begin_impl< fibo_tag >
	{
		template<class S>
		struct apply
		{
			typedef typename fibo_iterator<0,0> type;
		};
	};
}}

namespace boost { namespace mpl {
	template<>
	struct advance_impl< fibo_tag >
	{
		template<class It, class N>
		struct apply
		{
			typedef typename fibo_iterator<
				fibo_val< It::pos + N::value >::value
				, It::pos + N::value > type;
		};
	};
}}

namespace boost { namespace mpl {
	template<int Value, int PosValue>
	struct deref< fibo_iterator<Value, PosValue> >
	{
		typedef typename fibo_iterator<Value, PosValue> type;
	};
}}

// 0 1 1 2 3 5 8 13 21..
//fibonacci_series
// typedef mpl::advance_c< mpl::begin< fibon >::type, 6 >::type i;
// mpl::deref<i>:;type::value == 8

// typedef mpl::advance_c< i, 4 >::type j;
// mpl::deref<j>::type::value == 55

typedef mpl::advance_c<
	mpl::begin< fibo_series >::type, 6
	>::type i;
BOOST_STATIC_ASSERT( mpl::deref<i>::type::value == 8 );

typedef mpl::advance_c< i, 4>::type j;
BOOST_STATIC_ASSERT( mpl::deref<j>::type::value == 55 );

void main()
{
	int n = mpl::deref<i>::type::value;

}


///////////////////////////////////////////////////////////////////////////////
// ars::detail::tangent_intersection.hpp                                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_DETAIL_TANGENT_INTERSECTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_DETAIL_TANGENT_INTERSECTION_HPP_ER_2009
#include <boost/ars/constant.hpp>
#include <boost/ars/point.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

        //    x1  x2  x3            point abscissae
        //   / \ / \ / \
        //  z0  z1  z2  z3 = zn     tangent abscissae
        //
        // z0 = x_min
        // zn = x_max
        // Let tang[i](x) = y[i] + dy[i](x-x[i])
        // z[i] solves : tang[i+1](z[i]) == tang[i](z[i])
        // t[i] = tang[i](z[i])

template<typename T>
struct tangent_intersection{
    typedef constant<T> const_;

    tangent_intersection()
    :z_(const_::zero_),
    t_(const_::zero_),
    cum_sum_(const_::zero_){}

    tangent_intersection(const point<T>& a,const point<T>& b){
        T eps = exp(const_::lmin_); //TODO numeric_smallest()?
        if(fabs(a.y()-b.y()) <= eps){
            z_ = (a.x() + b.x()) / const_::two_;
            t_ = (a.y() + b.y()) / const_::two_;
        }else{
            T ddy = b.dy() - a.dy();
            // For greater precision :
            if(fabs(a.dy())<fabs(b.dy())){
                z_ = b.x() + (a.y() - b.y() + a.dy() * (b.x() - a.x()))/ddy;
                t_ = a.y() + a.dy() * (z_ - a.x());
            }else{
                z_ = a.x() + (a.y() - b.y() + b.dy() * (b.x() - a.x()))/ddy;
                t_ = b.y() +  b.dy() * (z_ - b.x());
            }
        }
    }

    tangent_intersection(T z,T t)
    :z_(z), t_(t),cum_sum_(const_::zero_){}

    const T& z()const{ return z_; }
    const T& t()const{ return t_; }

    private:
    T z_; //abscissa
    T t_; //ordinate
    public:
    T cum_sum_; //area under exp tangent up to z_
};

template <typename T>
std::ostream&
operator<<(std::ostream& out, const tangent_intersection<T> &ti)
{
    out << '(' << ti.z() << ',' << ti.t() << ',' << ti.cum_sum_ << ')';
    return out;
}

template <typename T>
bool operator<(
    const tangent_intersection<T> &a,
    const tangent_intersection<T> &b
){
    return (a.cum_sum_ < b.cum_sum_);
}

}// ars
}// detail
}// statistics
}// boost

#endif


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

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

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

// Triangulation Example - computes a delaunay triangulation and draws the
// triangulation and the corresponding finite voronoi edges

#include <fstream>
#include <random>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/extensions/triangulation/triangulation.hpp>
#include <boost/geometry/extensions/triangulation/geometries/voronoi_adaptor.hpp>

const int samples = 100;

int main()
{
    namespace bg = boost::geometry;
    typedef bg::model::point<double, 2, bg::cs::cartesian> point;
    typedef bg::model::triangulation<point> triangulation;

    std::default_random_engine gen(1);
    std::uniform_real_distribution<> dist(0.0, 1.0);
    std::vector<point> in;
    for (int i = 0 ; i < samples ; ++i)
    {
        in.push_back(point(dist(gen), dist(gen)));
    }
    triangulation out(samples);
    bg::delaunay_triangulation(in, out);

    std::ofstream svg("triangulation.svg");
    bg::svg_mapper<point> mapper(svg, 720, 720);
    mapper.add(point(0, 0));
    mapper.add(point(1, 1));

    for (auto const& f : bg::face_range(out))
    {
        mapper.map(f, "fill-opacity:0.3;fill:rgb(102,102,201);stroke:rgb(51,51,152);");
    }

    typedef bg::model::voronoi_face_view<triangulation> voronoi_face_view;
    for (auto it = out.vertices_begin() ; it != out.vertices_end() ; ++it)
    {
        voronoi_face_view vfv(out, it);
        mapper.map(voronoi_face_view(out, it), "opacity:1.0;fill:none;stroke:rgb(255,0,0);stroke-width:1");
    }

    return 0;
}


#include "Expression.h"

#include <iostream>

#include <Eigen/Dense>

#include <gmpxx.h>

typedef Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> MatrixXq;

static std::ostream& operator<<(std::ostream & stream, mpq_class const & rop) {
  stream << rop.get_str();
  return stream;
}

Expression Expression::ApplyGaugeSymmetry(Expression const & delta) const {
  Expression gauge_term (*this);
  gauge_term.MultiplyOther(delta);

  gauge_term.SortMonomials();

  gauge_term.EliminateEpsilonEpsilonI();
  gauge_term.EliminateEpsilonI();

  gauge_term.SortMonomials();
  gauge_term.EliminateEtaEta();

  gauge_term.SortMonomials();
  gauge_term.EliminateDelta();

  gauge_term.SortMonomials();
  gauge_term.EliminateEpsilon();

  gauge_term.ApplyMonomialSymmetries();

  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();

  gauge_term.ApplyMonomialSymmetriesToContractions();

  gauge_term.ApplyMonomialSymmetries();
  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();

  gauge_term.EliminateEtaPartial();
  gauge_term.EliminateEpsilon();

  gauge_term.ApplyMonomialSymmetries();
  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();

  gauge_term.EliminateEtaRankOne();
  gauge_term.RenameDummies();

  gauge_term.ApplyMonomialSymmetries();
  gauge_term.SortMonomials();
  gauge_term.SortSummands();
  gauge_term.CollectPrefactors();
  gauge_term.CanonicalisePrefactors();
  gauge_term.EliminateZeros();
  gauge_term.SortSummandsByPrefactors();

  std::cout << "Gauge transform:" << std::endl;
  std::cout << delta.GetLatexString() << std::endl;

  std::cout << "The gauge term reads:" << std::endl;
  std::cout << gauge_term.GetLatexString() << std::endl;

  std::cout << "It has to vanish identically for each possible xi." << std::endl;

  std::set<ScalarSum> sum_set;
  std::set<size_t> coefficient_set;

  std::for_each(gauge_term.summands->begin(), gauge_term.summands->end(),
    [&coefficient_set,&sum_set] (auto const & a) {
      coefficient_set.merge(a.second->CoefficientSet());
      sum_set.insert(*a.second);
    });

  std::map<size_t, size_t> coefficient_map = GetCoefficientMap();

  std::cout << "number of different (not necessarily linear independent) equations : " << sum_set.size() << std::endl;
  std::cout << "number of coefficients (e_.) in these equations                    : " << coefficient_map.size() << std::endl;

  std::vector<std::vector<Rational>> matrix;
  std::for_each(sum_set.begin(), sum_set.end(), [&coefficient_map, &matrix](auto & a) {
    matrix.push_back(a.CoefficientVector(coefficient_map));
    });

  MatrixXq mq(matrix.size(), coefficient_map.size());

  for (size_t row_counter = 0; row_counter < matrix.size(); ++row_counter) {
    for (size_t column_counter = 0; column_counter < coefficient_map.size(); ++column_counter) {
      Fraction frac = matrix[row_counter][column_counter].get_fraction();
      mq(row_counter, column_counter) = mpq_class(frac.first, frac.second);
    }
  }

  Eigen::FullPivLU<MatrixXq> lu_decompq(mq);

  std::cout << "the rank of the matrix is : " << lu_decompq.rank() << std::endl;
  std::cout << "null space basis: " << std::endl;

  MatrixXq kq = lu_decompq.kernel();

  std::cout << kq << std::endl;

  std::map<size_t, size_t> coefficient_rmap;
  std::for_each(coefficient_map.begin(), coefficient_map.end(), [&coefficient_rmap](auto a) { coefficient_rmap.insert(std::make_pair(a.second, a.first)); });

  std::map<size_t, ScalarSum> replace_map;

  std::vector<std::vector<mpq_class>> kq_vec (kq.rows());
  std::generate(kq_vec.begin(), kq_vec.end(),
    [n=0,&kq] () mutable {
      std::vector<mpq_class> vec_ret (kq.cols());
      std::generate(vec_ret.begin(), vec_ret.end(),
      [m=0,&kq,_n=n++] () mutable {
        return kq(_n,m++);
      });
      return vec_ret;
    });

  std::for_each(kq_vec.cbegin(), kq_vec.cend(),
    [&replace_map,coefficient_rmap,n=0] (auto const & a) mutable {
      ScalarSum scalar_sum;
      std::for_each(a.cbegin(), a.cend(),
        [&scalar_sum, m=0] (auto const & b) mutable {
          scalar_sum.AddScalar(Scalar(Rational(b), ++m));
        });
      replace_map[coefficient_rmap[n++]] = scalar_sum;
    });

  std::cout << "Thus the " << coefficient_map.size() << " original constants can be parameterized as:" << std::endl;
  std::for_each(replace_map.begin(), replace_map.end(),
    [](auto const & a) {
      std::cout << "e_" << a.first << " --> " << a.second.ToString() << std::endl;
    });

  std::cout << "With " << kq.cols() << " remaining constant" << (kq.cols() == 1 ? "" : "(s)") << "." << std::endl;

  Expression expression_copy = *this;

  expression_copy.SubstituteVariables(replace_map);

  expression_copy.CanonicalisePrefactors();
  expression_copy.EliminateZeros();

  return Expression(expression_copy);
}