/*
* 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 <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

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 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,
                    double alphaLine1 = 1.0, double alphaLine2 = 1.0 )
                    :   line1( line1In ), line2( line2In ),
                        a( a ), b( b ), c( c ), d( d ),
                        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;
    }

	/**
	* 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 );
    }

private:

	/**
	* 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
	*/
    // Was public when wanted to look at details of projection, etc
    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;

        //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 );

        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() );
        }

    }

    // 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;
    // }

    // const std::vector< int > * getConstPtrVerticesIndices( const bool isLine1 )
    // {
    //     if ( isLine1 )
    //         return & ( hull1PointIndices.indices );
    //     else
    //         return & ( hull2PointIndices.indices );
    // }

    // const std::vector< uint64_t > * getConstPtrlineInBothHullPointIndices( const bool isLine1 )
    // {
    //     if ( isLine1 )
    //         return & ( line1InBothHullPointIndices );
    //     else
    //         return & ( line2InBothHullPointIndices );
    // }

	/**
	* 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 );
    }

	/**
	* 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;

    /**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


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

namespace dr {

template<typename Scalar, int AmbientDim, int MatrixOptions, int PlaneOptions>
Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> rejection(
	Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> const & vector,
	Eigen::Hyperplane<Scalar, AmbientDim, PlaneOptions> const & plane
) {
	return vector - plane.projection(vector);
}

template<typename Scalar, int AmbientDim, int MatrixOptions, int PlaneOptions>
Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> reflection(
	Eigen::Matrix<Scalar, AmbientDim, 1, MatrixOptions> const & vector,
	Eigen::Hyperplane<Scalar, AmbientDim, PlaneOptions> const & plane
) {
	return vector - 2 * rejection(vector, plane);
}

}


#pragma once

#include "coma/Core"
#include <Eigen/Core>
#include <RBDyn/MultiBody.h>

template <int Order>
Eigen::MatrixXd makeDiag(const Eigen::MatrixXd& mat)
{
    constexpr int ord = Order;
    Eigen::MatrixXd out = Eigen::MatrixXd::Zero(ord * mat.rows(), ord * mat.cols());
    for (int i = 0; i < ord; ++i)
        out.block(i * mat.rows(), i * mat.cols(), mat.rows(), mat.cols()) = mat;

    return out;
}

template <typename CrossNOp>
Eigen::MatrixXd generateD(const CrossNOp& cx)
{
    constexpr int n_vec = coma::internal::traits<CrossNOp>::n_vec;
    using Scalar = typename coma::internal::traits<CrossNOp>::Scalar;
    using mat_t = Eigen::Matrix<Scalar, 6 * n_vec, 6 * n_vec>;
    using sub_mat_t = Eigen::Matrix<Scalar, 6, 6 * n_vec>;
    Eigen::MatrixXd D_N = Eigen::MatrixXd::Zero(6 * (n_vec - 1), 6 * (n_vec - 1));
    for (int i = 0; i < n_vec - 1; ++i)
        D_N.block<6, 6>(6 * i, 6 * i) = Eigen::Matrix6d::Identity() / (i + 1);
    return mat_t::Identity() + (mat_t() << sub_mat_t::Zero(), D_N * cx.dualMatrix().template topRows<6 * (n_vec - 1)>()).finished();
}

// M = I + Cd * I * C
template <typename Tree>
std::vector<Eigen::MatrixXd> getSubTreeInertia(const rbd::MultiBody& mb, const Tree& tree)
{
    constexpr int ord = Tree::order;
    std::vector<Eigen::MatrixXd> M(mb.nrBodies(), Eigen::MatrixXd::Zero(6 * ord, 6 * ord));
    const auto& bodies = mb.bodies();
    const auto& pred = mb.predecessors();
    for (int i = mb.nrBodies() - 1; i >= 0; --i) {
        M[i] += makeDiag<ord>(bodies[i].inertia().matrix());
        int p = pred[i]; // parent
        if (p != -1) {
            auto C_p_b = tree.links[p].inverse() * tree.links[i];
            M[p] += C_p_b.template dualMatrix<ord>() * M[i] * C_p_b.inverse().template matrix<ord>();
        }
    }

    return M;
}

namespace detail {

template <typename Transf, typename CMTM, size_t Order>
struct CMTMSetter {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints);
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 0> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T);
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 1> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 2> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 3> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 4> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 5> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 6> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 7> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 8> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 9> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 10> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof),
            S * dqs[9].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 11> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof),
            S * dqs[9].col(t).segment(pos, dof), S * dqs[10].col(t).segment(pos, dof));
    }
};

template <typename Transf, typename CMTM>
struct CMTMSetter<Transf, CMTM, 12> {
    static void impl(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
    {
        joints.set(T, S * dqs[0].col(t).segment(pos, dof), S * dqs[1].col(t).segment(pos, dof), S * dqs[2].col(t).segment(pos, dof),
            S * dqs[3].col(t).segment(pos, dof), S * dqs[4].col(t).segment(pos, dof), S * dqs[5].col(t).segment(pos, dof),
            S * dqs[6].col(t).segment(pos, dof), S * dqs[7].col(t).segment(pos, dof), S * dqs[8].col(t).segment(pos, dof),
            S * dqs[9].col(t).segment(pos, dof), S * dqs[10].col(t).segment(pos, dof), S * dqs[11].col(t).segment(pos, dof));
    }
};

}

template<typename Transf, typename CMTM, size_t Order>
void CMTMSet(const Transf& T, const Eigen::MatrixXd& S, const std::vector<Eigen::MatrixXd>& dqs, int t, int pos, int dof, CMTM& joints)
{
    detail::CMTMSetter<Transf, CMTM, Order>::impl(T, S, dqs, t, pos, dof, joints);
}


// -----------------------------------------------------------
// integer_log2.hpp
//
//   Gives the integer part of the logarithm, in base 2, of a
// given number. Behavior is undefined if the argument is <= 0.
//
//         Copyright (c) 2003-2004, 2008 Gennaro Prota
//
// 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_INTEGER_INTEGER_LOG2_HPP
#define BOOST_INTEGER_INTEGER_LOG2_HPP

#include <assert.h>
#ifdef __BORLANDC__
#include <climits>
#endif
#include <boost/limits.hpp>
#include <boost/config.hpp>

namespace boost {
 namespace detail {

  template <typename T>
  int integer_log2_impl(T x, int n) {

      int result = 0;

      while (x != 1) {

          const T t = static_cast<T>(x >> n);
          if (t) {
              result += n;
              x = t;
          }
          n /= 2;

      }

      return result;
  }

  // helper to find the maximum power of two
  // less than p (more involved than necessary,
  // to avoid PTS)
  //
  template <int p, int n>
  struct max_pow2_less {

      enum { c = 2*n < p };

      BOOST_STATIC_CONSTANT(int, value =
          c ? (max_pow2_less< c*p, 2*c*n>::value) : n);

  };

  template <>
  struct max_pow2_less<0, 0> {

      BOOST_STATIC_CONSTANT(int, value = 0);
  };

  // this template is here just for Borland :(
  // we could simply rely on numeric_limits but sometimes
  // Borland tries to use numeric_limits<const T>, because
  // of its usual const-related problems in argument deduction
  // - gps
  template <typename T>
  struct width {

#ifdef __BORLANDC__
      BOOST_STATIC_CONSTANT(int, value = sizeof(T) * CHAR_BIT);
#else
      BOOST_STATIC_CONSTANT(int, value = (std::numeric_limits<T>::digits));
#endif

  };

 } // detail

 // ---------
 // integer_log2
 // ---------------
 //
 template <typename T>
 int integer_log2(T x) {

     assert(x > 0);

     const int n = detail::max_pow2_less<
                     detail::width<T> :: value, 4
                   > :: value;

     return detail::integer_log2_impl(x, n);

 }

}

#endif // include guard


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing sinpi capabilities

    sine of angle in \f$\pi\f$ multiples.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = sinpi(x);
    @endcode

    is similar to:

    @code
    T r = sin(Pi<T>()*x);
    @endcode

    @see sine, sincospi, sin, sind

  **/
  const boost::dispatch::functor<tag::sinpi_> sinpi = {};
} }
#endif

#include <boost/simd/function/scalar/sinpi.hpp>
#include <boost/simd/function/simd/sinpi.hpp>

#endif


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object computes the integer conversion of the @ref round of its parameter.

    @par Header <boost/simd/function/iround.hpp>

    @par Note:
    This operation is properly saturated

    @see round, iround, iround, ifix, inearbyint

    @par Example:

      @snippet iround.cpp iround

    @par Possible output:

      @snippet iround.txt iround

  **/
  as_integer_t<Value> iround(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/iround.hpp>
#include <boost/simd/function/simd/iround.hpp>

#endif


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Clearpath Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#include <fuse_models/unicycle_2d_state_cost_function.h>
#include <fuse_models/unicycle_2d_state_cost_functor.h>

#include <benchmark/benchmark.h>

#include <ceres/autodiff_cost_function.h>
#include <Eigen/Dense>

#include <vector>

class Unicycle2DStateCostFunction : public benchmark::Fixture
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Unicycle2DStateCostFunction()
    : jacobians(num_parameter_blocks)
    , J(num_parameter_blocks)
  {
    for (size_t i = 0; i < num_parameter_blocks; ++i)
    {
      J[i].resize(num_residuals, block_sizes[i]);
      jacobians[i] = J[i].data();
    }
  }

  // Analytic cost function
  static constexpr double dt{ 0.1 };
  static const fuse_core::Matrix8d sqrt_information;

  static const fuse_models::Unicycle2DStateCostFunction cost_function;

  // Parameters
  static const double* parameters[];

  // Residuals
  fuse_core::Vector8d residuals;

  static const std::vector<int32_t>& block_sizes;
  static const size_t num_parameter_blocks;

  static const size_t num_residuals;

  // Jacobians
  std::vector<double*> jacobians;

private:
  // Cost function process noise and covariance
  static const double process_noise_diagonal[];

  static const fuse_core::Matrix8d covariance;

  // Parameter blocks
  static const double position1[];
  static const double yaw1[];
  static const double vel_linear1[];
  static const double vel_yaw1[];
  static const double acc_linear1[];

  static const double position2[];
  static const double yaw2[];
  static const double vel_linear2[];
  static const double vel_yaw2[];
  static const double acc_linear2[];

  // Jacobian matrices
  std::vector<fuse_core::MatrixXd> J;
};

// Cost function process noise and covariance
const double Unicycle2DStateCostFunction::process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 };

const fuse_core::Matrix8d Unicycle2DStateCostFunction::covariance =
    fuse_core::Vector8d(process_noise_diagonal).asDiagonal();

// Parameter blocks
const double Unicycle2DStateCostFunction::position1[] = { 0.0, 0.0 };
const double Unicycle2DStateCostFunction::yaw1[] = {0.0};
const double Unicycle2DStateCostFunction::vel_linear1[] = {1.0, 0.0};
const double Unicycle2DStateCostFunction::vel_yaw1[] = {1.570796327};
const double Unicycle2DStateCostFunction::acc_linear1[] = {1.0, 0.0};

const double Unicycle2DStateCostFunction::position2[] = {0.105, 0.0};
const double Unicycle2DStateCostFunction::yaw2[] = {0.1570796327};
const double Unicycle2DStateCostFunction::vel_linear2[] = {1.1, 0.0};
const double Unicycle2DStateCostFunction::vel_yaw2[] = {1.570796327};
const double Unicycle2DStateCostFunction::acc_linear2[] = {1.0, 0.0};

// Analytic cost function
const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt().matrixU());

const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{ dt, sqrt_information };

// Parameters
const double* Unicycle2DStateCostFunction::parameters[] = {  // NOLINT(whitespace/braces)
  position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2
};

const std::vector<int32_t>& Unicycle2DStateCostFunction::block_sizes = cost_function.parameter_block_sizes();
const size_t Unicycle2DStateCostFunction::num_parameter_blocks = block_sizes.size();

const size_t Unicycle2DStateCostFunction::num_residuals = cost_function.num_residuals();

BENCHMARK_F(Unicycle2DStateCostFunction, AnalyticUnicycle2DCostFunction)(benchmark::State& state)
{
  for (auto _ : state)
  {
    cost_function.Evaluate(parameters, residuals.data(), jacobians.data());
  }
}

BENCHMARK_F(Unicycle2DStateCostFunction, AutoDiffUnicycle2DStateCostFunction)(benchmark::State& state)
{
  // Create cost function using automatic differentiation on the cost functor
  ceres::AutoDiffCostFunction<fuse_models::Unicycle2DStateCostFunctor, 8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2>
      cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information));

  for (auto _ : state)
  {
    cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data());
  }
}

BENCHMARK_MAIN();


#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/platform/default/logging.h"
#include "tensorflow/core/framework/shape_inference.h"
#include "tensorflow/core/framework/tensor_types.h"

#include <Eigen/Core>

#include "celerite.h"

using namespace tensorflow;

static const char kErrMsg[] =
    "Cholesky decomposition failed";

REGISTER_OP("CeleriteFactor")
  .Attr("T: {float, double}")
  .Input("a: T")
  .Input("u: T")
  .Input("v: T")
  .Input("p: T")
  .Output("d: T")
  .Output("w: T")
  .Output("s: T")
  .SetShapeFn([](shape_inference::InferenceContext* c) {

    shape_inference::DimensionHandle J;
    shape_inference::ShapeHandle a, u, v, p;

    TF_RETURN_IF_ERROR(c->WithRank(c->input(0), 1, &a));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(1), 2, &u));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(2), 2, &v));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(3), 2, &p));
    TF_RETURN_IF_ERROR(c->Merge(u, v, &u));

    J = c->Dim(u, 1);
    TF_RETURN_IF_ERROR(c->Multiply(J, J, &J));
    TF_RETURN_IF_ERROR(c->ReplaceDim(u, 1, J, &u));

    c->set_output(0, c->input(0));
    c->set_output(1, c->input(1));
    c->set_output(2, u);

    return Status::OK();
  });

template <typename T>
class CeleriteFactorOp : public OpKernel {
 public:
  explicit CeleriteFactorOp(OpKernelConstruction* context) : OpKernel(context) {}

  void Compute(OpKernelContext* context) override {
    typedef Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>> c_vector_t;
    typedef Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> c_matrix_t;
    typedef Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> vector_t;
    typedef Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> matrix_t;

    const Tensor& a_t = context->input(0);
    const Tensor& U_t = context->input(1);
    const Tensor& V_t = context->input(2);
    const Tensor& P_t = context->input(3);

    OP_REQUIRES(context, (U_t.dims() == 2),
          errors::InvalidArgument("U should have the shape (N, J)"));

    int64 N = U_t.dim_size(0),
          J = U_t.dim_size(1);

    OP_REQUIRES(context, ((a_t.dims() == 1) && (a_t.dim_size(0) == N)),
        errors::InvalidArgument("a should have the shape (N)"));
    OP_REQUIRES(context, ((V_t.dims() == 2) && (V_t.dim_size(0) == N) && (V_t.dim_size(1) == J)),
        errors::InvalidArgument("V should have the shape (N, J)"));
    OP_REQUIRES(context, ((P_t.dims() == 2) && (P_t.dim_size(0) == N-1) && (P_t.dim_size(1) == J)),
        errors::InvalidArgument("P should have the shape (N-1, J)"));

    const auto U = c_matrix_t(U_t.template flat<T>().data(), N, J);
    const auto a = c_vector_t(a_t.template flat<T>().data(), N);
    const auto V = c_matrix_t(V_t.template flat<T>().data(), N, J);
    const auto P = c_matrix_t(P_t.template flat<T>().data(), N-1, J);

    // Create the outputs
    Tensor* d_t = NULL;
    Tensor* W_t = NULL;
    Tensor* S_t = NULL;

    OP_REQUIRES_OK(context, context->allocate_output(0, TensorShape({N}), &d_t));
    OP_REQUIRES_OK(context, context->allocate_output(1, TensorShape({N, J}), &W_t));
    OP_REQUIRES_OK(context, context->allocate_output(2, TensorShape({N, J*J}), &S_t));

    auto d = vector_t(d_t->template flat<T>().data(), N);
    auto W = matrix_t(W_t->template flat<T>().data(), N, J);
    auto S = matrix_t(S_t->template flat<T>().data(), N, J*J);

    d = a;
    W = V;
    int flag = celerite::factor(U, P, d, W, S);
    OP_REQUIRES(context, flag == 0, errors::InvalidArgument(kErrMsg));
  }
};

#define REGISTER_KERNEL(type)                                              \
  REGISTER_KERNEL_BUILDER(                                                 \
      Name("CeleriteFactor")                                               \
      .Device(DEVICE_CPU)                                                  \
      .TypeConstraint<type>("T"),                                          \
      CeleriteFactorOp<type>)

REGISTER_KERNEL(float);
REGISTER_KERNEL(double);

#undef REGISTER_KERNEL


#ifndef BOOST_MATH_INTERPOLATORS_QUINTIC_HERMITE_HPP
#define BOOST_MATH_INTERPOLATORS_QUINTIC_HERMITE_HPP
#include <algorithm>
#include <stdexcept>
#include <memory>
#include <boost/math/interpolators/detail/quintic_hermite_detail.hpp>

namespace boost::math::interpolators {

template<class RandomAccessContainer>
class quintic_hermite {
public:
    using Real = typename RandomAccessContainer::value_type;
    quintic_hermite(RandomAccessContainer && x, RandomAccessContainer && y, RandomAccessContainer && dydx, RandomAccessContainer && d2ydx2)
     : impl_(std::make_shared<detail::quintic_hermite_detail<RandomAccessContainer>>(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2)))
    {}

    Real operator()(Real x) const {
        return impl_->operator()(x);
    }

    Real prime(Real x) const {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const quintic_hermite & m)
    {
        os << *m.impl_;
        return os;
    }

    void push_back(Real x, Real y, Real dydx, Real d2ydx2) {
        impl_->push_back(x, y, dydx, d2ydx2);
    }

private:
    std::shared_ptr<detail::quintic_hermite_detail<RandomAccessContainer>> impl_;
};
}
#endif


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file stereo_camera_model_tests.cpp
 * \brief
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#include <gtest/gtest.h>

#include <chrono>
#include <cmath>
#include <random>

#include <Eigen/Geometry>

#include <vtr_logging/logging_init.hpp>
#include <vtr_vision/outliers.hpp>
#include <vtr_vision/sensors/stereo_transform_model.hpp>

using namespace vtr::vision;

TEST(Vision, stereoModel1) {
  // Create some intrinsics
  double baseline = 0.1;
  double focal = 1000;
  double cx = 500;
  double cy = 500;
  CameraIntrinsic intrinsics;
  intrinsics << focal, 0, cx, 0, focal, cy, 0, 0, 1;

  // Create the camera matrices
  CameraProjection projection_l = intrinsics * CameraProjection::Identity();
  CameraProjection projection_r = CameraProjection::Identity();
  projection_r(0, 3) = baseline;
  projection_r = intrinsics * projection_r;

  // Weird ref pointer unsafe type conversion (old)
  // Eigen::MatrixXd test (5,5);
  // const Eigen::Matrix<double,3,Eigen::Dynamic>& test_ref = test;
  // const Eigen::Matrix<double,3,Eigen::Dynamic>* test_ref_ptr = &test_ref; //
  // WTF? const Eigen::Matrix<double,3,Eigen::Dynamic>* test_ptr = &test; //
  // Doesn't compile

  // Create the ground truth transformation matrix
  // Will transform points in reference frame to camera frame
  Eigen::Affine3d tf = Eigen::Affine3d::Identity();
  tf.translation() << 0, 0.5, -0.8;

  // Create the two point clouds
  const unsigned int n_inliers = 4;
  const unsigned int n_outliers = 3;
  const unsigned int n_pts = n_inliers + n_outliers;
  Eigen::Matrix<double, 3, Eigen::Dynamic> pts_ref(3, n_pts), pts_cam(3, n_pts);
  pts_ref << 1, 0, 1, 0, 2, 3, -2, 0, 0, 0, -1, 1, 0, -1, 3, 4, 3, 3, 4, 5, 6;
  pts_cam = tf * pts_ref;

  // Corrupt the outlier points
  for (unsigned int i = n_inliers; i < n_pts; ++i) {
    pts_cam(1, i) += (i % 2) ? -1 : 1;
    pts_cam(2, i) += i % 3;
  }

  // Log the point clouds
  //    INFO( "ref:\n" << pts_ref
  //          << "\ncam:\n" << pts_cam
  //          << "\n" );
  EXPECT_EQ(tf * pts_ref.leftCols<n_inliers>(), pts_cam.leftCols<n_inliers>());
  EXPECT_NE(tf * pts_ref.rightCols<n_outliers>(),
            pts_cam.rightCols<n_outliers>());

  // Create the match pairs (just 1-1)
  SimpleMatches matches;
  matches.reserve(n_pts);
  for (unsigned int i = 0; i < n_pts; ++i) matches.push_back(SimpleMatch(i, i));

  // Create the camera model
  StereoTransformModel::Ptr model = std::make_shared<StereoTransformModel>();
  model->setCalibration(projection_l, projection_r);
  model->setCalibration(
      intrinsics, baseline);  // double check that overloaded constructor works
  model->setPoints(&pts_ref, &pts_cam);

  // Set up RANSAC
  auto verifier =
      std::make_shared<VerifySampleIndices>();  // Need 1 close feature
  auto sampler = std::make_shared<BasicSampler>(verifier);
  VanillaRansac<StereoTransformModel::SolutionType> ransac(sampler);
  ransac.setCallback(model);

  // Run RANSAC
  Eigen::Matrix4d tf_found;
  SimpleMatches inliers;
  unsigned int n_inliers_found = ransac.run(matches, &tf_found, &inliers);

  EXPECT_EQ(n_inliers_found, n_inliers);
  LOG(INFO) << "tf:\n" << tf.matrix() << "\nfound:\n" << tf_found << "\n";
  EXPECT_NEAR((tf.matrix() - tf_found).norm(), 0, std::pow(10, -9));
}

TEST(Vision, stereoModel2) {
  std::default_random_engine eng;
  eng.seed(std::chrono::system_clock::now().time_since_epoch().count());
  // Create some intrinsics
  double baseline = 0.24;
  std::uniform_real_distribution<> focal_dis(1000, 3000);
  double focal = focal_dis(eng);
  double width = 1024;
  double height = 768;
  double cx = width / 2.0;
  double cy = height / 2.0;
  CameraIntrinsic intrinsics;
  intrinsics << focal, 0, cx, 0, focal, cy, 0, 0, 1;
  // double hfov = 2*atan2(0.5*width,focal);
  // double vfov = 2*atan2(0.5*height,focal);
  // Create the camera matrices
  CameraProjection projection_l = intrinsics * CameraProjection::Identity();
  CameraProjection projection_r = CameraProjection::Identity();
  projection_r(0, 3) = baseline;
  projection_r = intrinsics * projection_r;

  // Create the ground truth transformation matrix
  // Will transform points in reference frame to camera frame
  Eigen::Affine3d tf = Eigen::Affine3d::Identity();
  std::uniform_real_distribution<> trans_dis(-1.0, 1.0);
  tf.translation() << trans_dis(eng), trans_dis(eng), trans_dis(eng);

  // Create the two point clouds
  const unsigned int n_inliers = 500;
  const unsigned int n_outliers = 500;
  const unsigned int n_pts = n_inliers + n_outliers;
  Eigen::Matrix<double, 3, Eigen::Dynamic> pts_ref(3, n_pts), pts_cam(3, n_pts);
  std::uniform_real_distribution<> x_dis(-10.0, 10.0);
  std::uniform_real_distribution<> y_dis(0.5, 1.0);
  std::uniform_real_distribution<> z_dis(3.0, 50.0);
  tf.translation() << trans_dis(eng), trans_dis(eng), trans_dis(eng);
  for (unsigned ii = 0; ii < n_pts; ii++) {
    pts_ref.col(ii) << x_dis(eng), y_dis(eng), z_dis(eng);
    pts_cam.col(ii) = tf * pts_ref.col(ii);
  }

  // Corrupt the outlier points
  std::normal_distribution<> sample_dis(0.0, 0.5);
  for (unsigned i = n_inliers; i < n_pts; ++i) {
    pts_cam(0, i) += sample_dis(eng);
    pts_cam(1, i) += sample_dis(eng);
    pts_cam(2, i) += sample_dis(eng);
  }
  // Log the point clouds
  //    INFO( "ref:\n" << pts_ref
  //          << "\ncam:\n" << pts_cam
  //          << "\n" );
  EXPECT_EQ(tf * pts_ref.leftCols<n_inliers>(), pts_cam.leftCols<n_inliers>());
  EXPECT_NE(tf * pts_ref.rightCols<n_outliers>(),
            pts_cam.rightCols<n_outliers>());

  // Create the match pairs (just 1-1)
  SimpleMatches matches;
  matches.reserve(n_pts);
  for (unsigned int i = 0; i < n_pts; ++i) matches.push_back(SimpleMatch(i, i));

  // Create the camera model
  StereoTransformModel::Ptr model = std::make_shared<StereoTransformModel>();
  model->setCalibration(projection_l, projection_r);
  model->setCalibration(
      intrinsics, baseline);  // double check that overloaded constructor works
  model->setPoints(&pts_ref, &pts_cam);

  // Set up RANSAC
  auto verifier =
      std::make_shared<VerifySampleIndices>();  // Need 1 close feature
  auto sampler = std::make_shared<BasicSampler>(verifier);
  VanillaRansac<StereoTransformModel::SolutionType> ransac(sampler, 3.5, 5.0,
                                                           100, 0.25, 150);
  ransac.setCallback(model);

  // Run RANSAC
  Eigen::Matrix4d tf_found;
  SimpleMatches inliers;
  int n_inliers_found = ransac.run(matches, &tf_found, &inliers);

  EXPECT_GE(n_inliers_found, 0.9 * n_inliers);
  LOG(INFO) << "tf:\n" << tf.matrix() << "\nfound:\n" << tf_found << "\n";
  EXPECT_NEAR((tf.matrix() - tf_found).norm(), 0, std::pow(10, -9));
}


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REC_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/fast.hpp>

#include <boost/simd/constant/one.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return One<A0>()/a0;
    }
  };
} } }

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

#endif


#include <math.h>
#include <vfh_rover/PathParams.h>
#include <vfh_rover/VFHistogram.h>
#include <vfh_rover/Vehicle.h>
#include <string>
#include <assert.h>
#include <iostream>
#include <cmath>
#include <pcl/point_types.h>
#include <geometry_msgs/Pose.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace Eigen;

VFHistogram::VFHistogram(boost::shared_ptr<octomap::OcTree> tree, Vehicle &v,
    float maxRange, float alpha) :
  PolarHistogram(alpha, v.x, v.y, v.z)
{
  for (int i = 0; i < getWidth() * getHeight(); i++) {
    data[i] = NAN;
  }
  octomath::Vector3 min (v.min().x()-maxRange,
      v.min().y()-maxRange,
      v.min().z()-maxRange);
  octomath::Vector3 max (v.max().x()+maxRange,
      v.max().y()+maxRange,
      v.max().z()+maxRange);

  // init variables for calculations
  float rad = tree->getResolution()+v.radius()+v.safety_radius; // voxel radius
  //double resolution = 0.05;

  // Add voxels to tree
  for (octomap::OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(min, max, 14),
      end=tree->end_leafs_bbx(); it!=end; ++it) {
    octomath::Vector3 pos = it.getCoordinate();
    //float val = (it->getValue()>0) ? it->getValue() : 0;
    float val = it->getValue();
    if (val > 0) {
      if (!isIgnored(pos.x(), pos.y(), pos.z(), maxRange)) {
        addVoxel(pos.x(), pos.y(), pos.z(), val, rad, maxRange);
        //h.addVoxel(pos.x(), pos.y(), pos.z(), val);
        checkTurning(pos.x(), pos.y(), pos.z(), val, v, rad);
      }
    } else {
      addVoxel(pos.x(), pos.y(), pos.z(), 0, rad, maxRange);
    }
  }
}

void VFHistogram::addVoxel(float x, float y, float z, float val) {
  int az = getI(x, y);
  int el = getJ(x, y, z);
  addValue(az, el, val);
}

void VFHistogram::addVoxel(float x, float y, float z, float val,
    float voxel_radius, float maxRange) {
  // For weight calculation
  float dist = sqrt(pow(x-ox, 2) +
      pow(y-oy, 2) +
      pow(z-oz, 2));
  float enlargement = asin(voxel_radius/dist);
  //float enlargement = degrees*M_PI/180;
  // Calc voxel weight
  float a = 0.5;
  float b = 4*(a-1)/pow(maxRange-1, 2);
  float h = val*val*(a-b*(dist));

  int bz = getI(x, y);
  int be = getJ(x, y, z);
  std::cout << be << std::endl;
  int voxelCellSize = int(enlargement/alpha); // divided by 2
  int az,el;
  addValues(h, bz-voxelCellSize, be-voxelCellSize,
      2*voxelCellSize, 2*voxelCellSize);
}

void VFHistogram::checkTurning(float x, float y, float z, float val,
    Vehicle &v, float voxel_radius) {
  // Iterate over half possible ways the rover can move (then check left and right
  int j = getJ(x,y,z);
  int i = getI(x,y);
  float turningLeftCenterX = v.turningRadiusR()*sin(v.getHeading());
  float turningRightCenterX = -v.turningRadiusR()*sin(v.getHeading());
  float turningLeftCenterY = v.turningRadiusL()*cos(v.getHeading());
  float turningRightCenterY = -v.turningRadiusL()*cos(v.getHeading());
  float dr = sqrt(pow((turningRightCenterX - (x-ox)), 2) +
      pow((turningRightCenterY - (x-ox)), 2));
  float dl = sqrt(pow((turningLeftCenterX - (x-ox)), 2) +
      pow((turningLeftCenterY - (x-ox)), 2));
  //float rad = v.safety_radius+v.radius()+voxel_radius;
  float rad = voxel_radius;
  if (dr < v.turningRadiusR()+rad || dl < v.turningRadiusL()+rad)
    addValue(i, j, val);
}

// Determine unoccupied paths
std::vector<geometry_msgs::Pose> VFHistogram::findPaths(int width, int height) {
  float ret_vals[width*height];
  std::vector<geometry_msgs::Pose> ps;
  float az, el;
  for (int i=0; i<getWidth(); i++) {
    for (int j=0; j<getHeight(); j++) {
      getValues(ret_vals, i, j, width, height);
      //float s = sum(ret_vals, width*height);
      bool empty = true;
      for (int k = 0; k < width*height; k++) {
        if (abs(ret_vals[k]) > 0.0001 || ret_vals[k] != ret_vals[k])
          empty = false;
      }
      //std::cout << s << std::endl;
      //if (abs(s) < 0.0001) {
      if (empty) {
        az = -(i+(float)width/2-getWidth()/4)*alpha;
        el = M_PI/2-(j+(float)height/2-1)*alpha;

        geometry_msgs::Pose p;
        // Conversion to quaternion ripped from wikipedia
        // Abbreviations for the various angular functions
        double cy = cos(az * 0.5);
        double sy = sin(az * 0.5);
        double cr = cos(0 * 0.5); // roll = 0
        double sr = sin(0 * 0.5); // roll = 0
        double cp = cos(el * 0.5);
        double sp = sin(el * 0.5);

        p.orientation.w = cy * cr * cp + sy * sr * sp;
        p.orientation.x = cy * sr * cp - sy * cr * sp;
        p.orientation.y = cy * cr * sp + sy * sr * cp;
        p.orientation.z = sy * cr * cp - cy * sr * sp;

        p.position.x = ox;
        p.position.y = oy;
        p.position.z = oz;
        ps.push_back(p);
      }
    }
    }
    return ps;
  }

  // Choose a path from a list of paths
  geometry_msgs::Pose* VFHistogram::optimalPath(Vehicle &v, geometry_msgs::Pose goal, PathParams p, std::vector<geometry_msgs::Pose>* openPoses) {
    // If no openPoses poses are found
    if (openPoses->size() == 0) {
      std::cout << "No open positions" << std::endl;
      return NULL;
    }
    // Difference to goal
    float vals[openPoses->size()];
    float dx = goal.position.x - v.x;
    float dy = goal.position.y - v.y;
    float dz = goal.position.z - v.z;
    // If we are at our goal
    if (sqrt(fabs(dx*dx+dy*dy+dz*dz)) < p.goal_radius) {
      std::cout << "Reached Goal" << std::endl;
      return NULL;
    }
    // Find angle to goal
    Quaternionf goalQ =
      AngleAxisf(atan2(dy, dx), Vector3f::UnitZ()) *
      AngleAxisf(-atan2(dz, dx), Vector3f::UnitY());
    // Calculate angle to previous path
    Quaternionf prevQ;

    if (v.prevHeading != NULL && v.prevSet == true) {
      prevQ = Quaternionf(v.prevHeading->orientation.w,
          v.prevHeading->orientation.x,
          v.prevHeading->orientation.y,
          v.prevHeading->orientation.z);
    }
    // std::cout << "--------------------------" << std::endl;
    // Iterate through each goal and calculate a score
    for (int i = 0; i < openPoses->size(); i++) {
      // Convert to quaternion
      geometry_msgs::Pose * path = &openPoses->at(i);
      Quaternionf pathQ (path->orientation.w, path->orientation.x,
          path->orientation.y, path->orientation.z);
      // Calc each difference score
      float prevDiff;
      if(v.prevSet)
        prevDiff = pathQ.angularDistance(prevQ);
      else
        prevDiff = 0;
      float goalDiff = pathQ.angularDistance(goalQ);
      float headDiff = pathQ.angularDistance(v.orientation);
      // Calc elevation of path
      float el = pathQ.toRotationMatrix().eulerAngles(2, 1, 0)[1];
      // Weigh incline
      float elScore = (fabs((M_PI/2 - fabs(el))) < 1.5 ? 10000000 : 1) * ((M_PI-fabs(el)) > (fabs(el)) ? fabs(el) : (M_PI - fabs(el)));
      // Check bounds
      //if(el > 0)
      //std::cout << el << std::endl;
      /*if(fabs(M_PI/2 - el) < 0.8)
        exit(0);*/

      float totalScore = - prevDiff*p.prevWeight - goalDiff*p.goalWeight - headDiff*p.headingWeight - elScore*p.elevationWeight;
      vals[i] = totalScore;
    }
    // Return best path
    geometry_msgs::Pose* bestPath = &openPoses->at(maxInd(vals, openPoses->size()));
    Quaternionf printPath (bestPath->orientation.w, bestPath->orientation.x,
        bestPath->orientation.y, bestPath->orientation.z);
    // Weigh incline
    //Quaternionf pathQ (bestPath->orientation.w, bestPath->orientation.x,
    //                   bestPath->orientation.y, bestPath->orientation.z);
    return bestPath;
  }

  void VFHistogram::binarize(int range) {
    float val, tHighB, tLowB, tHigh, tLow, meanArea, ratio;
    meanArea = getMeanArea();
    tHighB = mean() + (range*std());
    tLowB = mean() - (range*std());

    std::cout << "Thresholds: "<<tHighB << "---------------" << tLowB << std::endl;

    for(int j=0; j<getHeight(); j++) {
      //    ratio = primary.getArea(j) / meanArea;
      ratio = 1;
      //std::cout << "Ratio: "<<ratio << "----------------" << std::endl;
      tLow = tLowB * ratio;
      tHigh = tHighB * ratio;
      for(int i = 0; i<getWidth(); i++) {
        val = getValue(i, j);
        if(val > tHigh)
          setValue(i, j, 1.0);
        else if(val < tLow)
          setValue(i, j, 0.0);
        else if(val == 0.0)
          setValue(i, j, 0.0);
        else if(val != val)
          setValue(i, j, 0.0);
        else
          setValue(i, j, (fabs(val-tLow) < fabs(val-tHigh)) ? 0 : 1);
      }
    }
  }

  bool VFHistogram::isIgnored(float x, float y, float z, float ws) {
    float dist = sqrt(pow((x-ox), 2) +
        pow((y-oy), 2) +
        pow((z-oz), 2));
    return dist > ws;
  }


///////////////////////////////////////////////////////////////////////////////
// kernel::test::scalar_nw.cpp                                               //
//                                                                           //
//  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)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/tools/precision.hpp>

#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/construct.hpp>

#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include <boost/function.hpp>

#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/container/map.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/map_fwd.hpp>

#include <boost/statistics/detail/fusion/functor/at_key.hpp>
#include <boost/statistics/detail/cross_validation/error/sqrt_mse.hpp>
#include <boost/statistics/detail/cross_validation/data/input_output_generator.hpp>

#include <boost/statistics/detail/kernel/kernels/scalar/gaussian.hpp>
#include <boost/statistics/detail/kernel/bandwidth_selection/normal_distribution.hpp>
#include <boost/statistics/detail/kernel/bandwidth_selection/meta_k_fold_nw.hpp>

#include <libs/statistics/detail/kernel/test/scalar_nw.h>

void test_scalar_nw(std::ostream& os){

    os << "-> test_scalar_nw : " << std::endl;
    os.flush();

    // Runs K-fold cross-validation for a sequence of datasets of
    // increasing size and reports the sqrt mse. Each data unit is
    // of the form (x,f(y)).

    using namespace boost;

    namespace kernel = boost::statistics::detail::kernel;
    namespace cv = boost::statistics::detail::cross_validation;

    // Types
    typedef double                                          val_;
    typedef std::vector<val_>                               vals_;
    typedef boost::function<val_(const val_&)>              x_to_y_;
    typedef cv::data::input_output_generator<
        val_,
        x_to_y_
    > x_to_u_;
    typedef x_to_u_::meta_input_output meta_io_;
    typedef meta_io_::type                                  u_; // data-unit
    typedef meta_io_::input                                 meta_x_;
    typedef meta_io_::output                                meta_y_;
    typedef meta_x_::key_type                               key_x_;
    typedef meta_y_::key_type                               key_y_;
    typedef meta_x_::data_type                              data_x_;
    typedef meta_y_::data_type                              data_y_;

    typedef statistics::detail::fusion::functor::at_key<key_x_> at_key_x_;
    typedef statistics::detail::fusion::functor::at_key<key_y_> at_key_y_;

    typedef mt19937                                         urng_;
    typedef normal_distribution<val_>                       norm_;
    typedef variate_generator<urng_&,norm_>                 gen_;
    typedef kernel::scalar::gaussian_kernel<val_>           gauss_k_;
    typedef kernel::bandwidth_selection::normal_distribution<1,val_> bw_sel_;
    typedef kernel::bandwidth_selection::meta_k_fold_nw<
        at_key_x_,
        at_key_y_
    > meta_k_fold_nw_;

    typedef meta_k_fold_nw_::apply<
        u_,
        gauss_k_
    >::type k_fold_nw_;

    typedef unsigned n_;

    // num_fold * n_per_fold units at each iteration
          n_ n_per_fold = 1;
    const n_ n_folds    = 10;
    const n_ n_loops    = 5;
    const n_ n_factor   = 10; // n_per_fold *= n_factor at each iteration

    // Initialization
    vals_ pred_nw_s;
    vals_ true_nw_s;

    urng_ urng;
    norm_ norm;
    gen_ gen(urng,norm);

    typedef function<u_(const n_&)> index_to_u_;

    x_to_y_ x_to_y =
        boost::lambda::_1 * boost::lambda::_1;

    index_to_u_ index_to_u = boost::lambda::bind(
        x_to_u_(x_to_y),
        boost::lambda::bind(
            boost::ref(gen)
        )
    );

    os  << '('  << k_fold_nw_::description_header
        << ','  << "bandwidth"
        << ','  << "sqrt_mse"
        << ')'  << std::endl;

    for(n_ i = 0; i<n_loops; i++){
        n_ n = n_per_fold * n_folds;
        pred_nw_s.resize( n );
        true_nw_s.resize( n );

        k_fold_nw_ k_fold_nw(
            n_folds,
            make_transform_iterator(
                make_counting_iterator<n_>(0),
                index_to_u
            ),
            make_transform_iterator(
                make_counting_iterator<n_>(n),
                index_to_u
            )
        );

        // This bandwidth is not optimal for rp, not nw, but it should not
        // matter too much
        val_ bandwidth = bw_sel_::rp_bandwidth(n);
        k_fold_nw.cross_validate(
            bandwidth,
            boost::begin(pred_nw_s),
            boost::begin(true_nw_s)
        );

        val_ smse = cv::error::sqrt_mse(
            boost::begin(pred_nw_s),
            boost::end(pred_nw_s),
            boost::begin(true_nw_s)
        );

        os  << '(' <<   k_fold_nw
            << ',' <<   bandwidth
            << ',' <<   smse
            << ')' <<   std::endl;

        n_per_fold *= n_factor;
    }

    os << "<-" << std::endl;
}


/* (c) 2020 - Sun Ruping
   ruping@umn.edu
   Allocate Passenger mutations to each cell after comet run
   g++ -o passenger passenger.cpp -lz -lboost_iostreams -std=c++11
   gcc/4.8.5 was tested.
*/

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <vector>
#include <string>
#include <cstring>
#include <sstream>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include "passenger.h"
#include <random>

using namespace std;
//template<typename T, typename... Args>
//std::unique_ptr<T> make_unique(Args&&... args) {
//    return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
//}

inline void splitstring(const string &str, vector<string> &elements, const string &delimiter);

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

  struct parameters *param = 0;
  param = interface(param, argc, argv);

  float urate = std::stof(param->urate);
  std::poisson_distribution<int> poisson_distribution (urate);
  std::uniform_int_distribution<int> uniform_distribution(1000001, 50000000); // define the range of mutational space, here a broad exome

  using urbg_t = std::mt19937_64;
  uint_fast32_t seed = std::random_device{}();
  //std::unique_ptr<urbg_t> engine_ = make_unique<urbg_t>(seed);
  std::unique_ptr<urbg_t> engine_(new urbg_t(seed));

  std::cout << "id\tcoor\n";    // print out header

  std::ifstream passenger_f(param->passenger_f, std::ios_base::in | std::ios_base::binary);
  boost::iostreams::filtering_streambuf<boost::iostreams::input> inbuf_p;
  inbuf_p.push(boost::iostreams::gzip_decompressor());
  inbuf_p.push(passenger_f);
  //Convert streambuf to istream
  std::istream instream_p(&inbuf_p);
  //Iterate lines
  std::string line;
  bool firstline = true;

  while(std::getline(instream_p, line)) {  // each line of $passenger file

    if (firstline) {
      firstline = false;
      continue;
    }

    vector <string> line_content;
    splitstring(line, line_content, "\t");
    vector <string>::iterator iter = line_content.begin();
    unsigned int i;

    unsigned int cellid = 0;
    string passengers;

    for(i = 1; iter != line_content.end(); iter++, i++) {
      switch (i) {
      case 1:
        cellid = atoi((*iter).c_str());
        continue;
      default:
        break;
      }
    }

    // number of passenger mutations accordin to urate
    unsigned int n_passengers = poisson_distribution(*engine_);

    for(int n=0; n < n_passengers; ++n)
      // sample a coordinate
      passengers = passengers + std::to_string(uniform_distribution(*engine_)) + ",";

    // print passenger mutations
    std::cout << cellid << "\t" << passengers << std::endl;
  }
  //Cleanup
  passenger_f.close();

  return 0;

} //main

inline void splitstring(const string &str, vector<string> &elements, const string &delimiter) {
  string::size_type lastPos = str.find_first_not_of(delimiter, 0);
  string::size_type pos     = str.find_first_of(delimiter, lastPos);

  while (string::npos != pos || string::npos != lastPos) {
    elements.push_back(str.substr(lastPos, pos - lastPos));
    lastPos = str.find_first_not_of(delimiter, pos);
    pos = str.find_first_of(delimiter, lastPos);
  }
}


#include "matvec_sparse.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SparseLU>
#include <general/class_tic_toc.h>
#define profile_matrix_product_sparse 1

template<typename T>
using SparseMatrixType = Eigen::SparseMatrix<T>;
template<typename T>
using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
template<typename T>
using VectorType = Eigen::Matrix<T, Eigen::Dynamic, 1, Eigen::ColMajor>;
template<typename T>
using VectorTypeT = Eigen::Matrix<T, 1, Eigen::Dynamic, Eigen::RowMajor>;

namespace sparse_lu {
    std::optional<Eigen::SparseMatrix<double>>                             A_real_sparse  = std::nullopt;
    std::optional<Eigen::SparseMatrix<std::complex<double>>>               A_cplx_sparse  = std::nullopt;
    std::optional<Eigen::SparseLU<SparseMatrixType<double>>>               lu_real_sparse = {};
    std::optional<Eigen::SparseLU<SparseMatrixType<std::complex<double>>>> lu_cplx_sparse = {};
    std::optional<Eigen::PartialPivLU<MatrixType<double>>>                 lu_real_dense  = std::nullopt;
    std::optional<Eigen::PartialPivLU<MatrixType<std::complex<double>>>>   lu_cplx_dense  = std::nullopt;

    void reset() {
        A_real_sparse.reset();
        A_cplx_sparse.reset();
        lu_real_sparse.reset();
        lu_cplx_sparse.reset();
        lu_real_dense.reset();
        lu_cplx_dense.reset();
    }
}

template<typename Scalar, bool sparseLU>
MatVecSparse<Scalar, sparseLU>::~MatVecSparse() {
    sparse_lu::reset();
}

template<typename Scalar, bool sparseLU>
MatVecSparse<Scalar, sparseLU>::MatVecSparse(const Scalar *A_, long L_, bool copy_data, eig::Form form_, eig::Side side_)
    : A_ptr(A_), L(L_), form(form_), side(side_) {
    if(copy_data) {
        A_stl.resize(static_cast<size_t>(L * L));
        std::copy(A_ptr, A_ptr + static_cast<size_t>(L * L), A_stl.begin());
        A_ptr = A_stl.data();
    }

    if constexpr(sparseLU) {
        Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
        if constexpr(std::is_same_v<Scalar, double>) {
            sparse_lu::A_real_sparse = A_matrix.sparseView();
            sparse_lu::A_real_sparse.value().makeCompressed();
        }
        if constexpr(std::is_same_v<Scalar, std::complex<double>>) {
            sparse_lu::A_cplx_sparse = A_matrix.sparseView();
            sparse_lu::A_cplx_sparse.value().makeCompressed();
        }
    }
    init_profiling();
}

// Function definitions

template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::FactorOP()

/*  Sparse decomposition
 *  Factors P(A-sigma*I) = LU
 */

{
    if(readyFactorOp) { return; }
    if(not readyShift) throw std::runtime_error("Cannot FactorOP: Shift value sigma has not been set.");
    t_factorOP->tic();
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);

    // Real
    if constexpr(std::is_same_v<Scalar, double> and not sparseLU) {
        sparse_lu::lu_real_dense = Eigen::PartialPivLU<MatrixType<Scalar>>();
        sparse_lu::lu_real_dense.value().compute(A_matrix);
    }
    if constexpr(std::is_same_v<Scalar, double> and sparseLU) { sparse_lu::lu_real_sparse.value().compute(sparse_lu::A_real_sparse.value()); }
    // Complex
    if constexpr(std::is_same_v<Scalar, std::complex<double>> and not sparseLU) {
        sparse_lu::lu_cplx_dense = Eigen::PartialPivLU<MatrixType<Scalar>>();
        sparse_lu::lu_cplx_dense.value().compute(A_matrix);
    }
    if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU) { sparse_lu::lu_cplx_sparse.value().compute(sparse_lu::A_cplx_sparse.value()); }

    t_factorOP->toc();
    readyFactorOp = true;
}

template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::MultOPv(Scalar *x_in_ptr, Scalar *x_out_ptr) {
    assert(readyFactorOp and "FactorOp() has not been run yet.");
    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    Eigen::Map<VectorType> x_in(x_in_ptr, L);
    Eigen::Map<VectorType> x_out(x_out_ptr, L);

    switch(side) {
        case eig::Side::R: {
            if constexpr(std::is_same_v<Scalar, double> and not sparseLU)
                x_out.noalias() = sparse_lu::lu_real_dense.value().solve(x_in);
            else if constexpr(std::is_same_v<Scalar, std::complex<double>> and not sparseLU)
                x_out.noalias() = sparse_lu::lu_cplx_dense.value().solve(x_in);
            else if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                x_out.noalias() = sparse_lu::lu_real_sparse.value().solve(x_in);
            else if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                x_out.noalias() = sparse_lu::lu_cplx_sparse.value().solve(x_in);
            break;
        }
        case eig::Side::L: {
            if constexpr(std::is_same_v<Scalar, double> and not sparseLU)
                x_out.noalias() = x_in * sparse_lu::lu_real_dense.value().inverse();
            else if constexpr(std::is_same_v<Scalar, std::complex<double>> and not sparseLU)
                x_out.noalias() = x_in * sparse_lu::lu_cplx_dense.value().inverse();
            else {
                throw std::runtime_error("Left sided sparse shift invert hasn't been implemented yet...");
            }
            break;
        }
        case eig::Side::LR: {
            throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
        }
    }
    counter++;
}

template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::MultAx(Scalar *x_in, Scalar *x_out) {
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    switch(form) {
        case eig::Form::NSYM:
            switch(side) {
                case eig::Side::R: {
                    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
                    Eigen::Map<VectorType> x_vec_in(x_in, L);
                    Eigen::Map<VectorType> x_vec_out(x_out, L);
                    if constexpr(not sparseLU)
                        x_vec_out.noalias() = A_matrix * x_vec_in;
                    else if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                        x_vec_out.noalias() = sparse_lu::A_real_sparse.value() * x_vec_in;
                    else if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                        x_vec_out.noalias() = sparse_lu::A_cplx_sparse.value() * x_vec_in;
                    break;
                }
                case eig::Side::L: {
                    using VectorTypeT = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;
                    Eigen::Map<VectorTypeT> x_vec_in(x_in, L);
                    Eigen::Map<VectorTypeT> x_vec_out(x_out, L);
                    if constexpr(not sparseLU)
                        x_vec_out.noalias() = x_vec_in * A_matrix;
                    else if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                        x_vec_out.noalias() = x_vec_in * sparse_lu::A_real_sparse.value();
                    else if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                        x_vec_out.noalias() = x_vec_in * sparse_lu::A_cplx_sparse.value();
                    break;
                }
                case eig::Side::LR: {
                    throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
                }
            }
            break;
        case eig::Form::SYMM: {
            using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
            Eigen::Map<VectorType> x_vec_in(x_in, L);
            Eigen::Map<VectorType> x_vec_out(x_out, L);
            if constexpr(not sparseLU) x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Upper>() * x_vec_in;
            if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                x_vec_out.noalias() = sparse_lu::A_real_sparse.value().template selfadjointView<Eigen::Upper>() * x_vec_in;
            if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                x_vec_out.noalias() = sparse_lu::A_cplx_sparse.value().template selfadjointView<Eigen::Upper>() * x_vec_in;
            break;
        }
    }
    counter++;
}

template<typename T, bool sparseLU>
void MatVecSparse<T, sparseLU>::MultAx(void *x, int *ldx, void *y, int *ldy, int *blockSize, [[maybe_unused]] primme_params *primme,
                                       [[maybe_unused]] int *err) {
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    switch(form) {
        case eig::Form::NSYM:
            switch(side) {
                case eig::Side::R: {
                    using VectorType = Eigen::Matrix<T, Eigen::Dynamic, 1>;
                    for(int i = 0; i < *blockSize; i++) {
                        T *                    x_in  = static_cast<T *>(x) + *ldx * i;
                        T *                    x_out = static_cast<T *>(y) + *ldy * i;
                        Eigen::Map<VectorType> x_vec_in(x_in, L);
                        Eigen::Map<VectorType> x_vec_out(x_out, L);
                        if constexpr(not sparseLU)
                            x_vec_out.noalias() = A_matrix * x_vec_in;
                        else if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                            x_vec_out.noalias() = sparse_lu::A_real_sparse.value() * x_vec_in;
                        else if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                            x_vec_out.noalias() = sparse_lu::A_cplx_sparse.value() * x_vec_in;
                        counter++;
                    }
                    break;
                }
                case eig::Side::L: {
                    using VectorTypeT = Eigen::Matrix<T, 1, Eigen::Dynamic>;
                    for(int i = 0; i < *blockSize; i++) {
                        T *                     x_in  = static_cast<T *>(x) + *ldx * i;
                        T *                     x_out = static_cast<T *>(y) + *ldy * i;
                        Eigen::Map<VectorTypeT> x_vec_in(x_in, L);
                        Eigen::Map<VectorTypeT> x_vec_out(x_out, L);
                        if constexpr(not sparseLU)
                            x_vec_out.noalias() = x_vec_in * A_matrix;
                        else if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                            x_vec_out.noalias() = x_vec_in * sparse_lu::A_real_sparse.value();
                        else if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                            x_vec_out.noalias() = x_vec_in * sparse_lu::A_cplx_sparse.value();
                        counter++;
                    }
                    break;
                }
                case eig::Side::LR: {
                    throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
                }
            }
            break;
        case eig::Form::SYMM: {
            using VectorType = Eigen::Matrix<T, Eigen::Dynamic, 1>;
            for(int i = 0; i < *blockSize; i++) {
                T *                    x_in  = static_cast<T *>(x) + *ldx * i;
                T *                    x_out = static_cast<T *>(y) + *ldy * i;
                Eigen::Map<VectorType> x_vec_in(x_in, L);
                Eigen::Map<VectorType> x_vec_out(x_out, L);
                if constexpr(not sparseLU) x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Upper>() * x_vec_in;
                if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                    x_vec_out.noalias() = sparse_lu::A_real_sparse.value().template selfadjointView<Eigen::Upper>() * x_vec_in;
                if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                    x_vec_out.noalias() = sparse_lu::A_cplx_sparse.value().template selfadjointView<Eigen::Upper>() * x_vec_in;
                counter++;
            }
            break;
        }
    }
    *err = 0;
}

template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::print() const {
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
}

template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::set_shift(std::complex<double> sigma_) {
    if(readyShift) { return; }
    sigma = sigma_;
    if(A_stl.empty()) {
        A_stl.resize(static_cast<size_t>(L * L));
        std::copy(A_ptr, A_ptr + static_cast<size_t>(L * L), A_stl.begin());
        A_ptr = A_stl.data();
    }
    Eigen::Map<MatrixType<Scalar>> A_matrix(A_stl.data(), L, L);
    if constexpr(std::is_same_v<Scalar, eig::real>) A_matrix -= Eigen::MatrixXd::Identity(L, L) * std::real(sigma);
    if constexpr(std::is_same_v<Scalar, eig::cplx>) A_matrix -= Eigen::MatrixXd::Identity(L, L) * sigma;

    if constexpr(sparseLU) {
        if constexpr(std::is_same_v<Scalar, double>) {
            sparse_lu::A_real_sparse = A_matrix.sparseView();
            sparse_lu::A_real_sparse.value().makeCompressed();
        }
        if constexpr(std::is_same_v<Scalar, std::complex<double>>) {
            sparse_lu::A_cplx_sparse = A_matrix.sparseView();
            sparse_lu::A_cplx_sparse.value().makeCompressed();
        }
    }

    readyShift = true;
}

template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::set_mode(const eig::Form form_) {
    form = form_;
}
template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::set_side(const eig::Side side_) {
    side = side_;
}
template<typename Scalar, bool sparseLU>
const eig::Form &MatVecSparse<Scalar, sparseLU>::get_form() const {
    return form;
}
template<typename Scalar, bool sparseLU>
const eig::Side &MatVecSparse<Scalar, sparseLU>::get_side() const {
    return side;
}

template<typename Scalar, bool sparseLU>
void MatVecSparse<Scalar, sparseLU>::init_profiling() {
    t_factorOP = std::make_unique<class_tic_toc>(profile_matrix_product_sparse, 5, "Time FactorOp");
    t_multOPv  = std::make_unique<class_tic_toc>(profile_matrix_product_sparse, 5, "Time MultOpv");
    t_multAx   = std::make_unique<class_tic_toc>(profile_matrix_product_sparse, 5, "Time MultAx");
}

// Explicit instantiations
template class MatVecSparse<double, true>;
template class MatVecSparse<double, false>;
template class MatVecSparse<std::complex<double>, true>;
template class MatVecSparse<std::complex<double>, false>;


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

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;

//
// Force trunctation to float precision of input values:
// we must ensure that the input values are exactly representable
// in whatever type we are testing, or the output values will all
// be thrown off:
//
float external_f;
float force_truncate(const float* f)
{
   external_f = *f;
   return external_f;
}

float truncate_to_float(mp_t r)
{
   float f = boost::math::tools::real_cast<float>(r);
   return force_truncate(&f);
}

//
// Our generator takes two arguments, but the second is interpreted
// as an instruction not a value, the second argument won't be placed
// in the output matrix by class test_data, so we add our algorithmically
// derived second argument to the output.
//
struct igamma_data_generator
{
   boost::math::tuple<mp_t, mp_t, mp_t, mp_t, mp_t> operator()(mp_t a, mp_t x)
   {
      // very naively calculate spots:
      mp_t z;
      switch((int)real_cast<float>(x))
      {
      case 1:
         z = truncate_to_float((std::min)(mp_t(1), a/100));
         break;
      case 2:
         z = truncate_to_float(a / 2);
         break;
      case 3:
         z = truncate_to_float((std::max)(0.9*a, a - 2));
         break;
      case 4:
         z = a;
         break;
      case 5:
         z = truncate_to_float((std::min)(1.1*a, a + 2));
         break;
      case 6:
         z = truncate_to_float(a * 2);
         break;
      case 7:
         z = truncate_to_float((std::max)(mp_t(100), a*100));
         break;
      default:
         BOOST_ASSERT(0 == "Can't get here!!");
      }

      //mp_t g = boost::math::tgamma(a);
      mp_t lg = boost::math::tgamma_lower(a, z);
      mp_t ug = boost::math::tgamma(a, z);
      mp_t rlg = boost::math::gamma_p(a, z);
      mp_t rug = boost::math::gamma_q(a, z);

      return boost::math::make_tuple(z, ug, rug, lg, rlg);
   }
};

struct gamma_inverse_generator
{
   boost::math::tuple<mp_t, mp_t> operator()(const mp_t a, const mp_t p)
   {
      mp_t x1 = boost::math::gamma_p_inv(a, p);
      mp_t x2 = boost::math::gamma_q_inv(a, p);
      std::cout << "Inverse for " << a << " " << p << std::endl;
      return boost::math::make_tuple(x1, x2);
   }
};

struct gamma_inverse_generator_a
{
   boost::math::tuple<mp_t, mp_t> operator()(const mp_t x, const mp_t p)
   {
      mp_t x1 = boost::math::gamma_p_inva(x, p);
      mp_t x2 = boost::math::gamma_q_inva(x, p);
      std::cout << "Inverse for " << x << " " << p << std::endl;
      return boost::math::make_tuple(x1, x2);
   }
};

int main(int argc, char*argv [])
{
   bool cont;
   std::string line;

   parameter_info<mp_t> arg1, arg2;
   test_data<mp_t> data;

   if((argc >= 2) && (std::strcmp(argv[1], "-inverse") == 0))
   {
      std::cout << "Welcome.\n"
         "This program will generate spot tests for the inverse incomplete gamma function:\n"
         "  gamma_p_inv(a, p) and gamma_q_inv(a, q)\n\n";
      do{
         get_user_parameter_info(arg1, "a");
         get_user_parameter_info(arg2, "p");
         data.insert(gamma_inverse_generator(), arg1, arg2);

         std::cout << "Any more data [y/n]?";
         std::getline(std::cin, line);
         boost::algorithm::trim(line);
         cont = (line == "y");
      }while(cont);
   }
   else if((argc >= 2) && (std::strcmp(argv[1], "-inverse_a") == 0))
   {
      std::cout << "Welcome.\n"
         "This program will generate spot tests for the inverse incomplete gamma function:\n"
         "  gamma_p_inva(a, p) and gamma_q_inva(a, q)\n\n";
      do{
         get_user_parameter_info(arg1, "x");
         get_user_parameter_info(arg2, "p");
         data.insert(gamma_inverse_generator_a(), arg1, arg2);

         std::cout << "Any more data [y/n]?";
         std::getline(std::cin, line);
         boost::algorithm::trim(line);
         cont = (line == "y");
      }while(cont);
   }
   else
   {
      arg2 = make_periodic_param(mp_t(1), mp_t(8), 7);
      arg2.type |= boost::math::tools::dummy_param;

      std::cout << "Welcome.\n"
         "This program will generate spot tests for the incomplete gamma function:\n"
         "  gamma(a, z)\n\n";

      do{
         get_user_parameter_info(arg1, "a");
         data.insert(igamma_data_generator(), arg1, arg2);

         std::cout << "Any more data [y/n]?";
         std::getline(std::cin, line);
         boost::algorithm::trim(line);
         cont = (line == "y");
      }while(cont);
   }

   std::cout << "Enter name of test data file [default=igamma_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "igamma_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, "igamma_data");

   return 0;
}


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

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

// 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

// 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_CARTESIAN_BUFFER_JOIN_ROUND_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_HPP

#include <algorithm>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/policies/compare.hpp>
#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
#include <boost/geometry/io/wkt/wkt.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Let the buffer create rounded corners
\ingroup strategies
\details This strategy can be used as JoinStrategy for the buffer algorithm.
    It creates a rounded corners around each convex vertex. It can be applied
    for (multi)linestrings and (multi)polygons.
    This strategy is only applicable for Cartesian coordinate systems.

\qbk{
[heading Example]
[buffer_join_round]
[heading Output]
[$img/strategies/buffer_join_round.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_join_miter join_miter]
}
 */
class join_round
{
public :

    //! \brief Constructs the strategy
    //! \param points_per_circle points which would be used for a full circle
    explicit inline join_round(std::size_t points_per_circle = 90)
        : m_points_per_circle(points_per_circle)
    {}

private :
    template
    <
        typename PromotedType,
        typename Point,
        typename DistanceType,
        typename RangeOut
    >
    inline void generate_points(Point const& vertex,
                Point const& perp1, Point const& perp2,
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        PromotedType const dx1 = get<0>(perp1) - get<0>(vertex);
        PromotedType const dy1 = get<1>(perp1) - get<1>(vertex);
        PromotedType const dx2 = get<0>(perp2) - get<0>(vertex);
        PromotedType const dy2 = get<1>(perp2) - get<1>(vertex);

        PromotedType const two_pi = geometry::math::two_pi<PromotedType>();

        PromotedType const angle1 = atan2(dy1, dx1);
        PromotedType angle2 = atan2(dy2, dx2);
        while (angle2 > angle1)
        {
            angle2 -= two_pi;
        }
        PromotedType const angle_diff = angle1 - angle2;

        // Divide the angle into an integer amount of steps to make it
        // visually correct also for a low number of points / circle

        // If a full circle is divided into 3 parts (e.g. angle is 125),
        // the one point in between must still be generated
        // The calculation below:
        // - generates 1 point  in between for an angle of 125 based on 3 points
        // - generates 0 points in between for an angle of 90  based on 4 points

        int const n = (std::max)(static_cast<int>(
            ceil(m_points_per_circle * angle_diff / two_pi)), 1);

        PromotedType const diff = angle_diff / static_cast<PromotedType>(n);
        PromotedType a = angle1 - diff;

        // Walk to n - 1 to avoid generating the last point
        for (int i = 0; i < n - 1; i++, a -= diff)
        {
            Point p;
            set<0>(p, get<0>(vertex) + buffer_distance * cos(a));
            set<1>(p, get<1>(vertex) + buffer_distance * sin(a));
            range_out.push_back(p);
        }
    }

public :

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Fills output_range with a rounded shape around a vertex
    template <typename Point, typename DistanceType, typename RangeOut>
    inline bool apply(Point const& ip, Point const& vertex,
                Point const& perp1, Point const& perp2,
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        typedef typename coordinate_type<Point>::type coordinate_type;
        typedef typename boost::range_value<RangeOut>::type output_point_type;

        typedef typename geometry::select_most_precise
            <
                typename geometry::select_most_precise
                    <
                        coordinate_type,
                        typename geometry::coordinate_type<output_point_type>::type
                    >::type,
                double
            >::type promoted_type;

        geometry::equal_to<Point> equals;
        if (equals(perp1, perp2))
        {
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
            std::cout << "Corner for equal points " << geometry::wkt(ip) << " " << geometry::wkt(perp1) << std::endl;
#endif
            return false;
        }

        // Generate 'vectors'
        coordinate_type vix = (get<0>(ip) - get<0>(vertex));
        coordinate_type viy = (get<1>(ip) - get<1>(vertex));

        promoted_type length_i = geometry::math::sqrt(vix * vix + viy * viy);
        DistanceType const bd = geometry::math::abs(buffer_distance);
        promoted_type prop = bd / length_i;

        Point bp;
        set<0>(bp, get<0>(vertex) + vix * prop);
        set<1>(bp, get<1>(vertex) + viy * prop);

        range_out.push_back(perp1);
        generate_points<promoted_type>(vertex, perp1, perp2, bd, range_out);
        range_out.push_back(perp2);
        return true;
    }

    template <typename NumericType>
    static inline NumericType max_distance(NumericType const& distance)
    {
        return distance;
    }

#endif // DOXYGEN_SHOULD_SKIP_THIS

private :
    std::size_t m_points_per_circle;
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_HPP