#ifndef SCENE_GENERATION_H
#define SCENE_GENERATION_H

#include <cmath>
#include <cstdlib>
#include <ctime>

#include <Eigen/Core>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <vector_types.h>

#include "ppf.h"
#include "vector_ops.h"

inline double randd(unsigned int seed=0){
    return double(rand()) / RAND_MAX;
}

inline float3 RandomTranslation(){
    float3 t;
    t.x = randd();
    t.y = randd();
    t.z = randd();
    return t;
}

// Uniform Random Rotation
// Ken Shoemake
// Graphics Gems III, pg 124-132
inline float4 RandomRotation(){
    double x0 = randd();
    double x1 = randd();
    double x2 = randd();
    double th1 = 2*M_PI*x1;
    double th2 = 2*M_PI*x2;
    double s1 = sin(th1);
    double c1 = cos(th1);
    double s2 = sin(th2);
    double c2 = cos(th2);
    double r1 = sqrt(1-x0);
    double r2 = sqrt(x0);
    float4 q;
    q.x = s1*r1;
    q.y = c1*r1;
    q.z = s2*r2;
    q.w = c2*r2;
    return q;
}

// Apply rotation and translation (in that order) to model and append model to scene.
template <typename Point>
Eigen::Matrix4f GenerateSceneWithModel(typename pcl::PointCloud<Point>& model,
                                       typename pcl::PointCloud<Point>& scene,
                                       float3& translation, float4& rotation,
                                       typename pcl::PointCloud<Point>& new_scene){
    if(!to_bool(translation)){
        translation = RandomTranslation();
    }

    if(!to_bool(rotation)){
        rotation = RandomRotation();
    }

    Point model_centroid, scene_centroid;
    pcl::computeCentroid(model, model_centroid);
    pcl::computeCentroid(scene, scene_centroid);

    Eigen::Affine3f t =
        Eigen::Translation3f(scene_centroid.x, scene_centroid.y, scene_centroid.z) *
        Eigen::Translation3f(translation.x, translation.y, translation.z) *
        Eigen::Quaternionf(rotation.x, rotation.y, rotation.z, rotation.w) *
        Eigen::Translation3f(-1*model_centroid.x, -1*model_centroid.y, -1*model_centroid.z);

    Eigen::Matrix4f T = t.matrix();
    // cout << "generated T:" << T << endl;

    pcl::PointCloud<Point> transformed_model = pcl::PointCloud<Point>();
    pcl::transformPointCloudWithNormals(model, transformed_model, T);

    new_scene += scene;
    new_scene += transformed_model;
    return T;
}

template <typename Point>
void CenterScene(typename pcl::PointCloud<Point>& scene){
    // TODO: check return value
    Point centroid;
    pcl::computeCentroid(scene, centroid);

    Eigen::Affine3f t;
    // Having the scene be in a different octant breaks things.
    t = Eigen::Translation3f(-1*centroid.x+1, -1*centroid.y+1, -1*centroid.z+1);
    Eigen::Matrix4f T = t.matrix();

    pcl::PointCloud<Point> transformed_scene = pcl::PointCloud<Point>();
    pcl::transformPointCloudWithNormals(scene, transformed_scene, T);
    // is this a memory leak?
    scene = transformed_scene;
}

#endif /* SCENE_GENERATION_H */


#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>
#include "../../rmse.hpp"

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm1 (
           make_rowmajor_matrix_local_load<float>("./sample_4x4"));
    auto cm2 = cm1;

    // slicing rows and cols
    auto row1 = make_row_vector<float> (cm1,0);
    auto row2 = make_row_vector<float> (cm1,1);
    auto col1 = make_col_vector<float> (cm1,0);
    auto col2 = make_col_vector<float> (cm1,1);

    // updating row2 and col2 of cm1, using gemv (matrix-vector multiplication)
    gemv<float>(cm2,row1,row2); // row2-of-cm1 = cm2 * row1-of-cm1
    gemv<float>(cm2,col1,col2); // col2-of-cm1 = cm2 * col1-of-cm1

    double tol = 0.01;
    colmajor_matrix_local<float> ref (
           make_rowmajor_matrix_local_load<float> ("./ref_4x4"));
    BOOST_CHECK (calc_rms_err<float> (cm1.val, ref.val) < tol);
}


#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Geometry>
#include <iostream>
#include <unordered_map>

#include "LineGroup.h"
#include "DBSCAN.h"

#include "ImageProcessor.h"
namespace localization {

using Vector = Eigen::Vector2f;

ImageProcessor::ImageProcessor(const CameraInfo& cameraInfo, const QuadState& state)
    : cameraInfo_(cameraInfo), state_(state), preProcessing_(cameraInfo, state), intersectionTransform_(cameraInfo, state)
{
    srand(time(NULL));
}

ImageProcessor::~ImageProcessor()
{
}

void ImageProcessor::processImage(cv::Mat& input, cv::Mat& result)
{
    cv::Mat preProcessed;
    cv::Mat perspectiveTransform;
    preProcessing_.preProcessImage(input, preProcessed, perspectiveTransform);

    cv::Mat edges;
    findEdges(preProcessed, edges);

    findLines(edges);

    result = cv::Mat(edges.size(), CV_8UC3, cv::Scalar(0, 0, 0));
    analyzeLineCluster(result, edges.size(), perspectiveTransform);

    /*

    cv::Mat debug;
    cv::warpPerspective(input, debug, perspectiveTransform, debug.size());

    std::vector<cv::Point2f> dst;
    std::vector<cv::Point2f> src;
    for (int i = 0; i < intersections_.size(); ++i)
    {
        src.push_back(cv::Point2f(intersections_[i].x(), intersections_[i].y()));
    }
    if (!dst.empty())
    {
        cv::perspectiveTransform(src, dst, perspectiveTransform);

        std::vector<Eigen::Vector2f> tmp;
        for (int i = 0; i < dst.size(); ++i)
        {
            tmp.push_back(Eigen::Vector2f(dst[i].x, dst[i].y));
        }

        drawIntersection(tmp, cv::Scalar(150, 150, 0), input);

        cv::imshow("debug", input);
        cv::waitKey(30);
    }
    */
}
void ImageProcessor::findEdges(const cv::Mat& src, cv::Mat& edges)
{
    cv::Canny(src, edges, 1, 30, 3);
}

void ImageProcessor::drawLine(cv::Mat& dst, const Line& line, const cv::Scalar& color) const
{
    Vector centroid = line.getCentroid();
    Vector orientation = line.getOrientation();

    cv::Point2f pt1, pt2;
    pt1.x = cvRound(centroid.x() + 1000 * (orientation.x()));
    pt1.y = cvRound(centroid.y() + 1000 * (orientation.y()));
    pt2.x = cvRound(centroid.x() - 1000 * (orientation.x()));
    pt2.y = cvRound(centroid.y() - 1000 * (orientation.y()));
    cv::line(dst, pt1, pt2, color, 1, CV_AA);
    cv::circle(dst, cv::Point2f(centroid.x(), centroid.y()), 5, cv::Scalar(150, 0, 150), -1);
}

void ImageProcessor::findLines(const cv::Mat& edges)
{
    std::vector<cv::Vec2f> rawLines;
    cv::HoughLines(edges, rawLines, 1, CV_PI / 180, 100, 0, 0 );

    buildLineArray(rawLines);
}

void ImageProcessor::analyzeLineCluster(cv::Mat& debug, const cv::Size& size, const cv::Mat& perspectiveTransform)
{
    /*
    if (lineCluster_.size() == 0)
    {
        return;
    }
    */

    for (int i = 0; i < lineCluster_.size(); ++i)
    {
        drawLine(debug, lineCluster_[i], cv::Scalar(100, 100, 100));
    }

    lineDetection_.filterLineCluster(lineCluster_);
    const std::vector<Line>& filteredLines = lineDetection_.getFilteredLines();
    for (int i = 0; i < filteredLines.size(); ++i)
    {
        drawLine(debug, filteredLines[i], cv::Scalar(150, 0, 150));
    }

    std::vector<LineGroup> orientationGroup;
    groupByOrientation(orientationGroup, lineCluster_);

    findLineIntersections(orientationGroup, size);
    std::vector<int> clusterMemberships;
    tf::Transform origin2camera = state_.getOrigin2Fcu() * state_.getFcu2Camera();
    tf::Vector3 camDirection = tf::quatRotate(origin2camera.getRotation(), tf::Vector3(0.0, 0.0, 1.0));

    camDirection.normalize();
    float S = std::cos(std::atan(std::sqrt(std::pow(camDirection.x(), 2) + std::pow(camDirection.y(), 2)) / camDirection.z()));
    int radius = 0.5 * S * cameraInfo_.focalLength / state_.getOrigin2Fcu().getOrigin().z();
    DBSCAN::DBSCAN(intersections_, radius, 2, clusterMemberships);

    std::vector<Vector> intersections;
    parseClusterMemberships(clusterMemberships, intersections);

    //drawIntersection(intersections_, cv::Scalar(150, 150, 0), debug);
    drawIntersection(intersections, cv::Scalar(0, 0, 150), debug);

    intersectionTransform_.transformIntersections(intersections, perspectiveTransform, size);
}

void ImageProcessor::parseClusterMemberships(const std::vector<int>& clusterMemberships, std::vector<Vector>& intersections)
{
    if (clusterMemberships.size() != intersections_.size()) return;

    std::unordered_map<int, std::pair<Vector, int>> groups;

    for (int i = 0; i < clusterMemberships.size(); ++i) {
        int groupId = clusterMemberships[i];
        std::unordered_map<int, std::pair<Vector, int>>::iterator it = groups.find(groupId);
        if (it != groups.end()) {
            it->second.first *= it->second.second;
            it->second.first += intersections_[i];
            it->second.second++;
            it->second.first.x() /= it->second.second;
            it->second.first.y() /= it->second.second;
        } else {
            groups.insert({groupId, {intersections_[i], 1}});
        }
    }

    std::unordered_map<int, std::pair<Vector, int>>::iterator it;
    for(it = groups.begin(); it != groups.end(); it++) {
        intersections.push_back(it->second.first);
    }
}

void ImageProcessor::findLineIntersections(const std::vector<LineGroup>& orientationGroups, const cv::Size& size)
{
    intersections_.clear();
    for (size_t i = 0; i < orientationGroups.size(); ++i) {
        const LineGroup& firstGroup = orientationGroups[i];
        for (size_t j = (i + 1) % orientationGroups.size(); j < orientationGroups.size() - 1; j++) {
            const LineGroup& otherGroup = orientationGroups[j];
            findLineIntersections(firstGroup, otherGroup, size);
        }
    }
}

void ImageProcessor::findLineIntersections(const LineGroup& firstGroup, const LineGroup otherGroup, const cv::Size& size)
{
    const std::vector<const Line*> firstLines = firstGroup.getLines();
    const std::vector<const Line*> otherLines = otherGroup.getLines();

    for (int i = 0; i < firstLines.size(); ++i)
    {
        const Line* firstLine = firstLines[i];
        for (int j = 0; j < otherLines.size(); j++)
        {
            const Line* otherLine = otherLines[j];
            if (std::abs(otherLine->getOrientation().dot(firstLine->getOrientation())) > 0.8)
            {
                continue;
            }
            Vector intersection;
            if (firstLines[i]->findIntersection(*otherLines[j], intersection)) {
                Eigen::AlignedBox<float, 2> box(Vector(0.0, 0.0), Vector(size.width, size.height));
                if (box.contains(intersection)) {
                    intersections_.push_back(intersection);
                }
            }
        }
    }
}

void ImageProcessor::drawIntersection(const std::vector<Vector>& intersections, const cv::Scalar& color, cv::Mat& dst)
{
    for (int i = 0; i < intersections.size(); ++i) {
        cv::circle(dst,  cv::Point2f(intersections[i].x(), intersections[i].y()), 5, color, -1);
    }
}

void ImageProcessor::groupByOrientation(std::vector<LineGroup>& orientationGroup, const std::vector<Line>& lines)
{
    for (size_t i = 0; i < lineCluster_.size(); ++i) {
        groupByOrientation(orientationGroup, lineCluster_[i]);
    }
}

void ImageProcessor::groupByOrientation(std::vector<LineGroup>& group, Line& line)
{
    const double PRECISION_TRESHOLD = 0.9;
    double bestPrecision = 0.0;
    LineGroup*  bestGroup = nullptr;

    bool groupFound = false;
    for (int i = 0; i < group.size(); i++) {
        double precision = std::abs(group[i].getAvgOrientation().dot(line.getOrientation()));
        if (precision > PRECISION_TRESHOLD && precision > bestPrecision)
        {
            bestGroup = &group[i];
            bestPrecision = precision;
            groupFound = true;
        }
    }

    if(groupFound) {
        bestGroup->add(line);
    } else {
        group.push_back(LineGroup(line));
    }
}

void ImageProcessor::buildLineArray(const std::vector<cv::Vec2f>& lineCluster)
{
    Eigen::AlignedBox<float, 2> frame(Vector(0.0, 0.0), Vector(640.0, 480.0));
    lineCluster_.clear();
    for (int i = 0; i < lineCluster.size(); ++i)
    {
        lineCluster_.push_back(Line(lineCluster[i][0], lineCluster[i][1], frame));
    }
}

}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Easy calculation. Gravitational Acceleration Tutorial,
 *          http://easycalculation.com/physics/classical-physics
 *          /learn-gravitational-acceleration.php, last accessed: 26th February, 2012.
 *      MathWorks. gravityzonal, MATLAB 2012b, 2012.
 *      Melman, J. Propagate software, J.C.P.Melman@tudelft.nl, 2012.
 *      Ronse, A. A parametric study of space debris impact footprints, MSc thesis, Delft
 *          University of Technlogy, Delft, The Netherlands, in preparation.
 *
 */

#define BOOST_TEST_MAIN

#include <algorithm>
#include <cmath>
#include <limits>
#include <map>
#include <vector>

#include <boost/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/UnitTests/planetTestData.h"

namespace tudat
{
namespace unit_tests
{

using namespace gravitation;

typedef std::map< int, double > KeyIntValueDoubleMap;

BOOST_AUTO_TEST_SUITE( test_gravitational_acceleration )

//! Test if gravitational acceleration is computed correctly.
BOOST_AUTO_TEST_CASE( testGravitationalAcceleration )
{
    // Test 1: compute gravitational acceleration exerted on surface of Earth
    //         (Easy calculation, 2012).
    {
        // Set gravitational parameter of Earth [m^3 s^-2].
        const double gravitationalParameterOfEarth = 6.6726e-11 * 5.9742e24;

        // Set position vector of Earth [m].
        const Eigen::Vector3d positionOfEarth = Eigen::Vector3d::Zero( );

        // Set position vector on Earth surface [m].
        const Eigen::Vector3d positionOnEarthSurface( 6.3781e6, 0.0, 0.0 );

        // Compute gravitational accelerating acting on Earth's surface [N].
        const Eigen::Vector3d gravitationalAccelerationExertedAtEarthSurface
                = computeGravitationalAcceleration(
                    positionOnEarthSurface, gravitationalParameterOfEarth, positionOfEarth );

        // Check if computed gravitational force matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( 9.8, gravitationalAccelerationExertedAtEarthSurface.norm( ),
                                    1.0e-4 );
    }

    // Test 2: compute gravitational acceleration exerted on Lunar surface
    //         (Easy calculation, 2012).
    {
        // Set universal gravitational constant [m^3 kg^-1 s^-2].
        const double universalGravitationalConstant = 6.6726e-11;

        // Set mass of Moon [kg].
        const double massOfMoon = 7.36e22;

        // Set position vector of Moon [m].
        const Eigen::Vector3d positionOfMoon( 12.65, 0.23, -45.78 );

        // Set position vector on surface of Moon [m].
        const Eigen::Vector3d positionOfLunarSurface( 0.0, 1735771.89, 0.0 );

        // Compute gravitational accelerating acting on Lunar surface [N].
        const Eigen::Vector3d gravitationalAccelerationExertedAtLunarSurface
                = computeGravitationalAcceleration(
                    universalGravitationalConstant, positionOfLunarSurface,
                    massOfMoon, positionOfMoon );

        // Check if computed gravitational force matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( 1.63, gravitationalAccelerationExertedAtLunarSurface.norm( ),
                                    1.0e-6 );
    }
}

//! Test if gravitational acceleration sum due to zonal terms is computed correctly using MATLAB.
BOOST_AUTO_TEST_CASE( testGravitationalAccelarationSumZonalMatlab )
{
    // These tests check if total acceleration due to zonal terms is computed correctly by
    // comparing to output generated using gravityzonal() function in MATLAB (Mathworks, 2012).
    // The planet data used is obtained from the documentation for the gravityzonal() function.

    // Get planet test data.
    std::vector< PlanetTestData > planetData = getPlanetMatlabTestData( );

    // Loop over all planet test data and recompute the results using Tudat code. Check that the
    // values computed match MATLAB's output (Mathworks, 2012).
    for ( unsigned int planet = 0; planet < planetData.size( ); planet++ )
    {
        for ( unsigned int body1 = 0; body1 < planetData.at( planet ).body1Positions.size( );
              body1++ )
        {
            for ( unsigned int body2 = 0; body2 < planetData.at( planet ).body2Positions.size( );
                  body2++ )
            {
                // Compute central gravitational acceleration term [m s^-2].
                const Eigen::Vector3d computedCentralAcceleration
                        = computeGravitationalAcceleration(
                            planetData.at( planet ).body2Positions.at( body2 ),
                            planetData.at( planet ).gravitationalParameter,
                            planetData.at( planet ).body1Positions.at( body1 ) );

                // Check that the computed central gravitational acceleration matches the expected
                // values.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            planetData.at( planet ).expectedAcceleration[ body1 ][ body2 ][
                            central ],
                            computedCentralAcceleration,
                            1.0e-15 );

                // Declare zonal coefficients used.
                KeyIntValueDoubleMap zonalCoefficients;

                // Loop over all available zonal gravity field coefficients.
                for ( KeyIntValueDoubleMap::iterator zonalCoefficientIterator
                      = planetData.at( planet ).zonalCoefficients.begin( );
                      zonalCoefficientIterator
                      != planetData.at( planet ).zonalCoefficients.end( );
                      zonalCoefficientIterator++ )
                {
                    // Add current zonal coefficient to local list.
                    zonalCoefficients[ zonalCoefficientIterator->first ]
                            = zonalCoefficientIterator->second;

                    // Compute gravitational acceleration sum [m s^-2].
                    const Eigen::Vector3d computedAccelerationSum
                            = computeGravitationalAccelerationZonalSum(
                                planetData.at( planet ).body2Positions.at( body2 ),
                                planetData.at( planet ).gravitationalParameter,
                                planetData.at( planet ).equatorialRadius,
                                zonalCoefficients,
                                planetData.at( planet ).body1Positions.at( body1 ) );

                    // Check that computed gravitational acceleration sum matches expected values.
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                                planetData.at( planet ).expectedAcceleration[ body1 ][ body2 ][
                                zonalCoefficientIterator->first ],
                            computedAccelerationSum,
                            1.0e-15 );
                }
            }
        }
    }
}

//! Test if wrapper classes compute gravitational acceleration due to zonal terms correctly using
//! MATLAB.
BOOST_AUTO_TEST_CASE( testGravitationalAccelerationZonalSumWrapperClassesMatlab )
{
    // These tests check if total acceleration due to zonal terms is computed correctly by wrapper
    // class comparing to output generated using gravityzonal() function in MATLAB
    // (Mathworks, 2012). The planet data used is obtained from the documentation for the
    // gravityzonal() function. This check is for consistency purposes, since the wrapper class
    // wraps the free functions that are all tested too.

    // Short-cuts.
    using namespace gravitation;

    // Get planet test data.
    std::vector< PlanetTestData > planetData = getPlanetMatlabTestData( );

    // Loop over all planet test data and recompute the results using Tudat code. Check that the
    // values computed match MATLAB's output (Mathworks, 2012).
    for ( unsigned int planet = 0; planet < planetData.size( ); planet++ )
    {
        for ( unsigned int body1 = 0; body1 < planetData.at( planet ).body1Positions.size( );
              body1++ )
        {
            for ( unsigned int body2 = 0; body2 < planetData.at( planet ).body2Positions.size( );
                  body2++ )
            {
                // Test central gravitational acceleration wrapper class.
                {
                    // Declare central acceleration wrapper class object.
                    CentralGravitationalAccelerationModel3dPointer centralGravity
                            = std::make_shared< CentralGravitationalAccelerationModel3d >(
                                [ & ]( ){ return
                                    planetData.at( planet ).body2Positions.at( body2 ); },
                                planetData.at( planet ).gravitationalParameter,
                                [ & ]( ){ return
                                    planetData.at( planet ).body1Positions.at( body1 ); } );

                    // Compute central gravitational acceleration term [m s^-2].
                    const Eigen::Vector3d computedCentralAcceleration
                            = centralGravity->getAcceleration( );

                    // Check that the computed central gravitational acceleration matches the
                    // expected values.
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                                planetData.at( planet ).expectedAcceleration[ body1 ][ body2 ][
                                central ],
                                computedCentralAcceleration,
                                1.0e-15 );
                }

                // Test central + J2 gravitational acceleration wrapper class.
                if ( planetData.at( planet ).zonalCoefficients.rbegin( )->first == 2 )
                {
                    // Declare central + J2 acceleration wrapper class object.
                    CentralJ2GravitationalAccelerationModelPointer centralJ2Gravity
                            = std::make_shared< CentralJ2GravitationalAccelerationModel >(
                                [ & ]( ){ return
                                    planetData.at( planet ).body2Positions.at( body2 ); },
                                planetData.at( planet ).gravitationalParameter,
                                planetData.at( planet ).equatorialRadius,
                                planetData.at( planet ).zonalCoefficients[ 2 ],
                            [ & ]( ){ return
                                planetData.at( planet ).body1Positions.at( body1 ); } );

                    // Compute gravitational acceleration sum [m s^-2].
                    const Eigen::Vector3d computedCentralJ2AccelerationSum
                            = centralJ2Gravity->getAcceleration( );

                    // Check that computed central + J2 gravitational acceleration matches
                    // expected values.
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                                planetData.at( planet ).expectedAcceleration[
                                body1 ][ body2 ][ 2 ],
                            computedCentralJ2AccelerationSum,
                            1.0e-15 );
                }

                // Test central + J2 + J3 gravitational acceleration wrapper class.
                if ( planetData.at( planet ).zonalCoefficients.rbegin( )->first == 3 )
                {
                    // Declare central + J2 + J3 acceleration wrapper class object.
                    CentralJ2J3GravitationalAccelerationModelPointer centralJ2J3Gravity
                            = std::make_shared< CentralJ2J3GravitationalAccelerationModel >(
                                [ & ]( ){ return
                                    planetData.at( planet ).body2Positions.at( body2 ); },
                                planetData.at( planet ).gravitationalParameter,
                                planetData.at( planet ).equatorialRadius,
                                planetData.at( planet ).zonalCoefficients[ 2 ],
                            planetData.at( planet ).zonalCoefficients[ 3 ],
                            [ & ]( ){ return
                                planetData.at( planet ).body1Positions.at( body1 ); } );

                    // Compute gravitational acceleration sum [m s^-2].
                    const Eigen::Vector3d computedCentralJ2J3AccelerationSum
                            = centralJ2J3Gravity->getAcceleration( );

                    // Check that computed central + J2 + J3 gravitational acceleration matches
                    // expected values.
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                                planetData.at( planet ).expectedAcceleration[
                                body1 ][ body2 ][ 3 ],
                            computedCentralJ2J3AccelerationSum,
                            1.0e-15 );
                }

                // Test central + J2 + J3 + J4 gravitational acceleration wrapper class.
                if ( planetData.at( planet ).zonalCoefficients.rbegin( )->first == 4 )
                {
                    // Declare pointer to wrapper class object.
                    CentralJ2J3J4GravitationalAccelerationModelPointer centralJ2J3J4Gravity;

                    // Check if only J2 and J4 are given; if so set J3 to 0.0.
                    if ( planetData.at( planet ).zonalCoefficients.size( ) == 2 )
                    {
                        // Declare central + J2 + J3 + J4 acceleration wrapper class object.
                        centralJ2J3J4Gravity = std::make_shared<
                                CentralJ2J3J4GravitationalAccelerationModel >(
                                    [ & ]( ){ return
                                        planetData.at( planet ).body2Positions.at( body2 ); },
                                    planetData.at( planet ).gravitationalParameter,
                                    planetData.at( planet ).equatorialRadius,
                                    planetData.at( planet ).zonalCoefficients[ 2 ],
                                0.0,
                                planetData.at( planet ).zonalCoefficients[ 4 ],
                                [ & ]( ){ return
                                    planetData.at( planet ).body1Positions.at( body1 ); } );
                    }

                    // Else, include given J3.
                    else
                    {
                        // Declare central + J2 + J3 + J4 acceleration wrapper class object.
                        centralJ2J3J4Gravity = std::make_shared<
                                CentralJ2J3J4GravitationalAccelerationModel >(
                                    [ & ]( ){ return
                                        planetData.at( planet ).body2Positions.at( body2 ); },
                                    planetData.at( planet ).gravitationalParameter,
                                    planetData.at( planet ).equatorialRadius,
                                    planetData.at( planet ).zonalCoefficients[ 2 ],
                                planetData.at( planet ).zonalCoefficients[ 3 ],
                                planetData.at( planet ).zonalCoefficients[ 4 ],
                                [ & ]( ){ return
                                    planetData.at( planet ).body1Positions.at( body1 ); } );
                    }

                    // Compute gravitational acceleration sum [m s^-2].
                    const Eigen::Vector3d computedCentralJ2J3J4AccelerationSum
                            = centralJ2J3J4Gravity->getAcceleration( );

                    // Check that computed central + J2 gravitational acceleration matches
                    // expected values.
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                                planetData.at( planet ).expectedAcceleration[
                                body1 ][ body2 ][ 4 ],
                            computedCentralJ2J3J4AccelerationSum,
                            1.0e-15 );
                }
            }
        }
    }
}

//! Test if gravitational acceleration due to zonal terms is computed correctly (Melman, 2012).
BOOST_AUTO_TEST_CASE( testGravitationalAccelarationZonalMelman )
{
    typedef Eigen::Vector3d ( *GravitationalAccelerationPointer )(
                const Eigen::Vector3d&, const double, const double, const double,
                const Eigen::Vector3d& );

    // These tests check if acceleration due to zonal terms is computed correctly by comparing to
    // output generated by (Melman, 2012).

    // Get planet test data.
    PlanetTestData earthData = getEarthMelmanTestData( );

    // Set map of function pointers for zonal coefficients.
    std::map< int, GravitationalAccelerationPointer > zonalGravitationalAccelerationPointers
            = { { 2, &computeGravitationalAccelerationDueToJ2 }, { 3, &computeGravitationalAccelerationDueToJ3 },
                { 4, &computeGravitationalAccelerationDueToJ4 } };

    // Loop over all planet test data and recompute the results using Tudat code. Check that the
    // values computed match results obtained by (Melman, 2012).
    for ( unsigned int body2 = 0; body2 < earthData.body2Positions.size( ); body2++ )
    {
        // Loop over all available zonal gravity field coefficients.
        for ( KeyIntValueDoubleMap::iterator zonalCoefficientIterator
              = earthData.zonalCoefficients.begin( );
              zonalCoefficientIterator != earthData.zonalCoefficients.end( );
              zonalCoefficientIterator++ )
        {
            // Compute gravitational acceleration due to given zonal term [m s^-2].
            const Eigen::Vector3d computedZonalGravitationalAcceleration
                    = zonalGravitationalAccelerationPointers[ zonalCoefficientIterator->first ](
                        earthData.body2Positions.at( body2 ),
                        earthData.gravitationalParameter,
                        earthData.equatorialRadius,
                        zonalCoefficientIterator->second,
                        earthData.body1Positions.at( 0 ) );

            // Check that computed gravitational acceleration sum matches expected values.
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        earthData.expectedAcceleration[ 0 ][ body2 ][
                    zonalCoefficientIterator->first ],
                    computedZonalGravitationalAcceleration,
                    1.0e-14 );
        }
    }
}

//! Test if gravitational acceleration due to zonal terms is computed correctly (Ronse, 2012).
BOOST_AUTO_TEST_CASE( testGravitationalAccelarationZonalRonse )
{
    typedef Eigen::Vector3d ( *GravitationalAccelerationPointer )(
                const Eigen::Vector3d&, const double, const double, const double,
                const Eigen::Vector3d& );

    // These tests check if acceleration due to zonal terms is computed correctly by comparing to
    // output generated by (Ronse, 2012).

    // Get planet test data.
    PlanetTestData earthData = getEarthRonseTestData( );

    // Set map of function pointers for zonal coefficients.
    std::map< int, GravitationalAccelerationPointer > zonalGravitationalAccelerationPointers
            = { { 2, &computeGravitationalAccelerationDueToJ2 }, { 3, &computeGravitationalAccelerationDueToJ3 },
                { 4, &computeGravitationalAccelerationDueToJ4 } };

    // Loop over all planet test data and recompute the results using Tudat code. Check that the
    // values computed match results obtained by (Ronse, 2012).
    for ( unsigned int body2 = 0; body2 < earthData.body2Positions.size( ); body2++ )
    {
        // Compute central gravitational acceleration term [m s^-2].
        const Eigen::Vector3d computedCentralAcceleration
                = computeGravitationalAcceleration(
                    earthData.body2Positions.at( body2 ),
                    earthData.gravitationalParameter,
                    earthData.body1Positions.at( 0 ) );

        // Check that the computed central gravitational acceleration matches the expected
        // values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    earthData.expectedAcceleration[ 0 ][ body2 ][ central ],
                computedCentralAcceleration,
                1.0e-15 );

        // Loop over all available zonal gravity field coefficients.
        for ( KeyIntValueDoubleMap::iterator zonalCoefficientIterator
              = earthData.zonalCoefficients.begin( );
              zonalCoefficientIterator != earthData.zonalCoefficients.end( );
              zonalCoefficientIterator++ )
        {
            // Compute gravitational acceleration due to given zonal term [m s^-2].
            const Eigen::Vector3d computedZonalGravitationalAcceleration
                    = zonalGravitationalAccelerationPointers[ zonalCoefficientIterator->first ](
                        earthData.body2Positions.at( body2 ),
                        earthData.gravitationalParameter,
                        earthData.equatorialRadius,
                        zonalCoefficientIterator->second,
                        earthData.body1Positions.at( 0 ) );

            // Check that computed gravitational acceleration sum matches expected values.
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( earthData.expectedAcceleration[ 0 ][ body2 ][
                    zonalCoefficientIterator->first ],
                    computedZonalGravitationalAcceleration,
                    1.0e-13 );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <iostream>
#include <fstream>
#include <string.h>
#include <math.h>
#include <Eigen/Dense>
#include <algorithm>

#include "matrix.h"
#include "kdtree_nano.h"

using namespace std;

/**
 * @brief SparsifyData Sparsify a point cloud
 * @param M Input Point cloud
 * @param M_num Number of points in M
 * @param dim Dimemsion of points
 * @param idx_sparse Output binary masks indicating which point should be preserved
 * @param idx_size Output number of points to be preserved
 * @param min_dist Minimum distance for sparsifying
 * @param out_dist Distance for removing outliers
 * @param idx_start Start sparsifying from the specific point
 */
void SparsifyData (double *M,int32_t M_num,int32_t dim,int32_t* idx_sparse,int32_t &idx_size,double min_dist,double out_dist,int32_t idx_start) {

  // copy model data to kdtree
  PointCloud<double> cloud;
  for (int32_t m=0; m<M_num; m++) {
      cloud.pts.push_back(Point<double>((double)M[m*dim], (double)M[m*dim+1], (double)M[m*dim+2]));  }

  // build a kd tree from the model point cloud
  KDtree<double>* tree = new KDtree<double>(cloud);

  // for all data points do
  for (int32_t i=0; i<M_num; i++) {

    if (i>=idx_start) {

      vector<size_t> result;
      vector<double> dist;
      tree->radiusSearch(cloud.pts[i], min_dist, result, dist);

      bool neighbor_exists = false;
      for (int32_t j=0; j<result.size(); j++)
        neighbor_exists |= (bool)idx_sparse[result[j]];

      if (!neighbor_exists) {
        idx_sparse[i] = 1;
        idx_size++;
      }

    // simply add
    } else {
      idx_sparse[i] = 1;
      idx_size++;
    }
  }

  // remove outliers
  if (out_dist>0) {
    for (int32_t i=idx_start; i<M_num; i++) {

      if (idx_sparse[i] == 1) {

        vector<size_t> result;
        vector<double> dist;
        tree->radiusSearch(cloud.pts[i], out_dist, result, dist);

        int32_t num_neighbors = 0;
        for (int32_t j=1; j<result.size(); j++) {
          if (idx_sparse[result[j]]==1)
            num_neighbors++;
        }

        if (num_neighbors==0) {
          idx_sparse[i] = 0;
          idx_size--;
        }
      }
    }
  }

  // release memory of kd tree
  delete tree;
}

/**
 * @brief ExtractCols Extract columns of a matrix given indices
 * @param input Input matrix
 * @param output Output matrix
 * @param idx_sparse Binary masks indicate which columns should be preserved
 * @param idx_size Number of columns to be preserved
 */
void ExtractCols(Eigen::MatrixXd &input, Eigen::MatrixXd &output, int32_t *idx_sparse, int32_t idx_size){

    output.resize(input.rows(), idx_size);

    int32_t k=0;
    for (int i=0; i<input.cols(); i++){
        if (idx_sparse[i]){
            for (int c=0; c<input.rows(); c++){
                output(c,k) = input(c, i);
            }
            k++;
        }
    }
}

/**
 * @brief ExtractCols Extract columns of a matrix given indices
 * @param input Input matrix
 * @param output Output matrix
 * @param idx Indices of columns to extract
 */
void ExtractCols(Eigen::MatrixXd &input, Eigen::MatrixXd &output, vector<int> idx){

    int dim=input.rows();
    output.resize(dim, idx.size());
    for (int i=0; i<idx.size(); i++){
        for (int c=0; c<dim; c++){
            output(c, i) = input(c, idx[i]);
        }
    }
}

/**
 * @brief ExtractCols Extract columns of a matrix given indices
 * @param input Input matrix
 * @param output Output matrix
 * @param idx Indices of rows to extract
 */
void ExtractRows(Eigen::MatrixXd &input, Eigen::MatrixXd &output, vector<int> idx){

    int dim=input.cols();
    output.resize(idx.size(), dim);
    for (int i=0; i<idx.size(); i++){
        for (int c=0; c<dim; c++){
            output(i, c) = input(idx[i], c);
        }
    }
}

/**
 *@brief RemoveBlindSpot Remove the points within a sector
 *@param matIn Input point cloud
 *@param matOut Output point cloud
 *@param blind_splot_angle Sector angle
 */
void RemoveBlindSpot(Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, float blind_splot_angle){
    if (blind_splot_angle<=0){
        matOut = matIn;
        return;
    }
    Eigen::MatrixXd matInSub = matIn.block(0,0,matIn.rows(),2);
    Eigen::MatrixXd matNorm = matInSub.rowwise().norm();

    matInSub = matIn.block(0,0,matIn.rows(),1);
    Eigen::ArrayXd v = - matInSub.array() / matNorm.array();

    float angle = cos(blind_splot_angle/2);

    int k=0;
    matOut.resize(matIn.rows(), matIn.cols());
    for (int i=0; i<matIn.rows(); i++){
        if (v(i, 0)<=angle){
            matOut.block(k,0,1,matIn.cols()) = matIn.block(i,0,1,matIn.cols());
            k++;
        }
    }
    Eigen::MatrixXd matrixSlice = matOut.block(0,0,k,matIn.cols());
    matOut = matrixSlice;
}

/**
 *@brief CropVelodyneData Crop velodyne data given max and min distance
 *@param matIn Input point cloud
 *@param matOut Output point cloud
 *@param minDist Minimum distance to crop data
 *@param maxDist Maximum distance to crop data
 */
void CropVelodyneData (Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, float minDist, float maxDist){

    Eigen::MatrixXd matNorm = matIn.rowwise().norm();

    int k = 0;
    matOut.resize(matIn.rows(), matIn.cols());
    for (int i=0; i<matIn.rows(); i++){
        if (matNorm(i,0) > minDist && matNorm(i,0) < maxDist){
            matOut.block(k,0,1,matIn.cols()) = matIn.block(i,0,1,matIn.cols());
            k++;
        }
    }
    Eigen::MatrixXd matrixSlice = matOut.block(0,0,k,matIn.cols());
    matOut = matrixSlice;

}

/**
 *@brief CropVelodyneData Crop velodyne data given max and min distance
 *@param matIn Input point cloud
 *@param matOut Output indices of preserved points
 *@param minDist Minimum distance to crop data
 *@param maxDist Maximum distance to crop data
 */
void CropVelodyneData (Eigen::MatrixXd &matIn, vector<int> &idxOut, float minDist, float maxDist){

    Eigen::MatrixXd matNorm = matIn.rowwise().norm();

    idxOut.clear();
    for (int i=0; i<matIn.rows(); i++){
        if (matNorm(i,0) > minDist && matNorm(i,0) < maxDist){
            idxOut.push_back(i);
        }
    }
}

/**
 *@brief CurlVelodyneData Transform velodyne data given curl parameters
 *@param velo_in Input velodyne data
 *@param velo_out Output velodyne data
 *@param r Rotation matrix for curl
 *@param t Translation vector for curl
 */
void CurlVelodyneData (Eigen::MatrixXd &velo_in, Eigen::MatrixXd &velo_out, Eigen::Vector3d r, Eigen::Vector3d t){

  // for all points do
  int dim = velo_in.cols(); // 3
  int pt_num = velo_in.rows();
  velo_out.resize(pt_num, dim);
  for (int32_t i=0; i<pt_num; i++) {

    double vx = velo_in(i, 0);
    double vy = velo_in(i, 1);
    double vz = velo_in(i, 2);

    double s = 0.5*atan2(vy,vx)/M_PI;

    double rx = s*r(0);
    double ry = s*r(1);
    double rz = s*r(2);

    double tx = s*t(0);
    double ty = s*t(1);
    double tz = s*t(2);

    double theta = sqrt(rx*rx+ry*ry+rz*rz);

    if (theta>1e-10) {

      double kx = rx/theta;
      double ky = ry/theta;
      double kz = rz/theta;

      double ct = cos(theta);
      double st = sin(theta);

      double kv = kx*vx+ky*vy+kz*vz;

      velo_out(i, 0) = vx*ct + (ky*vz-kz*vy)*st + kx*kv*(1-ct) + tx;
      velo_out(i, 1) = vy*ct + (kz*vx-kx*vz)*st + ky*kv*(1-ct) + ty;
      velo_out(i, 2) = vz*ct + (kx*vy-ky*vx)*st + kz*kv*(1-ct) + tz;

    } else {

      velo_out(i, 0) = vx + tx;
      velo_out(i, 1) = vy + ty;
      velo_out(i, 2) = vz + tz;

    }

    // intensity
    // velo_out[i*4+3] = velo_in[i*4+3];
  }
}

/**
 *@brief GetHalfPoints Get either forward or backward points
 *@param matIn Input matrix
 *@param matOut Output matrix
 *@param direction Bool value indicating forward (true) or backward (false)
 */
void GetHalfPoints(Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, bool direction){

    int k = 0;
    matOut.resize(matIn.rows(), matIn.cols());
    for (int i=0; i<matIn.cols(); i++){
        if ((direction && matIn(0,i)>0) || (!direction && matIn(0,i)<0)){
            matOut.block(0,k,matIn.rows(),1) = matIn.block(0,i,matIn.rows(),1);
            k++;
        }
    }
    Eigen::MatrixXd matrixSlice = matOut.block(0,0,matIn.rows(),k);
    matOut = matrixSlice;
}

typedef struct {
    double r,g,b;
} COLOUR;

/**
 *@brief GetColour Turn a double into a color vector
 *@param v Input value
 *@param vmin Min value for coloring
 *@param vmax Max value for coloring
 */
COLOUR GetColour(double v,double vmin,double vmax)
{
   COLOUR c = {1.0,1.0,1.0}; // white
   double dv;

   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;
   dv = vmax - vmin;

   if (v < (vmin + 0.25 * dv)) {
      c.r = 0;
      c.g = 4 * (v - vmin) / dv;
   } else if (v < (vmin + 0.5 * dv)) {
      c.r = 0;
      c.b = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
   } else if (v < (vmin + 0.75 * dv)) {
      c.r = 4 * (v - vmin - 0.5 * dv) / dv;
      c.b = 0;
   } else {
      c.g = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
      c.b = 0;
   }

   return(c);
}

/**
 *@brief GetColorToHeight Assign a color to each point according to its height
 *@param pose Input point cloud
 *@param color Output color
 */
void GetColorToHeight(Eigen::MatrixXd &pose, Eigen::MatrixXd &color){
    // get height from poses
    Eigen::VectorXd height = pose.block(0,2,pose.rows(),1);

    // sort and get the quantiles
    sort(height.data(),height.data()+height.size());
    int idxL = (int)(ceil((double)height.size()*0.05));
    int idxH = (int)(floor((double)height.size()*0.95));
    double heightL = height(idxL);
    double heightH = height(idxH);

    for (int i=0; i<height.size(); i++){
        COLOUR c = GetColour(pose(i, 2), heightL, heightH);
        color(i, 0) = c.r;
        color(i, 1) = c.g;
        color(i, 2) = c.b;
    }
}


#pragma once

#include <Eigen/Dense>

#include "GNAObject.hh"
#include "Neutrino.hh"
#include "OscProbPMNS.hh"

class  OscProbMatter: public OscProbPMNSBase,
                      public TransformationBind<OscProbMatter> {
public:
    using TransformationBind<OscProbMatter>::transformation_;
    OscProbMatter(Neutrino from, Neutrino to, const std::string& baseline="L", const std::string& rho="rho");

    void calcOscProb(FunctionArgs fargs);

protected:
    variable<double> m_L;
    variable<double> m_rho;
    Neutrino m_from;
    Neutrino m_to;
};


/*
 * Copyright (C) 2018 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#include <curl/curl.h>
#include <boost/filesystem.hpp>

#include <ignition/math/Angle.hh>
#include <ignition/math/Helpers.hh>
#include <ignition/math/SphericalCoordinates.hh>
#include <ignition/math/Vector2.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/CommonIface.hh>
#include <gazebo/transport/Node.hh>

#include "StaticMapPlugin.hh"

namespace gazebo
{
  /// \brief Class to provide helper functions for Web Mercator projection
  class MercatorProjection
  {
    /// \brief Convert point in world coordinates to latitude and longitude
    /// \param[in] _point Point in world coordinates
    /// \return Latitude and longitude coorindates
    public: static ignition::math::SphericalCoordinates PointToLatLon(
        const ignition::math::Vector2d &_point);

    /// \brief Convert latitdue and longitude to point in world coordinates
    /// \param[in] Latitude and longitude coorindates
    /// \return Point in world coordinates
    public: static ignition::math::Vector2d LatLonToPoint(
        const ignition::math::SphericalCoordinates &_latLon);

    /// \brief Google map base level tile size
    public: static const unsigned int TILE_SIZE;
  };

  /// \brief Private data class for StaticMapPlugin
  class StaticMapPluginPrivate
  {
    /// \brief Download map tiles.
    /// \param[in] _centerLat Latitude of center point of map
    /// \param[in] _centerLon Longitude of center point of map
    /// \param[in] _zoom Map zoom level between 0 (entire world) and 21+
    /// (streets).
    /// \param[in] _tileSizePx Size of each map tile in pixels. Tiles will be
    /// square.
    /// \param[in] _worldSize Size of map in the world in meters.
    /// \param[in] _mapType Type of map to download: roadmap, satellite,
    /// terrain, hybrid
    /// \param[in] _apiKey Google API key
    /// \param[in] _saveDirPath Location in local filesystem to save tile
    /// images.
    public: std::vector<std::string> DownloadMapTiles(const double _centerLat,
        const double _centerLon, const unsigned int _zoom,
        const unsigned int _tileSizePx,
        const ignition::math::Vector2d &_worldSize,
        const std::string &_mapType, const std::string &_apiKey,
        const std::string &_saveDirPath);

    /// \brief Create textured map model and save it in specified path.
    /// \param[in] _name Name of map model
    /// \param[in] _tileWorldSize Size of map tiles in meters
    /// \param[in] _xNumTiles Number of tiles in x direction
    /// \param[in] _yNumTiles Number of tiles in y direction
    /// \param[in] _tiles Tile image filenames
    /// \param[in] _modelPath Path to model directory
    /// \return True if map tile model has been successfully created.
    public: bool CreateMapTileModel(
        const std::string &_name,
        const double _tileWorldSize,
        const unsigned int xNumTiles, const unsigned int yNumTiles,
        const std::vector<std::string> &_tiles, const std::string &_modelPath);

    /// \brief Get the ground resolution at the specified latitude and zoom
    /// level.
    /// \param[in] _lat Latitude
    /// \param[in] _zoom Map zoom Level
    /// \return Ground resolution in meters per pixel.
    public: double GroundResolution(const double _lat,
        const unsigned int _zoom) const;

    /// \brief Spawn a model into the world
    /// \param[in] _name Name of model
    /// \param[in] _pose Pose of model
    public: void SpawnModel(const std::string &_name,
        const ignition::math::Pose3d &_pose);

    /// \brief Pointer to world.
    public: physics::WorldPtr world;

    /// \brief Name of map model
    public: std::string modelName;

    /// \brief Pose of map model
    public: ignition::math::Pose3d modelPose;

    /// \brief Latitude and Longitude of map center
    public: ignition::math::Vector2d center;

    /// \brief Target size of world to be covered by map in meters.
    public: ignition::math::Vector2d worldSize;

    /// \brief Map zoom level. From 0 (entire world) to 21+ (streets)
    public: unsigned int zoom = 21u;

    /// \brief Size of map tile in pixels. 640 is max resolution for users of
    /// standard API
    public: unsigned int tileSizePx = 640u;

    /// \brief Type of map to use as texture: roadmap, satellite, terrain,
    /// hybrid
    public: std::string mapType = "satellite";

    /// \brief True to use cached model and image data from gazebo model path.
    /// False to redownload image tiles and recreate model sdf and config
    /// files.
    public: bool useCache = false;

    /// \brief Google API key
    public: std::string apiKey;

    /// \brief Filenames of map tile images
    public: std::vector<std::string> mapTileFilenames;

    /// \brief Pointer to a node for communication.
    public: transport::NodePtr node;

    /// \brief Factory publisher.
    public: transport::PublisherPtr factoryPub;

    /// \brief True if the plugin is loaded successfully
    public: bool loaded = false;
  };
}

using namespace gazebo;

const unsigned int MercatorProjection::TILE_SIZE = 256;

GZ_REGISTER_WORLD_PLUGIN(StaticMapPlugin)

/////////////////////////////////////////////////
size_t WriteData(void *_ptr, size_t _size, size_t _nmemb, FILE *_stream)
{
  return fwrite(_ptr, _size, _nmemb, _stream);
}

/////////////////////////////////////////////////
bool DownloadFile(const std::string &_url, const std::string &_outputFile)
{
  if (_url.empty())
    return false;

  CURL *curl = curl_easy_init();

  curl_easy_setopt(curl, CURLOPT_URL, _url.c_str());
  curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, WriteData);

  FILE *fp = fopen(_outputFile.c_str(), "wb");
  if (!fp)
  {
    gzerr << "Could not download model[" << _url << "] because we were"
      << "unable to write to file[" << _outputFile << "]."
      << "Please fix file permissions.";
    return false;
  }
  curl_easy_setopt(curl, CURLOPT_WRITEDATA, fp);

  char errbuf[CURL_ERROR_SIZE];
  // provide a buffer to store errors in
  curl_easy_setopt(curl, CURLOPT_ERRORBUFFER, errbuf);
  // set the error buffer as empty before performing a request
  errbuf[0] = 0;

  CURLcode success = curl_easy_perform(curl);
  if (success != CURLE_OK)
  {
    gzerr << "Error in REST request" << std::endl;
    size_t len = strlen(errbuf);
    fprintf(stderr, "\nlibcurl: (%d) ", success);
    if (len)
    {
      fprintf(stderr, "%s%s", errbuf,
              ((errbuf[len - 1] != '\n') ? "\n" : ""));
    }
    else
      fprintf(stderr, "%s\n", curl_easy_strerror(success));
  }
  fclose(fp);

  // Update the status code.
  int statusCode = 0;
  curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &statusCode);

  // Cleaning.
  curl_easy_cleanup(curl);

  return true;
}

/////////////////////////////////////////////////
ignition::math::Vector2d MercatorProjection::LatLonToPoint(
    const ignition::math::SphericalCoordinates &_latLon)
{
  // Adapted from:
  // https://developers.google.com/maps/documentation/javascript/examples/map-coordinates

  ignition::math::Vector2d point;
  // Truncating to 0.9999 effectively limits latitude to 89.189. This is
  // about a third of a tile past the edge of the world tile.
  double siny = std::min(std::max(std::sin(
      _latLon.LatitudeReference().Radian()), -0.9999), 0.9999);

  point.X() = TILE_SIZE * (0.5 + _latLon.LongitudeReference().Degree()/ 360.0);
  point.Y() = TILE_SIZE * (0.5 - std::log((1 + siny) / (1 - siny)) /
          (4 * IGN_PI));
  return point;
}

/////////////////////////////////////////////////
ignition::math::SphericalCoordinates MercatorProjection::PointToLatLon(
    const ignition::math::Vector2d &_point)
{
  ignition::math::SphericalCoordinates latLon;
  ignition::math::Angle lonAngle;
  ignition::math::Angle latAngle;

  double lonDegrees = (_point.X() / TILE_SIZE - 0.5) * 360;
  double latRadians = (_point.Y() - TILE_SIZE/2.0) /
        -(TILE_SIZE/(2*IGN_PI));

  lonAngle.Degree(lonDegrees);
  latAngle.Radian(2 * std::atan(std::exp(latRadians)) - IGN_PI / 2.0);

  latLon.SetLongitudeReference(lonAngle);
  latLon.SetLatitudeReference(latAngle);
  return latLon;
}

/////////////////////////////////////////////////
StaticMapPlugin::StaticMapPlugin()
  : dataPtr(new StaticMapPluginPrivate)
{
}

/////////////////////////////////////////////////
void StaticMapPlugin::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
  this->dataPtr->world = _world;

  if (!_sdf->HasElement("api_key"))
  {
    gzerr << "Missing Google API key needed to download map tiles" << std::endl;
    return;
  }

  if (!_sdf->HasElement("center"))
  {
    gzerr << "Please specify latitude and longitude coordinates of map center"
          << std::endl;
    return;
  }

  if (!_sdf->HasElement("world_size"))
  {
    gzerr << "Please specify size of map to cover in meters" << std::endl;
    return;
  }

  this->dataPtr->apiKey = _sdf->Get<std::string>("api_key");
  this->dataPtr->center =
      _sdf->Get<ignition::math::Vector2d>("center");
  double wSize = _sdf->Get<double>("world_size");
  if (wSize > 0)
  {
    // support only square map for now
    this->dataPtr->worldSize.X() = wSize;
    this->dataPtr->worldSize.Y() = wSize;
  }
  else
  {
    gzerr << "World size must be greater than 0 meters" << std::endl;
    return;
  }

  // optional params
  if (_sdf->HasElement("zoom"))
    this->dataPtr->zoom = _sdf->Get<unsigned int>("zoom");

  if (_sdf->HasElement("tile_size"))
  {
    this->dataPtr->tileSizePx = _sdf->Get<unsigned int>("tile_size");
    if (this->dataPtr->tileSizePx > 640u)
    {
      gzerr << "Tile size exceeds standard API usage limit. Setting to 640px."
            << std::endl;
      this->dataPtr->tileSizePx = 640u;
    }
  }

  if (_sdf->HasElement("map_type"))
    this->dataPtr->mapType= _sdf->Get<std::string>("map_type");

  if (_sdf->HasElement("use_cache"))
    this->dataPtr->useCache = _sdf->Get<bool>("use_cache");

  if (_sdf->HasElement("pose"))
    this->dataPtr->modelPose = _sdf->Get<ignition::math::Pose3d>("pose");

  if (_sdf->HasElement("model_name"))
    this->dataPtr->modelName = _sdf->Get<std::string>("model_name");
  else
  {
    // generate name based on input
    std::stringstream name;
    name << "map_" << this->dataPtr->mapType << "_" << std::setprecision(9)
         << this->dataPtr->center.X() << "_" << this->dataPtr->center.Y()
         << "_" << this->dataPtr->worldSize.X()
         << "_" << this->dataPtr->worldSize.Y();
    this->dataPtr->modelName = name.str();
  }

  this->dataPtr->loaded = true;
}

/////////////////////////////////////////////////
void StaticMapPlugin::Init()
{
  // don't init if params are not loaded successfully
  if (!this->dataPtr->loaded)
    return;

  // check if model exists locally
  auto basePath = common::SystemPaths::Instance()->GetLogPath() /
        boost::filesystem::path("models");

  this->dataPtr->node = transport::NodePtr(new transport::Node());
  this->dataPtr->node->Init();
  this->dataPtr->factoryPub =
      this->dataPtr->node->Advertise<msgs::Factory>("~/factory");

  boost::filesystem::path modelPath = basePath / this->dataPtr->modelName;
  if (this->dataPtr->useCache && common::exists(modelPath.string()))
  {
    gzmsg << "Model: '" << this->dataPtr->modelName << "' exists. "
          << "Spawning existing model.." << std::endl;
    this->dataPtr->SpawnModel("model://" + this->dataPtr->modelName,
        this->dataPtr->modelPose);
    return;
  }

  // create tmp dir to save model files
  boost::filesystem::path tmpModelPath =
      boost::filesystem::temp_directory_path() / this->dataPtr->modelName;
  boost::filesystem::path scriptsPath(tmpModelPath / "materials" / "scripts");
  boost::filesystem::create_directories(scriptsPath);
  boost::filesystem::path texturesPath(tmpModelPath / "materials" / "textures");
  boost::filesystem::create_directories(texturesPath);

  // download map tile images into model/materials/textures
  std::vector<std::string> tiles = this->dataPtr->DownloadMapTiles(
      this->dataPtr->center.X(),
      this->dataPtr->center.Y(),
      this->dataPtr->zoom,
      this->dataPtr->tileSizePx,
      this->dataPtr->worldSize,
      this->dataPtr->mapType,
      this->dataPtr->apiKey,
      texturesPath.string());

  // assume square model for now
  unsigned int xNumTiles = std::sqrt(tiles.size());
  unsigned int yNumTiles = xNumTiles;

  double tileWorldSize = this->dataPtr->GroundResolution(
      IGN_DTOR(this->dataPtr->center.X()), this->dataPtr->zoom)
      * this->dataPtr->tileSizePx;

  // create model and spawn it into the world
  if (this->dataPtr->CreateMapTileModel(
      this->dataPtr->modelName, tileWorldSize,
      xNumTiles, yNumTiles, tiles, tmpModelPath.string()))
  {
    // verify model dir is created
    if (common::exists(tmpModelPath.string()))
    {
      // remove existing map model
      if (common::exists(modelPath.string()))
        boost::filesystem::remove_all(modelPath);

      try
      {
        // move new map model to gazebo model path
        boost::filesystem::rename(tmpModelPath, modelPath);
      }
      catch(boost::filesystem::filesystem_error &_e)
      {
        // rename failed. Could be an invalid cross-device link error
        // try copy and remove method
        bool result = common::copyDir(tmpModelPath, modelPath);
        if (result)
        {
          boost::filesystem::remove_all(tmpModelPath);
        }
        else
        {
          gzerr<< "Unable to copy model from '" << tmpModelPath.string()
                 << "' to '" << modelPath.string() << "'" << std::endl;
          return;
        }
      }
      // spawn the model
      this->dataPtr->SpawnModel("model://" + this->dataPtr->modelName,
          this->dataPtr->modelPose);
    }
    else
      gzerr << "Failed to create model: " << tmpModelPath.string() << std::endl;
  }
}

/////////////////////////////////////////////////
double StaticMapPluginPrivate::GroundResolution(const double _lat,
    const unsigned int _zoom) const
{
  double earthEquatorialRadius = 6378137;
  double metersPerPx = 2 * IGN_PI * earthEquatorialRadius *
      std::cos(_lat) / (MercatorProjection::TILE_SIZE * std::pow(2, _zoom));
  return metersPerPx;
}

/////////////////////////////////////////////////
std::vector<std::string> StaticMapPluginPrivate::DownloadMapTiles(
    const double _centerLat, const double _centerLon,
    const unsigned int _zoom, const unsigned int _tileSizePx,
    const ignition::math::Vector2d &_worldSize, const std::string &_mapType,
    const std::string &_apiKey, const std::string &_saveDirPath)
{
  ignition::math::Angle lonAngle;
  ignition::math::Angle latAngle;
  latAngle.Degree(_centerLat);
  lonAngle.Degree(_centerLon);
  ignition::math::SphericalCoordinates centerLatLon;
  centerLatLon.SetLatitudeReference(latAngle);
  centerLatLon.SetLongitudeReference(lonAngle);

  // ground resolution - varies by latitude and zoom level
  double metersPerPx =
      this->GroundResolution(centerLatLon.LatitudeReference().Radian(), _zoom);

  // determine number of tiles necessary to cover specified world size
  unsigned int xNumTiles = static_cast<unsigned int>(
      std::ceil(_worldSize.X() / metersPerPx /
      _tileSizePx));
  // y is only approximate because ground resolution is based on latitude
  unsigned int yNumTiles = static_cast<unsigned int>(
      std::ceil(_worldSize.Y() / metersPerPx /
      _tileSizePx));

  // scale for converting between pixel and world point
  double scale = std::pow(2, _zoom);

  ignition::math::Vector2d centerPx =
      MercatorProjection::LatLonToPoint(centerLatLon) * scale;

  // compute starting x, y values in pixel coordinates
  double halfWidthPx = _tileSizePx * std::floor(xNumTiles / 2);
  double halfHeightPx = _tileSizePx * std::floor(yNumTiles / 2);
  double halfTileSize = _tileSizePx / 2.0;
  double x = centerPx.X() - halfWidthPx;
  double y = centerPx.Y() - halfHeightPx;
  if (xNumTiles % 2 == 0u)
    x += halfTileSize;
  if (yNumTiles % 2 == 0u)
    y += halfTileSize;
  double startx = x;

  // download map tiles using google static map API
  std::string url = "https://maps.googleapis.com/maps/api/staticmap";
  for (unsigned int i = 0; i < yNumTiles; ++i)
  {
    for (unsigned int j = 0; j < xNumTiles; ++j)
    {
      // convert from pixels to world point
      auto px = ignition::math::Vector2d(x, y);
      auto point = px / scale;
      // convert world point to lat lon
      auto latLon = MercatorProjection::PointToLatLon(point);

      // download tile image
      std::stringstream query;
      query << "?center="
            << std::setprecision(9)
            << latLon.LatitudeReference().Degree() << ","
            << latLon.LongitudeReference().Degree()
            << "&zoom=" << _zoom
            << "&size=" << _tileSizePx << "x" << _tileSizePx
            << "&maptype=" << _mapType
            << "&key=" << _apiKey;
      std::string fullURL = url + query.str();
      std::stringstream filename;
      filename << "tile_"
               << std::setprecision(9) << latLon.LatitudeReference().Degree()
               << "_" << latLon.LongitudeReference().Degree() << ".png";
      std::string fullPath = _saveDirPath + "/" + filename.str();
      DownloadFile(fullURL, fullPath);
      gzmsg << "Downloading map tile: " << filename.str() << std::endl;
      mapTileFilenames.push_back(filename.str());

      x += _tileSizePx;
    }
    x = startx;
    y += _tileSizePx;
  }
  return mapTileFilenames;
}

/////////////////////////////////////////////////
bool StaticMapPluginPrivate::CreateMapTileModel(
    const std::string &_name,
    const double _tileWorldSize,
    const unsigned int _xNumTiles, const unsigned int _yNumTiles,
    const std::vector<std::string> &_tiles, const std::string &_modelPath)
{
  // create material script
  std::stringstream materialScriptStr;
  for (unsigned int i = 0; i < _yNumTiles; ++i)
  {
    for (unsigned int j = 0; j < _xNumTiles; ++j)
    {
      materialScriptStr <<
        "material " << _name << "/" << i << "_" << j << "\n"
        "{\n"
        "  technique\n"
        "  {\n"
        "    pass\n"
        "    {\n"
        "      texture_unit\n"
        "      {\n"
        "        texture " << _tiles[j + i * _xNumTiles] << "\n"
        "      }\n"
        "    }\n"
        "  }\n"
        "}\n\n";
    }
  }

  // save material script file to disk
  boost::filesystem::path scriptFilePath(_modelPath);
  scriptFilePath = scriptFilePath / "materials" / "scripts"
      / "map_tiles.material";
  std::ofstream scriptFile;
  scriptFile.open(scriptFilePath.string().c_str());
  if (!scriptFile.is_open())
  {
    gzerr << "Couldn't open file for writing: " << scriptFilePath.string()
          << std::endl;
    return false;
  }
  scriptFile << materialScriptStr.str();
  scriptFile.close();

  // create model.sdf file
  double sizeX = _tileWorldSize;
  double sizeY = _tileWorldSize;
  double sizeZ = 1.0;

  ignition::math::Vector2d wCenter(0, 0);
  double halfTileWorldWidth = sizeX * std::floor(_xNumTiles / 2);
  double halfTileWorldHeight = sizeY * std::floor(_yNumTiles / 2);
  double x = wCenter.X() - halfTileWorldWidth;
  double y = wCenter.Y() + halfTileWorldHeight;
  double halfTileWorldSizeX = sizeX / 2.0;
  double halfTileWorldSizeY = sizeY / 2.0;
  if (_xNumTiles % 2 == 0u)
    x += halfTileWorldSizeX;
  if (_yNumTiles % 2 == 0u)
    y -= halfTileWorldSizeY;
  double startx = x;
  double zPos = -sizeZ / 2.0;

  // rotate around z to line up textures
  ignition::math::Vector3d tileRot(0, 0, IGN_PI / 2.0);
  ignition::math::Vector3d colSize(sizeX * _xNumTiles, sizeY * _yNumTiles,
      sizeZ);

  // Model will have boxed-shaped tiles with z size of 1.0
  // Surface of model will be at z=0.0
  std::stringstream newModelStr;
  newModelStr << "<sdf version='" << SDF_VERSION << "'>\n"
    "<model name='" << _name << "'>\n"
    "  <static>true</static>\n"
    "  <link name='link'>\n"
    "    <collision name='collision'>\n"
    "      <pose>0 0 " << zPos << " 0 0 0</pose>\n"
    "      <geometry>\n"
    "        <box>\n"
    "          <size>" << colSize << "</size>\n"
    "        </box>\n"
    "      </geometry>\n"
    "    </collision>\n";
  for (unsigned int i = 0; i < _yNumTiles; ++i)
  {
    for (unsigned int j = 0; j < _xNumTiles; ++j)
    {
      newModelStr <<
        "    <visual name='visual" << i << "_" << j <<"'>\n"
        "      <pose>" << x << " " << y << " " << zPos
                       << " " << tileRot << "</pose>\n"
        "      <geometry>\n"
        "        <box>\n"
        "          <size>" << sizeX << " " << sizeY << " " << sizeZ
                           << "</size>\n"
        "        </box>\n"
        "      </geometry>\n"
        "      <material>\n"
        "        <script>\n"
        "          <uri>model://" << _name << "/materials/scripts</uri>\n"
        "          <uri>model://" << _name << "/materials/textures</uri>\n"
        "          <name>" << _name << "/" << i << "_" << j << "</name>\n"
        "        </script>\n"
        "      </material>\n"
        "    </visual>\n";
      x += sizeX;
    }
    x = startx;
    y -= sizeY;
  }
  newModelStr <<
    "  </link>\n"
    "</model>\n"
    "</sdf>";

  // save model.sdf file to disk
  boost::filesystem::path modelSDFFilePath(_modelPath);
  modelSDFFilePath /= "model.sdf";
  std::ofstream modelSDFFile;
  modelSDFFile.open(modelSDFFilePath.string().c_str());
  if (!modelSDFFile.is_open())
  {
    gzerr << "Couldn't open file for writing: " << modelSDFFilePath.string()
          << std::endl;
    return false;
  }
  modelSDFFile << newModelStr.str();
  modelSDFFile.close();

  // create model.config file
  std::ostringstream modelConfigStr;
  modelConfigStr << "<?xml version=\"1.0\"?>\n"
  << "<model>\n"
  << "  <name>" << _name << "</name>\n"
  << "  <version>1.0</version>\n"
  << "  <sdf version=\"" << SDF_VERSION << "\">model.sdf</sdf>\n"
  << "  <author>\n"
  << "    <name>gazebo</name>\n"
  << "    <email></email>\n"
  << "  </author>\n"
  << "  <description>\n"
  << "    Made with Gazebo using Google Static Map API. "
  <<     "https://developers.google.com/maps/documentation/static-maps\n"
  << "  </description>\n"
  << "</model>";

  // save model.config file to disk
  boost::filesystem::path modelConfigFilePath(_modelPath);
  modelConfigFilePath /= "model.config";
  std::ofstream modelConfigFile;
  modelConfigFile.open(modelConfigFilePath.string().c_str());
  if (!modelConfigFile.is_open())
  {
    gzerr << "Couldn't open file for writing: "
        << modelConfigFilePath.string() << std::endl;
    return false;
  }
  modelConfigFile << modelConfigStr.str();
  modelConfigFile.close();

  return true;
}

/////////////////////////////////////////////////
void StaticMapPluginPrivate::SpawnModel(const std::string &_uri,
    const ignition::math::Pose3d &_pose)
{
  // publish to factory topic to spawn the model
  msgs::Factory msg;
  msg.set_sdf_filename(_uri);
  msgs::Set(msg.mutable_pose(), _pose);
  this->factoryPub->Publish(msg);
}


#include <cdrm_welding/kinematic_utils.h>

#include <Eigen/Geometry>
#include <moveit/robot_state/robot_state.h>

namespace cdrm_welding
{
void applyNozzleConfigurationToState(moveit::core::RobotState &state,
                                     const Eigen::VectorXd &q,
                                     const moveit::core::LinkModel *ee)
{
  // The three dimensions are Rx, Ry, CTWD. We want to place the nozzle at origin and then
  // transform it by these amounts and voxelise it.
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
  tf.rotate(Eigen::AngleAxisd(q(0), Eigen::Vector3d::UnitX()));
  tf.rotate(Eigen::AngleAxisd(q(1), Eigen::Vector3d::UnitY()));
  tf.translation() -= q(2) * tf.linear().col(2);

  state.updateStateWithLinkAt(ee, tf, true);
}

void applyToolConfigurationToState(moveit::core::RobotState &state,
                                   const Eigen::VectorXd &q,
                                   const moveit::core::LinkModel *ee)
{
  // Dimensions are Rx, Ry, Rz and CTWD.
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
  tf.rotate(Eigen::AngleAxisd(q(0), Eigen::Vector3d::UnitX()));
  tf.rotate(Eigen::AngleAxisd(q(1), Eigen::Vector3d::UnitY()));
  tf.rotate(Eigen::AngleAxisd(q(2), Eigen::Vector3d::UnitZ()));
  tf.translation() -= q(3) * tf.linear().col(2);

  // Get the parent flange.
  const auto *flange_link = ee;

  while (flange_link->getParentJointModel()->getType() == moveit::core::JointModel::FIXED)
    flange_link = flange_link->getParentLinkModel();

  const Eigen::Isometry3d tf_nozzle_flange = state.getGlobalLinkTransform(ee).inverse() *
                                             state.getGlobalLinkTransform(flange_link);

  state.updateStateWithLinkAt(flange_link, tf * tf_nozzle_flange);
}
}


/*
   The MIT License (MIT)

   Copyright (c) 2011 - 2013, Philipp Heise and Sebastian Klose

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
*/

//
//  SL3Test.cpp
//  CVTools
//
//  Created by Sebastian Klose on 08.02.11.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#include <cvt/math/SL3.h>
#include <cvt/math/Math.h>
#include <cvt/math/Matrix.h>
#include <cvt/util/CVTTest.h>

#include <Eigen/Core>

namespace cvt {

    static bool testJacobianWithGenerators()
    {
        Eigen::Matrix<double, 3, 8> jGenerated, jAnalytic, jDiff;

		SL3<double> pose;
		pose.set( Math::deg2Rad( 10 ), Math::deg2Rad( -25 ), 0.76, 0.89, 10, -10, 0.0001, 0.0002 );

		Eigen::Matrix<double, 3, 1> point;
		Eigen::Matrix<double, 3, 1> p, col;
		point[ 0 ] = 13; point[ 1 ] = 8; point[ 2 ] = 12;;

		pose.transform( p, point );

		for( size_t i = 0; i < SL3<double>::NPARAMS; i++ ){

			col = SL3<double>::generator( i ) * p;
			jGenerated.block<3, 1>( 0, i ) = col;
		}

		pose.jacobian( jAnalytic, point );

		bool b, ret = true;
		jDiff = jAnalytic - jGenerated;
		b = ( jDiff.array().abs().sum() / 24.0 ) < 0.00001;
		CVTTEST_PRINT( "Generated Pose Jacobian", b );
		if( !b ){
			std::cout << "Analytic:\n" << jAnalytic << std::endl;
			std::cout << "Generated:\n" << jGenerated << std::endl;
			std::cout << "Difference:\n" << jDiff << std::endl;
		}
		ret &= b;

        return ret;
    }

	static bool testJacobian()
	{
		Eigen::Matrix<double, 8, 1> delta = Eigen::Matrix<double, 8, 1>::Zero();
		Eigen::Matrix<double, 3, 8> jNumeric, jAnalytic, jDiff;
		Eigen::Matrix<double, 2, 8> sJN, sJA, sJDiff;

		SL3<double> pose;
		pose.set( Math::deg2Rad( 10 ), Math::deg2Rad( -25 ), 0.76, 0.89, 10, -10, 0.0001, 0.0002 );

		Eigen::Matrix<double, 3, 1> point;
		Eigen::Matrix<double, 3, 1> p, pp;
		Eigen::Matrix<double, 2, 1> sp, spp;
		point[ 0 ] = 13; point[ 1 ] = 8; point[ 2 ] = 12;;

		pose.transform( p, point );
		pose.project( sp, point );

		double h = 0.000000001;
		for( size_t i = 0; i < 8; i++ ){
			delta[ i ] = h;

			pose.apply( delta );

			pose.transform( pp, point );
			jNumeric.col( i ) = ( pp - p ) / h;

			pose.project( spp, point );
			sJN.col( i ) = ( spp - sp ) / h;

			delta[ i ] = -h;
			pose.apply( delta );
			delta[ i ] = 0;
		}

		pose.jacobian( jAnalytic, point );
		pose.project( spp, sJA, point );

		bool b, ret = true;
		jDiff = jAnalytic - jNumeric;
		b = ( jDiff.array().abs().sum() / 24.0 ) < 0.00001;

		CVTTEST_PRINT( "Pose Jacobian", b );
		if( !b ){
			std::cout << "Analytic:\n" << jAnalytic << std::endl;
			std::cout << "Numeric:\n" << jNumeric << std::endl;
			std::cout << "Difference:\n" << jDiff << std::endl;
		}
		ret &= b;

		sJDiff = sJA - sJN;
		b = ( sJDiff.array().abs().sum() / 16.0 ) < 0.0001;
		CVTTEST_PRINT( "Screen Jacobian", b );
		if( !b ){
			std::cout << "Analytic:\n" << sJA << std::endl;
			std::cout << "Numeric:\n" << sJN << std::endl;
			std::cout << "Difference:\n" << sJDiff << std::endl;
			std::cout << "Avg. Diff: " << sJDiff.array().abs().sum() / 16.0 << std::endl;
		}
		ret &= b;

		return ret;
	}

	static bool testHessian()
	{
        Eigen::Matrix<double, 8, 1> delta = Eigen::Matrix<double, 8, 1>::Zero();
		Eigen::Matrix<double, 24, 8> hN, hA;

		SL3<double> pose;
		pose.set( Math::deg2Rad( 10 ), Math::deg2Rad( -25 ), 0.76, 0.89, 10, -10, 0.0001, 0.0002 );

		Eigen::Matrix<double, 3, 1> point;
		Eigen::Matrix<double, 3, 1> p, ff, fb, bf, bb, xxf, xxb, hess;
		point[ 0 ] = 16;
		point[ 1 ] = 80;
		point[ 2 ] = 13;

		pose.transform( p, point );

		double h = 0.001;
		for( size_t i = 0; i < 8; i++ ){
            for( size_t j = 0; j < 8; j++ ){
				delta.setZero();
				if( i == j ){
					// +
					delta[ j ] = h;
					pose.apply( delta );
					pose.transform( xxf, point );
					pose.apply( -delta );

					delta[ j ] = -h;
					pose.apply( delta );
					pose.transform( xxb, point );
					pose.apply( -delta );

					hess = ( xxb - 2 * p + xxf ) / ( h*h );
				} else {
					delta[ i ] = h;
					delta[ j ] = h;
					pose.apply( delta );
					pose.transform( ff, point );
					pose.apply( -delta );

					delta[ i ] = h;
					delta[ j ] = -h;
					pose.apply( delta );
					pose.transform( fb, point );
					pose.apply( -delta );

					delta[ i ] = -h;
					delta[ j ] =  h;
					pose.apply( delta );
					pose.transform( bf, point );
					pose.apply( -delta );

					delta[ i ] = -h;
					delta[ j ] = -h;
					pose.apply( delta );
					pose.transform( bb, point );
					pose.apply( -delta );

					hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
				}

                hN( 3 * i , j ) = hess[ 0 ];
                hN( 3 * i + 1 , j ) = hess[ 1 ];
                hN( 3 * i + 2 , j ) = hess[ 2 ];
            }
		}

		pose.hessian( hA, p );

		bool b, ret = true;
        Eigen::Matrix<double, 24, 8> jDiff;
        jDiff = hN - hA;
		b = ( jDiff.array().abs().sum() / ( 24.0 * 8.0 ) ) < 0.00001;

		CVTTEST_PRINT( "Pose Hessian", b );
		if( !b ){
			std::cout << "Analytic:\n" << hA << std::endl;
			std::cout << "Numeric:\n" << hN << std::endl;
			std::cout << "Difference:\n" << jDiff << std::endl;
		}
		ret &= b;

		return ret;
	}

    static bool testScreenHessian()
    {
        Eigen::Matrix<double, 8, 1> delta = Eigen::Matrix<double, 8, 1>::Zero();
		Eigen::Matrix<double, 8, 8> shNumericX, shNumericY, shX, shY;

		SL3<double> pose;
		pose.set( Math::deg2Rad( 10 ), Math::deg2Rad( -25 ), 0.76, 0.89, 10, -10, 0.0001, 0.0002 );

		Eigen::Matrix<double, 3, 1> point, ptrans;
		Eigen::Matrix<double, 2, 1> sp, ff, fb, bf, bb, xxf, xxb, hess;
		point[ 0 ] = 13; point[ 1 ] = 8; point[ 2 ] = 13;

        // project the point with current parameters
		pose.project( sp, point );
		pose.transform( ptrans, point );

		double h = 0.0001;
		for( size_t i = 0; i < 8; i++ ){
            for( size_t j = 0; j < 8; j++ ){

				if( i == j ){
					// +
					delta[ j ] = h;
					pose.apply( delta );
					pose.project( xxf, point );
					delta[ j ] = -2 * h;
					pose.apply( delta );
					pose.project( xxb, point );

					hess = ( xxb - 2 * sp + xxf ) / ( h*h );

					// back to start
					delta[ j ] = h;
					pose.apply( delta );
					delta[ j ] = 0;
				} else {
					delta[ i ] = h;
					delta[ j ] = h;
					pose.apply( delta );
					pose.project( ff, point );
					pose.apply( -delta );

					delta[ i ] = h;
					delta[ j ] = -h;
					pose.apply( delta );
					pose.project( fb, point );
					pose.apply( -delta );

					delta[ i ] = -h;
					delta[ j ] =  h;
					pose.apply( delta );
					pose.project( bf, point );
					pose.apply( -delta );

					delta[ i ] = -h;
					delta[ j ] = -h;
					pose.apply( delta );
					pose.project( bb, point );
					pose.apply( -delta );

					hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
					delta.setZero();
				}

                shNumericX( i, j ) = hess[ 0 ];
                shNumericY( i, j ) = hess[ 1 ];

            }
		}

		pose.screenHessian( shX, shY, sp );

		bool b, ret = true;
        Eigen::Matrix<double, 8, 8> jDiff;
        jDiff = shNumericX - shX;
		b = ( jDiff.array().abs().sum() / 64.0 ) < 0.001;

		CVTTEST_PRINT( "Pose ScreenHessian X", b );
		if( !b ){
			std::cout << "Analytic:\n" << shX << std::endl;
			std::cout << "Numeric:\n" << shNumericX << std::endl;
			std::cout << "Difference:\n" << jDiff << std::endl;
		}
		ret &= b;

        jDiff = shNumericY - shY;
		b = ( jDiff.array().abs().sum() / 64.0 ) < 0.001;

		CVTTEST_PRINT( "Pose ScreenHessian Y", b );
		if( !b ){
			std::cout << "Analytic:\n" << shY << std::endl;
			std::cout << "Numeric:\n" << shNumericY << std::endl;
			std::cout << "Difference:\n" << jDiff << std::endl;
		}
		ret &= b;

        return ret;
    }

    static bool testMatrices( const Eigen::Matrix3d & m1, const Matrix3d & m2 )
    {
        for ( size_t i = 0; i < 3; i++ ) {
            for ( size_t k = 0; k < 3; k++ ) {
                if ( Math::abs( m1( i, k ) - m2[ i ][ k ] ) > 0.00001 ) {
                    return false;
                }
            }
        }
        return true;
    }

    static bool testSet()
    {
        bool result = true;

        SL3<double> pose;
        Matrix3d H;

        pose.set( 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0 );
        H.setHomography( 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0 );

        result &= testMatrices( pose.transformation(), H );

        return result;
    }

BEGIN_CVTTEST( SL3 )
	bool ret = true;

	SL3<double> pose;
	bool b = true;
	b = ( pose.transformation() == Eigen::Matrix<double, 3, 3>::Identity() );

	CVTTEST_PRINT( "Initialization", b );
	ret &= b;

    b = testSet();
    CVTTEST_PRINT( "SL3::set( ... )", b );
	ret &= b;

	ret &= testJacobian();
	ret &= testJacobianWithGenerators();
    ret &= testHessian();
    ret &= testScreenHessian();

	return ret;
END_CVTTEST

}


//==================================================================================================
/*!
  @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_ARCH_COMMON_SCALAR_FUNCTION_POW_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_POW_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/arch/common/detail/generic/pow_expander.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    inline A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      auto ltza0 = is_ltz(a0);
      auto isinfa1 = is_inf(a1);
      if( a0 == Mone<A0>() && isinfa1 ) return One<A0>();
      if( ltza0 && !is_flint(a1) && !is_inf(a1) ) return Nan<A0>();
      A0 z = pow_abs(a0, a1);
      if (isinfa1) return z;
      return  (is_negative(a0) && is_odd(a1)) ? -z : z;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    inline A0 operator() (const std_tag &,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return std::pow(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::constant_< bd::uint_<A1> >
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE result_type operator() ( A0 a0, A1) const BOOST_NOEXCEPT
    {
      return pow_expander<A1::value>::call(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::constant_< bd::int_<A1> >
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE result_type operator() ( A0  a0, A1) const BOOST_NOEXCEPT
    {
      return eval(a0, boost::mpl::bool_<(A1::value >= 0)>());
    }

    BOOST_FORCEINLINE result_type eval( A0  a0, boost::mpl::true_) const BOOST_NOEXCEPT
    {
      return pow_expander<A1::value>::call(a0);
    }

    BOOST_FORCEINLINE result_type eval( A0  a0, boost::mpl::false_) const BOOST_NOEXCEPT
    {
      return pow_expander<-A1::value>::call(rec(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::uint_<A1> >
                          )
  {
    using result_type = A0;

    A0 operator() ( A0  a0, A1  a1) const BOOST_NOEXCEPT
    {
      A0 base = a0;
      A1 expo = a1;

      result_type result = One<result_type>();
      while(expo)
      {
        if(is_odd(expo))
          result *= base;
        expo >>= 1;
        base = sqr(base);
      }

      return result;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          , bd::scalar_< bd::int_<A1> >
                          )
  {
    using result_type = A0;

    BOOST_FORCEINLINE A0 operator() ( A0  a0, A1  a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( boost::simd::assert_all(a1 >= 0), "integral pow with signed exponent" );

      using u_t =  bd::as_integer_t<A1, unsigned>;
      return pow(a0, bitwise_cast<u_t>(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::int_<A1> >
                          )
  {
    using result_type = A0;

    A0 operator() ( A0  a0, A1  a1) const BOOST_NOEXCEPT
    {
      using u_t =  bd::as_integer_t<A1, unsigned>;
      auto ltza1 = is_ltz(a1);
      A0 p = pow(a0, bitwise_cast<u_t>(ltza1? -a1 : a1));
      return ltza1 ? rec(p) : p;
    }
  };
} } }

#endif


#ifndef ALEPH_GEOMETRY_HEAT_KERNEL_HH__
#define ALEPH_GEOMETRY_HEAT_KERNEL_HH__

#include <aleph/config/Eigen.hh>

#ifdef ALEPH_WITH_EIGEN
  #include <Eigen/Core>
  #include <Eigen/Eigenvalues>
#endif

#include <aleph/math/KahanSummation.hh>

#include <algorithm>
#include <unordered_map>
#include <stdexcept>
#include <string>
#include <vector>

#include <cmath>

#define THROW_EIGEN_REQUIRED_ERROR()\
{\
  auto message =  std::string( __FILE__ )           \
                + std::string( ":" )                \
                + std::to_string( __LINE__ )        \
                + std::string( " in " )             \
                + std::string( __PRETTY_FUNCTION__ )\
                + std::string( ":" )                \
                + std::string( " Eigen is required for this function to work properly" );\
  \
  throw std::runtime_error( message );\
}

namespace aleph
{

namespace geometry
{

#ifdef ALEPH_WITH_EIGEN

/**
  Extracts a weighted adjacency matrix from a simplicial complex. At
  present, this function only supports adjacencies between edges, so
  the resulting matrix is a graph adjacency matrix.

  @param K Simplicial complex

  @returns Weighted adjacency matrix. The indices of rows and columns
           follow the order of the vertices in the complex.
*/

template <class SimplicialComplex> auto weightedAdjacencyMatrix( const SimplicialComplex& K ) -> Eigen::Matrix<typename SimplicialComplex::ValueType::DataType, Eigen::Dynamic, Eigen::Dynamic>
{
  using Simplex    = typename SimplicialComplex::ValueType;
  using VertexType = typename Simplex::VertexType;
  using DataType   = typename Simplex::DataType;
  using Matrix     = Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>;

#if EIGEN_VERSION_AT_LEAST(3,3,0)
  using IndexType  = Eigen::Index;
#else
  using IndexType  = typename Matrix::Index;
#endif

  // Prepare map from vertex to index ----------------------------------

  std::unordered_map<VertexType, IndexType> vertex_to_index;
  IndexType n = IndexType();

  {
    std::vector<VertexType> vertices;
    K.vertices( std::back_inserter( vertices ) );

    IndexType index = IndexType();

    for( auto&& vertex : vertices )
      vertex_to_index[vertex] = index++;

    n = static_cast<IndexType>( vertices.size() );
  }

  // Prepare matrix ----------------------------------------------------

  Matrix W = Matrix::Zero( n, n );

  for(auto&& s : K )
  {
    if( s.dimension() != 1 )
      continue;

    auto&& u = s[0];
    auto&& v = s[1];
    auto&& i = vertex_to_index.at( u );
    auto&& j = vertex_to_index.at( v );

    W(i,j)   = s.data();
    W(j,i)   = W(i,j);
  }

  return W;
}

/**
  Calculates the weighted Laplacian matrix of a given simplicial
  complex and returns it.

  @param K Simplicial complex

  @returns Weighted Laplacian matrix. The indices of rows and columns
           follow the order of the vertices in the complex.
*/

template <class SimplicialComplex> auto weightedLaplacianMatrix( const SimplicialComplex& K ) -> Eigen::Matrix<typename SimplicialComplex::ValueType::DataType, Eigen::Dynamic, Eigen::Dynamic>
{
  auto W          = weightedAdjacencyMatrix( K );
  using Matrix    = decltype(W);
  using IndexType = typename Matrix::Index;

  Matrix L = Matrix::Zero( W.rows(), W.cols() );

  auto V = W.rowwise().sum();

  for( IndexType i = 0; i < V.size(); i++ )
    L(i,i) = V(i);

  return L - W;
}

/**
  Calculates the Moore--Penrose pseudo-inverse of the weighted Laplacian
  matrix of a given simplicial complex and returns it.

  @param K Simplicial complex

  @returns Moore--Penrose pseudo-inverse matrix. The indices of rows and
           columns follow the order of the vertices in the complex.
*/

template <class SimplicialComplex> auto pinvWeightedLaplacianMatrix( const SimplicialComplex& K ) -> Eigen::Matrix<typename SimplicialComplex::ValueType::DataType, Eigen::Dynamic, Eigen::Dynamic>
{
  auto L = weightedLaplacianMatrix( K );
  auto n = L.rows();

  using Matrix    = decltype(L);
  using Simplex   = typename SimplicialComplex::ValueType;
  using DataType  = typename Simplex::DataType;

  Matrix M = Matrix::Constant( L.rows(), L.cols(), static_cast<DataType>( 1/ n ) );

  return (M+L).inverse() - M;
}

#endif

/**
  @class HeatKernel
  @brief Calculates the heat kernel for simplicial complexes

  This class acts as a query functor for the heat kernel values of
  vertices in a weighted simplicial complex. It will pre-calculate
  the heat matrix and permit queries about the progression of heat
  values for *all* vertices for some time \f$t\f$.
*/

class HeatKernel
{
public:

  using T = double;

#ifdef ALEPH_WITH_EIGEN
  using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  using Vector = Eigen::Matrix<T, 1, Eigen::Dynamic>;

#if EIGEN_VERSION_AT_LEAST(3,3,0)
  using IndexType  = Eigen::Index;
#else
  using IndexType  = typename Matrix::Index;
#endif

#else
  // This declares a fallback index type in case Eigen is not available,
  // making sure that the interface of the class remains the same.
  using IndexType = unsigned;
#endif

  /**
    Constructs a heat kernel from a given simplicial complex. Afterwards,
    the functor will be ready for queries.

    @param K Simplicial complex
  */

  template <class SimplicialComplex> HeatKernel( const SimplicialComplex& K )
  {
#ifdef ALEPH_WITH_EIGEN

    auto L = weightedLaplacianMatrix( K );

    Eigen::SelfAdjointEigenSolver< decltype(L) > solver;
    solver.compute( L );

    auto&& eigenvalues  = solver.eigenvalues(). template cast<T>();
    auto&& eigenvectors = solver.eigenvectors().template cast<T>();

    _eigenvalues.reserve( std::size_t( eigenvalues.size() ) );
    _eigenvectors.reserve( std::size_t( eigenvectors.size() ) );

    using IndexType_ = typename decltype(L)::Index;

    // If configured, skip both the first eigenvector and the first
    // eigenvalue because they do not contribute anything later on.
    for( IndexType_ i = _skip ? 1 : 0; i < eigenvalues.size(); i++ )
      _eigenvalues.push_back( eigenvalues(i) );

    for( IndexType_ i = _skip ? 1 : 0; i < eigenvectors.cols(); i++ )
      _eigenvectors.push_back( eigenvectors.col(i) );

#else
  (void) K;

  THROW_EIGEN_REQUIRED_ERROR();
#endif

  }

  /**
    Evaluates the heat kernel for *all* vertices at a given time \f$t\f$
    and returns the resulting values. This function is guaranteed to be
    more efficient than calling the per-element functions repeatedly.
  */

  std::vector<T> operator()( T t )
  {
#ifdef ALEPH_WITH_EIGEN

    Vector result = Vector();

    for( std::size_t k = 0; k < _eigenvalues.size(); k++ )
    {
      auto&& lk  = std::exp( -t * _eigenvalues[k] );
      auto&& uk = _eigenvectors[k];

      result += lk * uk * uk.transpose();
    }

    return std::vector<T>( result.data(), result.data() + result.size() );

#else
    (void) t;

    THROW_EIGEN_REQUIRED_ERROR();
#endif
  }

  /**
    Evaluates the heat kernel for two vertices \f$i\f$ and \f$j\f$ at
    a given time \f$t\f$ and returns the result.
  */

  T operator()( IndexType i, IndexType j, T t )
  {
#ifdef ALEPH_WITH_EIGEN

    aleph::math::KahanSummation<T> result = T();

    for( std::size_t k = 0; k < _eigenvalues.size(); k++ )
    {
      auto&& lk  = std::exp( -t * _eigenvalues[k] );
      auto&& uik = _eigenvectors[k](i);
      auto&& ujk = _eigenvectors[k](j);

      result += lk * uik * ujk;
    }

    return result;

#else
  (void) i;
  (void) j;
  (void) t;

  THROW_EIGEN_REQUIRED_ERROR();
#endif
  }

  /**
    Calculates the auto-diffusion for a given vertex \f$i\f$ and a given
    time \f$t\f$ and returns it.
  */

  T operator()( IndexType i, T t )
  {
#ifdef ALEPH_WITH_EIGEN

    // Note that this function could have been implemented in terms of
    // operator(i,j,t), but this implementation is a *little* bit more
    // efficient as it defines the multiplication explicitly.

    aleph::math::KahanSummation<T> result = T();

    for( std::size_t k = 0; k < _eigenvalues.size(); k++ )
    {
      auto&& lk  = std::exp( -t * _eigenvalues[k] );
      auto&& uik = _eigenvectors[k](i);

      result += lk * uik * uik;
    }

    return result;

#else
  (void) i;
  (void) t;

  THROW_EIGEN_REQUIRED_ERROR();
#endif
  }

  /**
    Calculates the *trace* of the heat kernel for a given time \f$t\f$
    and returns it.
  */

  T trace( T t ) const
  {
    aleph::math::KahanSummation<T> result = T();

    for( auto&& eigenvalue : _eigenvalues )
      result += std::exp( -t * eigenvalue );

    return result;
  }

  /**
    Calculates the *determinant* of the heat kernel for a given time
    \f$t\f$ and returns it.
  */

  T determinant( T t) const
  {
    T result = T();

    for( auto&& eigenvalue : _eigenvalues )
      result = result * std::exp( -t * eigenvalue );

    return result;
  }

  // Sampling intervals ------------------------------------------------

  /**
    Uses a heuristic to determine a sampling interval for the time
    parameter \f$t\f$ of the heat kernel. This heuristic was first
    described by Sun et al. in their paper *A Concise and Provably
    Informative Multi-Scale Signature Based on Heat Diffusion*.

    @param n Number of sampling points
    @returns Vector of sampling points
  */

  std::vector<T> logarithmicSamplingInterval( unsigned n ) const
  {
    auto t_min  = 4 * std::log( 10 ) / _eigenvalues.back();
    auto t_max  = 4 * std::log( 10 ) / ( _skip ? _eigenvalues.front() : *( _eigenvalues.begin() + 1 ) );
    auto offset = ( std::log( t_max ) - std::log( t_min ) ) / ( n - 1 );

    std::vector<T> samples;
    samples.reserve( n );

    for( unsigned i = 0; i < n; i++ )
      samples.push_back( std::log( t_min ) + i * offset );

    std::transform( samples.begin(), samples.end(),
                    samples.begin(),
                    [] ( const T x )
                    {
                      return std::pow( std::exp(1), x );
                    } );

    return samples;
  }

  // Configuration -----------------------------------------------------

  void setSkip( bool value = true ) { _skip = value; }
  bool skip() const noexcept        { return _skip;  }

private:

  /** If set, skips the first eigenvector and eigenvalue */
  bool _skip = false;

  /**
    Stores the eigenvalues of the heat matrix, or, more precisely, the
    eigenvalues of the Laplacian. They will be used for the evaluation
    of the heat kernel.
  */

  std::vector<T> _eigenvalues;

#ifdef ALEPH_WITH_EIGEN

  /** Stores the eigenvectors of the heat matrix */
  std::vector<Vector> _eigenvectors;

  /**
    Heat matrix; will be created automatically upon constructing this
    functor class.
  */

  Matrix _H;
#endif

};

} // namespace geometry

} // namespace aleph

#endif


#ifndef STAN_MATH_FWD_MAT_FUN_LOG_DETERMINANT_HPP
#define STAN_MATH_FWD_MAT_FUN_LOG_DETERMINANT_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/fwd/core.hpp>
#include <stan/math/fwd/mat/fun/typedefs.hpp>
#include <stan/math/fwd/mat/fun/determinant.hpp>
#include <stan/math/fwd/scal/fun/fabs.hpp>
#include <stan/math/fwd/scal/fun/log.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
namespace math {

template <typename T, int R, int C>
inline fvar<T> log_determinant(const Eigen::Matrix<fvar<T>, R, C>& m) {
  check_square("log_determinant", "m", m);

  return log(fabs(determinant(m)));
}

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


#include <boost/test/unit_test.hpp>

#include <crave/ConstrainedRandom.hpp>

#include <boost/format.hpp>

#include <set>
#include <iostream>

// using namespace std;
using namespace crave;

using boost::format;

BOOST_FIXTURE_TEST_SUITE(Distribution_t, Context_Fixture)

BOOST_AUTO_TEST_CASE(randv_dist_t1) {
  randv<int> v(NULL);
  Generator gen;
  gen(dist(v(), distribution<int>::create(range<int>(0, 5))(range<int>(50, 65))(range<int>(100, 125))));

  std::map<int, int> s;
  int total = 100000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(gen.next());
    BOOST_REQUIRE((0 <= v && v <= 5) || (50 <= v && v <= 65) || (100 <= v && v <= 125));
    ++s[v];
  }
  int min = s[0], max = s[0];
  for (int i = 1; i <= 200; i++)
    if (s.find(i) != s.end()) {
      if (s[i] < min) min = s[i];
      if (s[i] > max) max = s[i];
    }
  double avg = total / (6. + 16. + 26.);
  BOOST_REQUIRE_LT(100. * (avg - min) / avg, 5);
  BOOST_REQUIRE_LT(100. * (max - avg) / avg, 5);
}

BOOST_AUTO_TEST_CASE(randv_dist_t2) {
  randv<int> v(NULL);
  Generator gen;
  BOOST_CHECK_THROW(
      gen(dist(v(), distribution<int>::create(range<int>(0, 10))(range<int>(50, 75))(range<int>(30, 51)))),
      std::runtime_error);
}

BOOST_AUTO_TEST_CASE(randv_dist_t3) {
  randv<char> v(NULL);
  Generator gen;
  gen(dist(v(), distribution<char>::create(weighted_range<char>(1, 5, 50))(weighted_range<char>(10, 20, 20))(
                    weighted_range<char>(-50, -50, 30))));
  int cnt1 = 0, cnt2 = 0, cnt3 = 0;
  int total = 50000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(gen.next());
    BOOST_REQUIRE((1 <= v && v <= 5) || (10 <= v && v <= 20) || (v == -50));
    if (1 <= v && v <= 5) cnt1++;
    if (10 <= v && v <= 20) cnt2++;
    if (v == -50) cnt3++;
  }
  double q = 50.0 / cnt1;
  BOOST_REQUIRE_LT(abs(cnt2 * q - 20), 0.2);
  BOOST_REQUIRE_LT(abs(cnt3 * q - 30), 0.2);
}

BOOST_AUTO_TEST_CASE(randv_dist_t4) {
  randv<int> v(NULL);
  Generator gen;
  gen("x", dist(v(), distribution<int>::create(range<int>(0, 10))(range<int>(50, 75))(range<int>(100, 200))));
  gen("y", dist(v(), distribution<int>::simple_range(5000, 6000)));
  gen.disableConstraint("x");
  int total = 10000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(gen.next());
    BOOST_REQUIRE(5000 <= v && v <= 6000);
  }
}

BOOST_AUTO_TEST_SUITE_END()  // Context

//  vim: ft=cpp:ts=2:sw=2:expandtab