/*    Copyright (c) 2010-2019, 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.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <vector>
#include <limits>

#include <Eigen/Core>

#include <boost/test/unit_test.hpp>

#include "tudat/basics/testMacros.h"
#include "tudat/math/basic/mathematicalConstants.h"
#include "tudat/math/statistics/randomSampling.h"
#include "tudat/math/statistics/basicStatistics.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_Random_Sampler )

BOOST_AUTO_TEST_CASE( test_randomVectorUniform )
{
    int dimension = 3;
    int numberOfSamples = 1E6;
    int seed = 511;

    Eigen::VectorXd lower( dimension );
    Eigen::VectorXd upper( dimension );
    lower << 0.0, 1.0, -2.0;
    upper << 1.0, 3.0, 4.0;

    Eigen::VectorXd width = upper - lower;
    Eigen::VectorXd average = (upper + lower) / 2.0;

    Eigen::VectorXd standardDeviation = std::sqrt( 1.0 / 12.0 ) * width;
    {
        std::vector< Eigen::VectorXd > samples =
                tudat::statistics::generateUniformRandomSample( seed, numberOfSamples, lower, upper );

        // Compute sample mean and standard deviation
        Eigen::VectorXd sampleMean = statistics::computeSampleMean( samples );
        Eigen::ArrayXd sampleStandardDeviations = statistics::computeSampleVariance( samples ).array( ).sqrt( );

        BOOST_CHECK_SMALL( std::fabs( average( 0 ) - sampleMean( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( average( 1 ) - sampleMean( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( average( 2 ) - sampleMean( 2 ) ), 5.0E-3 );

        BOOST_CHECK_SMALL( std::fabs( standardDeviation( 0 ) - sampleStandardDeviations( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( standardDeviation( 1 ) - sampleStandardDeviations( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( standardDeviation( 2 ) - sampleStandardDeviations( 2 ) ), 5.0E-3 );
    }

    {
        std::vector< Eigen::VectorXd > samples = tudat::statistics::generateUniformRandomSample( seed, numberOfSamples, dimension );

        // Compute sample mean and standard deviation
        Eigen::VectorXd sampleMean = statistics::computeSampleMean( samples );
        Eigen::ArrayXd sampleStandardDeviations = statistics::computeSampleVariance( samples ).array( ).sqrt( );

        BOOST_CHECK_SMALL( std::fabs( 0.5 - sampleMean( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( 0.5 - sampleMean( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( 0.5 - sampleMean( 2 ) ), 5.0E-3 );

        BOOST_CHECK_SMALL( std::fabs( std::sqrt( 1.0 / 12.0 ) - sampleStandardDeviations( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( std::sqrt( 1.0 / 12.0 ) - sampleStandardDeviations( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( std::sqrt( 1.0 / 12.0 ) - sampleStandardDeviations( 2 ) ), 5.0E-3 );
    }
}

BOOST_AUTO_TEST_CASE( test_randomVectorGaussian )
{
    int dimension = 3;
    int numberOfSamples = 1E6;
    int seed = 511;

    Eigen::VectorXd mean( dimension );
    Eigen::VectorXd standardDeviation( dimension );
    mean << 0.0, 1.0, -2.0;
    standardDeviation << 1.0, 3.0, 4.0;

    {
        std::vector< Eigen::VectorXd > samples =
                tudat::statistics::generateGaussianRandomSample( seed, numberOfSamples, mean, standardDeviation );

        // Compute sample mean and standard deviation
        Eigen::VectorXd sampleMean = statistics::computeSampleMean( samples );
        Eigen::ArrayXd sampleStandardDeviations = statistics::computeSampleVariance( samples ).array( ).sqrt( );

        BOOST_CHECK_SMALL( std::fabs( mean( 0 ) - sampleMean( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( mean( 1 ) - sampleMean( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( mean( 2 ) - sampleMean( 2 ) ), 5.0E-3 );

        BOOST_CHECK_SMALL( std::fabs( standardDeviation( 0 ) - sampleStandardDeviations( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( standardDeviation( 1 ) - sampleStandardDeviations( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( standardDeviation( 2 ) - sampleStandardDeviations( 2 ) ), 5.0E-3 );
    }

    {
        std::vector< Eigen::VectorXd > samples =
                tudat::statistics::generateGaussianRandomSample( seed, numberOfSamples, 3 );

        // Compute sample mean and standard deviation
        Eigen::VectorXd sampleMean = statistics::computeSampleMean( samples );
        Eigen::ArrayXd sampleStandardDeviations = statistics::computeSampleVariance( samples ).array( ).sqrt( );

        BOOST_CHECK_SMALL( std::fabs( 0.0 - sampleMean( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( 0.0 - sampleMean( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( 0.0 - sampleMean( 2 ) ), 5.0E-3 );

        BOOST_CHECK_SMALL( std::fabs( 1.0 - sampleStandardDeviations( 0 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( 1.0 - sampleStandardDeviations( 1 ) ), 5.0E-3 );
        BOOST_CHECK_SMALL( std::fabs( 1.0 - sampleStandardDeviations( 2 ) ), 5.0E-3 );
    }
}

#if USE_GSL

//! Test if Sobol sampler interface is working correctly. Note that this test is somewhat minimal, but the core of the
//! Sobol sampling is tested in the GSL unit tests.
BOOST_AUTO_TEST_CASE( test_Sobol_Sampler )
{
    // Define settings and generate samples.
    int dimension = 3;
    int numberOfSamples = 1E6;

    Eigen::VectorXd lower( dimension );
    Eigen::VectorXd upper( dimension );
    lower << 0.0, 1.0, -2.0;
    upper << 1.0, 3.0, 4.0;

    Eigen::VectorXd width = upper - lower;
    Eigen::VectorXd average = (upper + lower) / 2.0;

    std::vector< Eigen::VectorXd > sobolSamples = tudat::statistics::generateVectorSobolSample(
                numberOfSamples, lower, upper );

    // Compute sample average
    Eigen::VectorXd sampleMean = statistics::computeSampleMean( sobolSamples );

    BOOST_CHECK_SMALL( std::fabs( average( 0 ) - sampleMean( 0 ) ), 2.0E-6 );
    BOOST_CHECK_SMALL( std::fabs( average( 1 ) - sampleMean( 1 ) ), 2.0E-6 );
    BOOST_CHECK_SMALL( std::fabs( average( 2 ) - sampleMean( 2 ) ), 2.0E-6 );

    sobolSamples = tudat::statistics::generateVectorSobolSample( dimension, numberOfSamples );

    // Compute sample average
    sampleMean = statistics::computeSampleMean( sobolSamples );

    BOOST_CHECK_SMALL( std::fabs( 0.5 - sampleMean( 0 ) ), 2.0E-6 );
    BOOST_CHECK_SMALL( std::fabs( 0.5 - sampleMean( 1 ) ), 2.0E-6 );
    BOOST_CHECK_SMALL( std::fabs( 0.5 - sampleMean( 2 ) ), 2.0E-6 );
}
#endif

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Siargey Kachanovich
 *
 *    Copyright (C) 2019 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "permutahedral_representation"
#include <boost/test/unit_test.hpp>

#include <gudhi/Permutahedral_representation.h>

BOOST_AUTO_TEST_CASE(permutahedral_representation) {
  typedef std::vector<int> Vertex;
  typedef std::vector<std::size_t> Part;
  typedef std::vector<Part> Partition;
  typedef Gudhi::coxeter_triangulation::Permutahedral_representation<Vertex, Partition> Simplex_handle;
  Vertex v0(10, 0);
  Partition omega = {Part({5}), Part({2}), Part({3, 7}), Part({4, 9}), Part({0, 6, 8}), Part({1, 10})};
  Simplex_handle s(v0, omega);

  // Dimension check
  BOOST_CHECK(s.dimension() == 5);

  // Vertex number check
  std::vector<Vertex> vertices;
  for (auto& v : s.vertex_range()) vertices.push_back(v);
  BOOST_CHECK(vertices.size() == 6);

  // Facet number check
  std::vector<Simplex_handle> facets;
  for (auto& f : s.facet_range()) facets.push_back(f);
  BOOST_CHECK(facets.size() == 6);

  // Face of dim 3 number check
  std::vector<Simplex_handle> faces3;
  for (auto& f : s.face_range(3)) faces3.push_back(f);
  BOOST_CHECK(faces3.size() == 15);

  // Cofacet number check
  std::vector<Simplex_handle> cofacets;
  for (auto& f : s.cofacet_range()) cofacets.push_back(f);
  BOOST_CHECK(cofacets.size() == 12);

  // Is face check
  Vertex v1(10, 0);
  Partition omega1 = {Part({5}), Part({0, 1, 2, 3, 4, 6, 7, 8, 9, 10})};
  Simplex_handle s1(v1, omega1);
  Vertex v2(10, 0);
  v2[1] = -1;
  Partition omega2 = {Part({1}), Part({5}), Part({2}), Part({3, 7}), Part({4, 9}), Part({0, 6, 8}), Part({10})};
  Simplex_handle s2(v2, omega2);
  BOOST_CHECK(s.is_face_of(s));
  BOOST_CHECK(s1.is_face_of(s));
  BOOST_CHECK(!s2.is_face_of(s));
  BOOST_CHECK(s.is_face_of(s2));
}


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

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[unique
//` Shows how to make a so-called minimal set of a polygon by removing duplicate points

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

int main()
{
    boost::geometry::model::polygon<boost::tuple<double, double> > poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 0,0 5,5 5,5 5,5 5,5 0,5 0,0 0,0 0,0 0,0 0))", poly);
    boost::geometry::unique(poly);
    std::cout << boost::geometry::wkt(poly) << std::endl;

    return 0;
}

//]

//[unique_output
/*`
Output:
[pre
POLYGON((0 0,0 5,5 5,5 0,0 0))
]
*/
//]


/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/

// An implementation of the EPnP algorithm (for >=6 points) based on the following publication:
// V. Lepetit, F. Moreno-Noguer, P. Fua, EPnP: An Accurate O(n) Solution to the PnP Problem,
// International Journal Of Computer Vision (IJCV), 2009
// See manuscript at http://infoscience.epfl.ch/record/160138/files/top.pdf

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <cmath>

#include <iostream>
#include <random>
#include "packages/pnp/gems/epnp/epnp.hpp"
#include "packages/pnp/gems/tests/simu.hpp"

// Global number of repetitions for some randomized tests.
constexpr int kRepeatTests = 1000;

// Test the output size of ChooseBasis() with 0,1,2,3 and 4 input points in general configuration.
TEST(EpnpTest, ChooseBasisNotEnoughPointsTest) {
  // Tolerance for collapsing dimensions inside ChooseBasis().
  const double tol = 1e-4;

  // Test ChooseBasis() with empty input.
  isaac::Matrix3Xd points;
  isaac::Matrix3Xd ctl_points = isaac::pnp::epnp::ChooseBasis(points, tol);
  EXPECT_EQ(ctl_points.cols(), 0);

  // Fixed set of 4 non-planar points for testing.
  points.resize(3, 4);
  points << 10, -10, 10, 1.5, 8, -3, 0, 100, 20, -30, 8, 2;

  // Test the output size of ChooseBasis() for the first N = 1, 2, 3, 4 points.
  for (int num_points = 1; num_points < points.cols(); num_points++) {
    ctl_points = isaac::pnp::epnp::ChooseBasis(points.block(0, 0, 3, num_points), tol);

    // Output size equals the number of input points up to 4 points in general configuration.
    // This only holds when points do not coincide and are not all collinear or coplanar.
    EXPECT_EQ(ctl_points.cols(), num_points);
  }
}

// Test ChooseBasis() with perfectly collinear input point set.
// It should only return two points along the line that define the basis (origin and axis end-point)
// to represent the original points.
TEST(EpnpTest, ChooseBasisCollinearTest) {
  const int num_points = 100;  // Number of input points to generate and feed to ChooseBasis().
  const double tol = 1e-2;     // Tolerance for collapsing dimensions inside ChooseBasis().

  // Test for many different random lines in space.
  for (int i = 0; i < kRepeatTests; i++) {
    // Generate a random line in space via a random direction and a random pivot point on the line.
    isaac::Vector3d line_dir = isaac::pnp::RandomVector<isaac::Vector3d>(3);
    line_dir.normalize();
    isaac::Vector3d line_pivot = isaac::pnp::RandomVector<isaac::Vector3d>(3, -100, 100);

    // Generate input points along the 3D line.
    isaac::VectorXd line_param = isaac::pnp::RandomVector<isaac::VectorXd>(num_points, -100, 100);
    isaac::Matrix3Xd points(3, num_points);
    for (int i = 0; i < num_points; i++) {
      points.col(i) = line_param(i) * line_dir + line_pivot;
    }

    // Feed the collinear points to ChooseBasis().
    isaac::Matrix3Xd ctl_points = isaac::pnp::epnp::ChooseBasis(points, tol);

    // ChooseBasis() should return 2 control points: origin and a single axis end-point on the line.
    EXPECT_EQ(ctl_points.cols(), 2);

    // Returned axis direction should be parallel to the line.
    EXPECT_LT((line_dir.cross(ctl_points.col(1) - ctl_points.col(0))).norm(), 1e-9);
  }
}

// Test if a 3D basis returned by ChooseBasis() and represented as 4 control points
// is aligned with a Gaussian distribution.
// The basis is computed from statistical samples and therefore is never perfectly aligned
// with the generator distribution. If sufficiently many samples were used, the basis will be
// be approximately aligned unless there are computation errors. There is no guarantee
// that the error is within a certain threshold. Loose thresholds are used here.
void CheckControlPoints(const isaac::Matrix3Xd& ctl_points, const isaac::pnp::Gaussian3& gaussian) {
  // This test is meant for the planar and non-planar case, not for other degenerate inputs.
  int dims = int(ctl_points.cols() - 1);
  ASSERT_TRUE(dims == 2 || dims == 3);

  // The first control point (the basis origin) should be an estimate for the mean.
  EXPECT_LT((ctl_points.col(0) - gaussian.mean()).norm(), 0.3);

  // Calculate vectors and distances between control points.
  isaac::Matrix3Xd basis_axes(3, dims);
  isaac::VectorXd axis_lengths(dims);
  for (int i = 0; i < dims; i++) {
    isaac::Vector3d axis = ctl_points.col(i + 1) - ctl_points.col(0);
    double axis_length = axis.norm();
    if (axis_length) {
      basis_axes.col(i) = axis / axis_length;
    } else {
      basis_axes.col(i) = axis;
    }
    axis_lengths(i) = axis_length;
  }

  // Test mutual orthonormality of basis axes defined by the calculated control points.
  EXPECT_LT((basis_axes.transpose() * basis_axes - isaac::MatrixXd::Identity(dims, dims)).norm(),
            1e-4);

  // Normalized axis directions of the 3D Gaussian are in the columns of the orientation matrix.
  // Test their mutual orthonormality.
  const isaac::Matrix3Xd& gaussian_axes = gaussian.orientation();
  EXPECT_LT((gaussian_axes.transpose() * gaussian_axes - isaac::Matrix3d::Identity()).norm(), 1e-4);

  // Calculate the pairwise similarity (scalar product, cosine of angle) between each basis axis
  // direction and each axis of the Gaussian distribution.
  // The i-th row is the distance between the i-th basis axis and each reference Gaussian axis.
  isaac::MatrixXd similarity_matrix = (basis_axes.transpose() * gaussian_axes).cwiseAbs();

  // Find the closest Gaussian axis to each basis axis.
  Eigen::VectorXi axis_ids = Eigen::VectorXi::Zero(dims);
  isaac::VectorXd cos_angles(dims);
  for (int i = 0; i < dims; i++) cos_angles(i) = similarity_matrix.row(i).maxCoeff(&axis_ids(i));

  // Test if basis axes (which are vectors between control points) are close to parallel
  // to the corresponding Gaussian axis. Use high threshold because of comparing a distribution
  // parameter to its statistical estimate.
  EXPECT_LT((cos_angles - isaac::VectorXd::Ones(dims)).norm(), 0.1);

  // Test if distances between control points are equal to the deviation along the corresponding
  // Gaussian axis.
  isaac::VectorXd deviations(dims);
  for (int i = 0; i < dims; i++) {
    deviations(i) = gaussian.deviations()(axis_ids(i));
  }
  EXPECT_LT((deviations - axis_lengths).norm(), 0.2);
}

// Test ChooseBasis() with a random planar point distribution.
// Points are drawn from a 3D Gaussian distribution flattened along one major axis.
TEST(EpnpTest, ChooseBasisPlanarTest) {
  // Draw 3D points from a 3D Gaussian distribution that is perfectly flat along one of its
  // major axes - equivalent to 2D Gaussian samples on a plane.
  isaac::pnp::Gaussian3 gaussian;
  gaussian.setOrientation(isaac::pnp::RandomVector<isaac::Vector3d>(3));
  gaussian.setMean(isaac::Vector3d(50, -200, 25.89));
  gaussian.setDeviations(isaac::Vector3d(0, 2.3, 1.7));  // squash the Gaussian along one dimension
  isaac::Matrix3Xd points = gaussian.generate(1000);

  // Feed the planar point configuration to ChooseBasis()
  isaac::Matrix3Xd ctl_points = isaac::pnp::epnp::ChooseBasis(points, 0.01);

  // Verify that only 3 control points are returned: origin and two direction vectors in the plane.
  ASSERT_EQ(ctl_points.cols(), 3);

  // Make sure the the basis defined by the control points is actually aligned with the planar
  // Gaussian blob used to generate the input points.
  CheckControlPoints(ctl_points, gaussian);
}

// Test ChooseBasis() with a non-planar point set drawn from a 3D Gaussian distribution.
TEST(EpnpTest, ChooseBasisNonPlanarTest) {
  // Draw 3D points from a generic 3D Gaussian distribution.
  isaac::pnp::Gaussian3 gaussian;
  gaussian.setOrientation(isaac::pnp::RandomVector<isaac::Vector3d>(3));
  gaussian.setMean(isaac::Vector3d(50, -200, 25.89));
  gaussian.setDeviations(isaac::Vector3d(0.3, 1.2, 1.5));
  isaac::Matrix3Xd points = gaussian.generate(1000);

  // Feed the point configuration to ChooseBasis()
  isaac::Matrix3Xd ctl_points = isaac::pnp::epnp::ChooseBasis(points, 0.001);

  // Verify that 4 control points are returned: origin and three axes.
  // Test if the axis lengths are properly aligned with the Gaussian blob.
  ASSERT_EQ(ctl_points.cols(), 4);
  CheckControlPoints(ctl_points, gaussian);
}

// Test ComputeBaryCoords() on 3D samples drawn from a Gaussian distribution.
TEST(EpnpTest, BaryCoordsNonPlanarTest) {
  // Run the test for different 3D sample sets drawn from the same 3D Gaussian.
  for (int i = 0; i < kRepeatTests; i++) {
    // Draw many points from a fixed non-isotropic and slanted 3D Gaussian distribution.
    isaac::pnp::Gaussian3 gaussian;
    gaussian.setOrientation(isaac::pnp::RandomVector<isaac::Vector3d>(3));
    gaussian.setMean(isaac::Vector3d(50, -200, 25.89));
    gaussian.setDeviations(isaac::Vector3d(0.3, 1.2, 1.5));
    isaac::Matrix3Xd points = gaussian.generate(100);

    // Compute 3D basis represented by 4 control points (non-planar case).
    isaac::Matrix3Xd ctl_points = isaac::pnp::epnp::ChooseBasis(points, 1e-3);
    ASSERT_EQ(ctl_points.cols(), 4);

    // Describe all input points by their barycentric coordinates in this basis.
    isaac::MatrixXd bary_coeffs = isaac::pnp::epnp::ComputeBaryCoords(points, ctl_points);

    // Given N input points, the output is a 4xN matrix of barycentric coefficients.
    ASSERT_EQ(bary_coeffs.rows(), 4);
    ASSERT_EQ(bary_coeffs.cols(), points.cols());

    // Test if barycentric coordinates are correct.
    // This also checks if barycentric coeffs sum to 1.0 for each point.
    isaac::Matrix4Xd hom_basis = isaac::pnp::HomogeneousFromEuclidean(ctl_points);
    isaac::Matrix4Xd hom_points = isaac::pnp::HomogeneousFromEuclidean(points);
    ASSERT_EQ(hom_basis.rows(), ctl_points.rows() + 1);
    ASSERT_EQ(hom_basis.cols(), ctl_points.cols());
    ASSERT_EQ(hom_points.rows(), points.rows() + 1);
    ASSERT_EQ(hom_points.cols(), points.cols());
    double max_residual = (hom_basis * bary_coeffs - hom_points).cwiseAbs().maxCoeff();
    EXPECT_LT(max_residual, 1e-9);
  }
}

// Test ComputeBaryCoords() for points in a plane.
// Points are drawn from a 3D Gaussian distribution flattened along one major axis.
TEST(EpnpTest, BaryCoordsPlanarTest) {
  for (int i = 0; i < kRepeatTests; i++) {
    // Draw many points from a fixed non-isotropic and slanted 3D Gaussian distribution.
    isaac::pnp::Gaussian3 gaussian;
    gaussian.setOrientation(isaac::pnp::RandomVector<isaac::Vector3d>(3));
    gaussian.setMean(isaac::Vector3d(50, -200, 25.89));
    gaussian.setDeviations(isaac::Vector3d(0, 1.2, 1.5));
    isaac::Matrix3Xd points = gaussian.generate(100);

    // Compute planar basis represented by only 3 control points.
    isaac::Matrix3Xd ctl_points = isaac::pnp::epnp::ChooseBasis(points, 1e-2);
    ASSERT_EQ(ctl_points.cols(), 3);

    // Describe all input points by their barycentric coordinates in this basis.
    isaac::MatrixXd bary_coeffs = isaac::pnp::epnp::ComputeBaryCoords(points, ctl_points);
    ASSERT_EQ(bary_coeffs.rows(), 3);
    ASSERT_EQ(bary_coeffs.cols(), points.cols());

    // Test if barycentric coordinates are correct.
    // This also checks if barycentric coeffs sum to 1.0 for each point.
    isaac::Matrix4Xd hom_basis = isaac::pnp::HomogeneousFromEuclidean(ctl_points);
    isaac::Matrix4Xd hom_points = isaac::pnp::HomogeneousFromEuclidean(points);
    double max_residual = (hom_basis * bary_coeffs - hom_points).cwiseAbs().maxCoeff();
    EXPECT_LT(max_residual, 1e-9);
  }
}

// Test SolveControlPoints() for the non-planar case.
// The solution is sought in the form x = B*w, where x is a solution vector of 12 elements
// (camera coordinates of 4 control points as a vector) that satisfy certain distance criteria.
// B is a known 12xD matrix of D basis vectors, and w is the vector of unknown weights.
// The test works as follows:
//  (1) Simulate a random (non-orthogonal) D-dimensional basis
//      as a random 12xD matrix B with normalized columns.
//  (2) Generate a ground-truth vector w randomly.
//  (3) Synthesize the ground-truth solution x as B*w.
//  (4) Calculate the true distances between parts of x.
//  (5) Run SolveControlPoints() given B and the distances to calculate the weights w.
//  (6) Compare the calculated weights to the known weights generated in step (2).
// The parameter dims is the number of dimensions of the solution space 1 <= D <= 4 to test with.
void ControlPointSolverNonPlanarTest(int dims) {
  // Make sure dims is between 1 and 4
  if (dims < 1) {
    dims = 1;
  }
  if (dims > 4) {
    dims = 4;
  }

  // Repeat the process (1-6) many times to test with different random bases and weights.
  for (int i = 0; i < kRepeatTests; i++) {
    // (1) Generate 4 random normalized basis vectors in a 12-D space.
    isaac::MatrixXd sol_basis = isaac::pnp::RandomUniformMatrix<isaac::MatrixXd>(12, 4, -1, 1);
    sol_basis.colwise().normalize();
    for (int i = 0; i < sol_basis.cols(); i++) {
      EXPECT_NEAR(sol_basis.col(i).norm(), 1.0, 1e-6);
    }

    // (2) Generate the true weights which are to be found by SolveControlPoints().
    isaac::Vector4d true_weights = isaac::pnp::RandomVector<isaac::Vector4d>(4, -100.0f, 100.0f);
    for (int i = dims; i < 4; i++) {
      true_weights(i) = 0;
    }

    // (3) Synthesize the ground truth solution as the linear combination of the basis vectors.
    // The solution is represented as a 3x4 matrix instead of a vector of 12 elements.
    isaac::Matrix3Xd true_ctl_points =
        isaac::pnp::epnp::ReshapeToMatrix3xN(sol_basis * true_weights);

    // (4) Calculate the true pairwise distances between control points.
    isaac::VectorXd distances = isaac::pnp::epnp::ComputeDistances(true_ctl_points);

    // SolveControlPoints(): Compute the weights given the solution basis and the distances.
    isaac::Vector4d weights = isaac::pnp::epnp::SolveControlPoints(sol_basis, dims, distances);

    // Make sure that weights are not all zeros.
    ASSERT_GT(weights.squaredNorm(), 0);

    // Make sure that the computed weights match their known values up to a single global sign,
    // because the simulation above does not guarantee that all z-coordinates are positive
    // for the control points (last row of true_ctl_points).
    double err = std::min((true_weights - weights).cwiseAbs().maxCoeff(),
                          (true_weights + weights).cwiseAbs().maxCoeff());
    ASSERT_LT(err, 1e-9);
  }
}

// Test SolveControlPoints() for non-planar input and assuming a 1-dimensional a solution space.
TEST(EpnpTest, ControlPointSolver1_NonPlanar) {
  SCOPED_TRACE("1 dimensional");
  ControlPointSolverNonPlanarTest(1);
}

// Test SolveControlPoints() for non-planar input and assuming a 2-dimensional a solution space.
TEST(EpnpTest, ControlPointSolver2_NonPlanar) {
  SCOPED_TRACE("2 dimensional");
  ControlPointSolverNonPlanarTest(2);
}

// Test SolveControlPoints() for non-planar input and assuming a 3-dimensional a solution space.
TEST(EpnpTest, ControlPointSolver3_NonPlanar) {
  SCOPED_TRACE("3 dimensional");
  ControlPointSolverNonPlanarTest(3);
}

// Test to make sure that each stage of the EPnP pipeline works as expected in the noise-free case.
// Check the output pose and internal calculation results of epnp::ComputeCameraPose(),
// given the perfect (noise-free) input points and camera passed to epnp::ComputeCameraPose().
//   result     Output provided by epnp::ComputeCameraPose().
//   camera     Ideal camera passed to epnp::ComputeCameraPose(). Contains the ground-truth pose.
//   points2,3  Ideal (noise-free) input 2D and 3D points passed to epnp::ComputeCameraPose().
void CheckEpnpResult(const isaac::pnp::epnp::Result& result, const isaac::pnp::Camera& camera,
                     const isaac::Matrix3Xd& points3, const isaac::Matrix2Xd& points2) {
  ASSERT_EQ(points3.cols(), points2.cols());

  // Input point cloud should be either planar (2D arrangement) or non-planar (3D arrangement).
  // This should be reflected in result.input_dims and the number of control points calculated.
  ASSERT_GE(result.input_dims, 2);
  ASSERT_LE(result.input_dims, 3);
  ASSERT_EQ(result.ctl_points_world.cols(), result.input_dims + 1);

  // Test barycentric coordinates of the input points in the calculated basis
  // defined by the control points.
  ASSERT_EQ(result.bary_coeffs.rows(), result.input_dims + 1);
  ASSERT_EQ(result.bary_coeffs.cols(), points3.cols());
  EXPECT_LT((isaac::pnp::HomogeneousFromEuclidean(result.ctl_points_world) * result.bary_coeffs -
             isaac::pnp::HomogeneousFromEuclidean(points3))
                .cwiseAbs()
                .maxCoeff(),
            1e-9);

  // Check the size of the coefficient matrix of the projection equations.
  // Check the size of the solution basis of the projection equation and the number of corresponding
  // singular values. All should be consistent with the number of dimensions.
  ASSERT_EQ(result.proj_coeffs.rows(), 2 * points3.cols());
  ASSERT_EQ(result.proj_coeffs.cols(), 3 * (result.input_dims + 1));
  ASSERT_EQ(result.solution_basis.rows(), 3 * (result.input_dims + 1));
  ASSERT_EQ(result.singular_values.size(), 3 * (result.input_dims + 1));
  ASSERT_EQ(result.solution_basis.cols(), 4);

  // Assuming no noise in the input points, the homogeneous projection equations should have an
  // infinite number of perfect solutions: all solutions parallel to the first basis vector of the
  // hypothesized solution space all saisfy the original equations.
  // Thus, the solution space is at least 1-dimensional in the noise-free case.
  // In other terms, the least singular value should be zero.
  const double tol = 1e-9;
  EXPECT_LT(result.singular_values(0), tol);

  // The 4 hypothesized basis vectors of the solution subspace are all singular vectors
  // of the coefficient matrix of the projection equations (linear homogeneous equations).
  // Theory says that the norm of the algebraic residual when substituting each singular vector
  // into the original homogeneous equation equals the corresponding singular value, so test this.
  // This also holds in the noisy case!
  EXPECT_NEAR((result.proj_coeffs * result.solution_basis.col(0)).norm(), result.singular_values(0),
              tol);
  EXPECT_NEAR((result.proj_coeffs * result.solution_basis.col(1)).norm(), result.singular_values(1),
              tol);
  EXPECT_NEAR((result.proj_coeffs * result.solution_basis.col(2)).norm(), result.singular_values(2),
              tol);
  EXPECT_NEAR((result.proj_coeffs * result.solution_basis.col(3)).norm(), result.singular_values(3),
              tol);

  // Since the algebraic error of the first basis vector (and its scalar multiples) is always zero,
  // the geometric (reprojection) error of the corresponding control points should also be zero.
  // Calculate reprojection errors are all zero up to numerical precision.
  // This test assumes ideal (noise-free) input.
  isaac::Matrix3Xd solution = isaac::pnp::epnp::ReshapeToMatrix3xN(result.solution_basis.col(0));
  isaac::VectorXd repr_errors =
      isaac::pnp::ColwiseNorms(points2 - isaac::pnp::epnp::ProjectPoints(
                                             camera.calib_matrix, solution * result.bary_coeffs));
  EXPECT_LT(repr_errors.maxCoeff(), 1e-9);

  // Calculate the ground-truth control points in the camera frame by transforming the computed
  // control points in the world frame with the ground-truth camera pose.
  isaac::Matrix3Xd true_ctl_points_cam =
      camera.rotation_matrix * (result.ctl_points_world.colwise() - camera.position);

  // Compare the computed control points to ground-truth in the camera frame.
  // They should match up to numerical precision because the input was noise-free.
  EXPECT_LT((result.ctl_points_cam - true_ctl_points_cam).norm(), 1e-9);

  // Test if the rotation matrix computed by EPnP is orthonormal.
  EXPECT_LT((result.rotation.transpose() * result.rotation - isaac::Matrix3d::Identity()).norm(),
            1e-9);

  // The pose is computed as the rigid transformation that takes the computed control points
  // in the world frame into the computed control points in the camera frame.
  // As the input was noise-free, the residual of this transformation should be zero.
  EXPECT_LT(((result.rotation * result.ctl_points_world - result.ctl_points_cam).colwise() +
             result.translation)
                .norm(),
            1e-9);

  // Compare the calculated camera orientation to the ground-truth.
  double rot_error = isaac::RadToDeg(
      isaac::pnp::AngleAxisFromMatrix(camera.rotation_matrix.transpose() * result.rotation).norm());

  // Compare the calculated camera position to the ground-truth.
  isaac::Vector3d camera_position = -result.rotation.transpose() * result.translation;
  double tran_error = (camera_position - camera.position).norm();

  // Alternative: directly compare the translation vectors instead of the camera positions.
  // isaac::Vector3d true_translation = - camera.rotation_matrix * camera.position;
  // double tran_error = (result.translation - true_translation).norm();

  // As the input was noise-free, the rotation and translation errors should be zero.
  EXPECT_LT(rot_error, 1e-9);   // degrees
  EXPECT_LT(tran_error, 1e-9);  // meters (position range in huge: +/-100 meters)
}

// Test integrated EPnP pose estimation pipeline in case of ideal non-planar input arrangement.
// Repeats the following randomized test procedure many times:
//  (1) Generate a camera with random pose but fixed intrinsics.
//  (2) Generate sufficient number of ideal (noise-free) random 2D-3D matches such that
//      the 3D points are in front of the camera between the near and far plane.
//  (3) Camera pose estimation from the synthetic input using EPnP.
//  (4) Test if computed pose equals ground-truth and if the intermediate results are consistent.
TEST(EpnpTest, FullNonPlanarTest) {
  // Camera intrinsic parameters.
  const int width = 1280;
  const int height = 720;
  const double focal = 700.0;

  // Repeat the pose estimation for different random inputs and random cameras (all planar).
  for (int i = 0; i < kRepeatTests; i++) {
    // (1) Generate a camera with random pose but fixed intrinsics.
    isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);
    const double focal_u = camera.calib_matrix(0, 0);
    const double focal_v = camera.calib_matrix(1, 1);
    const double principal_u = camera.calib_matrix(0, 2);
    const double principal_v = camera.calib_matrix(1, 2);

    // (2) Generate sufficient number of ideal (noise-free) random 2D-3D matches such that
    //     the 3D points are in front of the camera between the near and far plane.
    const double near = 5.0;
    const double far = 10.0;
    const unsigned num_points = 6;
    isaac::Matrix3Xd points3;
    isaac::Matrix2Xd points2;
    isaac::pnp::GenerateFovPoints(num_points, camera, near, far, &points3, &points2);

    // (3) Camera pose estimation from the synthetic noise-free input using EPnP.
    // EPnP should always succeed for such input.
    isaac::pnp::epnp::Result result;
    ASSERT_EQ(isaac::pnp::epnp::ComputeCameraPose(focal_u, focal_v, principal_u, principal_v,
                                                  points3, points2, &result),
              isaac::pnp::Status::kSuccess);

    // (4) Test if computed pose equals ground-truth and if the intermediate results are consistent.
    SCOPED_TRACE("non-planar");
    CheckEpnpResult(result, camera, points3, points2);
  }
}

// Test integrated EPnP pose estimation pipeline in case of ideal input on a fronto-parallel plane.
// Repeats the following randomized test procedure many times:
//  (1) Generate a camera with random pose but fixed intrinsics.
//  (2) Generate sufficient number of ideal (noise-free) random 2D-3D matches such that
//      the 3D points are on a FRONTO-PARALLEL PLANE in front of the camera.
//  (3) Camera pose estimation from the synthetic input using EPnP.
//  (4) Test if computed pose equals ground-truth and if the intermediate results are consistent.
TEST(EpnpTest, FullFrontoPlanarTest) {
  // Camera intrinsic parameters.
  const int width = 1280;
  const int height = 720;
  const double focal = 700;

  // Repeat the pose estimation for different random inputs and random cameras (all planar).
  for (int i = 0; i < kRepeatTests; i++) {
    // (1) Generate a camera with random pose but fixed intrinsics.
    isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);
    const double focal_u = camera.calib_matrix(0, 0);
    const double focal_v = camera.calib_matrix(1, 1);
    const double principal_u = camera.calib_matrix(0, 2);
    const double principal_v = camera.calib_matrix(1, 2);

    // (2) Generate sufficient number of ideal (noise-free) random 2D-3D matches such that
    //     the 3D points are on a fronto-parallel plane in front of the camera.
    const double depth = 8.0;
    const double angle = 0.0;
    const unsigned num_points = 6;
    isaac::Matrix3Xd points3;
    isaac::Matrix2Xd points2;
    isaac::Vector4d plane;
    isaac::pnp::GenerateFovPointsPlanar(num_points, camera, depth, angle, 0, &points3, &points2,
                                        &plane);

    // (3) Camera pose estimation from the synthetic noise-free input using EPnP.
    // EPnP should always succeed for such input.
    isaac::pnp::epnp::Result result;
    ASSERT_EQ(isaac::pnp::epnp::ComputeCameraPose(focal_u, focal_v, principal_u, principal_v,
                                                  points3, points2, &result),
              isaac::pnp::Status::kSuccess);

    // (4) Test if computed pose equals ground-truth and if the intermediate results are consistent.
    SCOPED_TRACE("fronto-parallel planar");
    CheckEpnpResult(result, camera, points3, points2);
  }
}

// Test integrated EPnP pose estimation pipeline in case of ideal input on a fronto-parallel plane.
// Repeats the following randomized test procedure many times:
//  (1) Generate a camera with random pose but fixed intrinsics.
//  (2) Generate sufficient number of ideal (noise-free) random 2D-3D matches such that
//      the 3D points are on a SLANTED PLANE in front of the camera.
//  (3) Camera pose estimation from the synthetic input using EPnP.
//  (4) Test if computed pose equals ground-truth and if the intermediate results are consistent.
TEST(EpnpTest, FullSlantedPlanarTest) {
  // Camera intrinsic parameters.
  const int width = 1280;
  const int height = 720;
  const double focal = 700.0;

  // Repeat the pose estimation for different random inputs and random cameras (all planar).
  for (int i = 0; i < kRepeatTests; i++) {
    // (1) Generate a camera with random pose but fixed intrinsics.
    isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);
    const double focal_u = camera.calib_matrix(0, 0);
    const double focal_v = camera.calib_matrix(1, 1);
    const double principal_u = camera.calib_matrix(0, 2);
    const double principal_v = camera.calib_matrix(1, 2);

    // (2) Generate sufficient number of ideal (noise-free) random 2D-3D matches such that
    //     the 3D points are on a SLANTED PLANE in front of the camera.
    const double depth = 10.0;
    const double angle = 80.0;
    const unsigned num_points = 6;
    isaac::Matrix3Xd points3;
    isaac::Matrix2Xd points2;
    isaac::Vector4d plane;
    isaac::pnp::GenerateFovPointsPlanar(num_points, camera, depth, angle, 0, &points3, &points2,
                                        &plane);

    // (3) Camera pose estimation from the synthetic noise-free input using EPnP.
    // EPnP should always succeed for such input.
    isaac::pnp::epnp::Result result;
    ASSERT_EQ(isaac::pnp::epnp::ComputeCameraPose(focal_u, focal_v, principal_u, principal_v,
                                                  points3, points2, &result),
              isaac::pnp::Status::kSuccess);

    // (4) Test if computed pose equals ground-truth and if the intermediate results are consistent.
    SCOPED_TRACE("slanted planar");
    CheckEpnpResult(result, camera, points3, points2);
  }
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


//=======================================================================
// Copyright 2008 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/property_map/property_map.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>

#include <vector>

using namespace boost;

struct coord_t
{
  std::size_t x;
  std::size_t y;
};

int test_main(int, char*[])
{
  typedef adjacency_list< vecS, vecS, undirectedS,
                          property<vertex_index_t, int>
                         > graph_t;

  typedef std::vector< coord_t > drawing_storage_t;

  typedef boost::iterator_property_map
      < drawing_storage_t::iterator,
        property_map<graph_t, vertex_index_t>::type
       > drawing_t;

  graph_t g(4);
  add_edge(0,1,g);
  add_edge(2,3,g);

  drawing_storage_t drawing_storage(num_vertices(g));
  drawing_t drawing(drawing_storage.begin(), get(vertex_index,g));

  // two perpendicular lines that intersect at (1,1)
  drawing[0].x = 1; drawing[0].y = 0;
  drawing[1].x = 1; drawing[1].y = 2;
  drawing[2].x = 0; drawing[2].y = 1;
  drawing[3].x = 2; drawing[3].y = 1;

  BOOST_REQUIRE(!is_straight_line_drawing(g,drawing));

  // two parallel horizontal lines
  drawing[0].x = 0; drawing[0].y = 0;
  drawing[1].x = 2; drawing[1].y = 0;

  BOOST_REQUIRE(is_straight_line_drawing(g,drawing));

  // two parallel vertical lines
  drawing[0].x = 0; drawing[0].y = 0;
  drawing[1].x = 0; drawing[1].y = 2;
  drawing[2].x = 1; drawing[2].y = 0;
  drawing[3].x = 1; drawing[3].y = 2;

  BOOST_REQUIRE(is_straight_line_drawing(g,drawing));

  // two lines that intersect at (1,1)
  drawing[0].x = 0; drawing[0].y = 0;
  drawing[1].x = 2; drawing[1].y = 2;
  drawing[2].x = 0; drawing[2].y = 2;
  drawing[3].x = 2; drawing[3].y = 0;

  BOOST_REQUIRE(!is_straight_line_drawing(g,drawing));

  // K_4 arranged in a diamond pattern, so that edges intersect
  g = graph_t(4);
  add_edge(0,1,g);
  add_edge(0,2,g);
  add_edge(0,3,g);
  add_edge(1,2,g);
  add_edge(1,3,g);
  add_edge(2,3,g);

  drawing_storage = drawing_storage_t(num_vertices(g));
  drawing = drawing_t(drawing_storage.begin(), get(vertex_index,g));

  drawing[0].x = 1; drawing[0].y = 2;
  drawing[1].x = 2; drawing[1].y = 1;
  drawing[2].x = 1; drawing[2].y = 0;
  drawing[3].x = 0; drawing[3].y = 1;

  BOOST_REQUIRE(!is_straight_line_drawing(g, drawing));

  // K_4 arranged so that no edges intersect
  drawing[0].x = 0; drawing[0].y = 0;
  drawing[1].x = 1; drawing[1].y = 1;
  drawing[2].x = 1; drawing[2].y = 2;
  drawing[3].x = 2; drawing[3].y = 0;

  BOOST_REQUIRE(is_straight_line_drawing(g, drawing));

  // a slightly more complicated example - edges (0,1) and (4,5)
  // intersect
  g = graph_t(8);
  add_edge(0,1,g);
  add_edge(2,3,g);
  add_edge(4,5,g);
  add_edge(6,7,g);

  drawing_storage = drawing_storage_t(num_vertices(g));
  drawing = drawing_t(drawing_storage.begin(), get(vertex_index,g));

  drawing[0].x = 1; drawing[0].y = 1;
  drawing[1].x = 5; drawing[1].y = 4;
  drawing[2].x = 2; drawing[2].y = 5;
  drawing[3].x = 4; drawing[3].y = 4;
  drawing[4].x = 3; drawing[4].y = 4;
  drawing[5].x = 3; drawing[5].y = 2;
  drawing[6].x = 4; drawing[6].y = 2;
  drawing[7].x = 1; drawing[7].y = 1;

  BOOST_REQUIRE(!is_straight_line_drawing(g, drawing));

  // form a graph consisting of a bunch of parallel vertical edges,
  // then place an edge at various positions to intersect edges
  g = graph_t(22);
  for(int i = 0; i < 11; ++i)
    add_edge(2*i,2*i+1,g);

  drawing_storage = drawing_storage_t(num_vertices(g));
  drawing = drawing_t(drawing_storage.begin(), get(vertex_index,g));

  for(int i = 0; i < 10; ++i)
    {
      drawing[2*i].x = i; drawing[2*i].y = 0;
      drawing[2*i+1].x = i; drawing[2*i+1].y = 10;
    }

  // put the final edge as a horizontal edge intersecting one other edge
  drawing[20].x = 5; drawing[20].y = 5;
  drawing[21].x = 7; drawing[21].y = 5;

  BOOST_REQUIRE(!is_straight_line_drawing(g, drawing));

  // make the final edge a diagonal intersecting multiple edges
  drawing[20].x = 2; drawing[20].y = 4;
  drawing[21].x = 9; drawing[21].y = 7;

  BOOST_REQUIRE(!is_straight_line_drawing(g, drawing));

  // reverse the slope
  drawing[20].x = 2; drawing[20].y = 7;
  drawing[21].x = 9; drawing[21].y = 4;

  BOOST_REQUIRE(!is_straight_line_drawing(g, drawing));

  return 0;
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Pawel Dlotko
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "Persistence_landscapes_on_grid_test"
#include <boost/test/unit_test.hpp>
#include <gudhi/reader_utils.h>
#include <gudhi/Persistence_landscape_on_grid.h>
#include <gudhi/Unitary_tests_utils.h>

#include <iostream>

using namespace Gudhi;
using namespace Gudhi::Persistence_representations;

double epsilon = 0.0005;

BOOST_AUTO_TEST_CASE(check_construction_of_landscape) {
  Persistence_landscape_on_grid l("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());
  l.print_to_file("landscape_from_file_with_diagram_1");

  Persistence_landscape_on_grid g;
  g.load_landscape_from_file("landscape_from_file_with_diagram_1");

  BOOST_CHECK(l == g);
}

BOOST_AUTO_TEST_CASE(check_construction_of_landscape_using_only_ten_levels) {
  // TODO
  unsigned number = 10;
  Persistence_landscape_on_grid l("data/file_with_diagram_1", 100, number);
  Persistence_landscape_on_grid g("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());
  // cut all the elements of order > 10 in g.

  for (size_t level = 0; level != number; ++level) {
    std::vector<double> v1 = l.vectorize(level);
    std::vector<double> v2 = g.vectorize(level);
    BOOST_CHECK(v1.size() == v2.size());
    for (size_t i = 0; i != v1.size(); ++i) {
      GUDHI_TEST_FLOAT_EQUALITY_CHECK(v1[i], v2[i]);
    }
  }
}

BOOST_AUTO_TEST_CASE(check_computations_of_integrals) {
  Persistence_landscape_on_grid p("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_integral_of_landscape(), 27.343, epsilon);
}

BOOST_AUTO_TEST_CASE(check_computations_of_integrals_for_each_level_separatelly) {
  Persistence_landscape_on_grid p("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());

  std::vector<double> integrals_fir_different_levels;
  // integrals_fir_different_levels.push_back();
  integrals_fir_different_levels.push_back(0.241168);
  integrals_fir_different_levels.push_back(0.239276);
  integrals_fir_different_levels.push_back(0.237882);
  integrals_fir_different_levels.push_back(0.235193);
  integrals_fir_different_levels.push_back(0.230115);
  integrals_fir_different_levels.push_back(0.227626);
  integrals_fir_different_levels.push_back(0.226132);
  integrals_fir_different_levels.push_back(0.223643);
  integrals_fir_different_levels.push_back(0.221651);
  integrals_fir_different_levels.push_back(0.220556);
  integrals_fir_different_levels.push_back(0.21727);
  integrals_fir_different_levels.push_back(0.215976);
  integrals_fir_different_levels.push_back(0.213685);
  integrals_fir_different_levels.push_back(0.211993);
  integrals_fir_different_levels.push_back(0.2102);
  integrals_fir_different_levels.push_back(0.208707);
  integrals_fir_different_levels.push_back(0.207014);
  integrals_fir_different_levels.push_back(0.205122);
  integrals_fir_different_levels.push_back(0.204226);
  integrals_fir_different_levels.push_back(0.202633);

  for (size_t level = 0; level != integrals_fir_different_levels.size(); ++level) {
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_integral_of_landscape(level), integrals_fir_different_levels[level],
                                    epsilon);
  }
}

BOOST_AUTO_TEST_CASE(check_computations_of_integrals_of_powers_of_landscape) {
  Persistence_landscape_on_grid p("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());

  std::vector<double> integrals_fir_different_powers;
  integrals_fir_different_powers.push_back(0.241168);
  integrals_fir_different_powers.push_back(0.239276);
  integrals_fir_different_powers.push_back(0.237882);
  integrals_fir_different_powers.push_back(0.235193);
  integrals_fir_different_powers.push_back(0.23011);

  for (size_t power = 0; power != 5; ++power) {
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_integral_of_landscape(power), integrals_fir_different_powers[power],
                                    epsilon);
  }
}

BOOST_AUTO_TEST_CASE(check_computations_of_values_on_different_points) {
  Persistence_landscape_on_grid p("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());

  std::vector<double> results_level_0;
  results_level_0.push_back(0.00997867);
  results_level_0.push_back(0.0521921);
  results_level_0.push_back(0.104312);
  results_level_0.push_back(0.156432);
  results_level_0.push_back(0.208552);
  results_level_0.push_back(0.260672);
  results_level_0.push_back(0.312792);
  results_level_0.push_back(0.364912);
  results_level_0.push_back(0.417032);
  results_level_0.push_back(0.429237);

  std::vector<double> results_level_10;
  results_level_10.push_back(7.21433e-05);
  results_level_10.push_back(0.0422135);
  results_level_10.push_back(0.0943335);
  results_level_10.push_back(0.146453);
  results_level_10.push_back(0.198573);
  results_level_10.push_back(0.240715);
  results_level_10.push_back(0.272877);
  results_level_10.push_back(0.324997);
  results_level_10.push_back(0.359232);
  results_level_10.push_back(0.379344);

  double x = 0.0012321;
  double dx = 0.05212;
  for (size_t i = 0; i != 10; ++i) {
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(0, x), results_level_0[i], epsilon);
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(10, x), results_level_10[i], epsilon);
    x += dx;
  }
}

BOOST_AUTO_TEST_CASE(check_computations_sum_differences_and_multiplications) {
  Persistence_landscape_on_grid p("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());
  Persistence_landscape_on_grid second("data/file_with_diagram_1", 100, std::numeric_limits<unsigned short>::max());

  Persistence_landscape_on_grid sum = p + second;
  Persistence_landscape_on_grid difference = p - second;
  Persistence_landscape_on_grid multiply_by_scalar = 10 * p;
  ;

  Persistence_landscape_on_grid template_sum;
  template_sum.load_landscape_from_file("data/sum_on_grid_test");

  Persistence_landscape_on_grid template_difference;
  template_difference.load_landscape_from_file("data/difference_on_grid_test");

  Persistence_landscape_on_grid template_multiply_by_scalar;
  template_multiply_by_scalar.load_landscape_from_file("data/multiply_by_scalar_on_grid_test");

  BOOST_CHECK(sum == template_sum);
  BOOST_CHECK(difference == template_difference);
  BOOST_CHECK(multiply_by_scalar == template_multiply_by_scalar);
}

BOOST_AUTO_TEST_CASE(check_computations_of_maxima_and_norms) {
  Persistence_landscape_on_grid p("data/file_with_diagram_1", 0., 1., 100);
  Persistence_landscape_on_grid second("data/file_with_diagram_2", 0., 1., 100);
  Persistence_landscape_on_grid sum = p + second;

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_maximum(), 0.46, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_norm_of_landscape(1), 27.3373, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_norm_of_landscape(2), 1.84143, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_norm_of_landscape(3), 0.927067, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(compute_distance_of_landscapes_on_grid(p, sum, 1), 16.8519, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(compute_distance_of_landscapes_on_grid(p, sum, 2), 1.44542, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(compute_distance_of_landscapes_on_grid(p, sum, std::numeric_limits<double>::max()),
                                  0.45, epsilon);
}

BOOST_AUTO_TEST_CASE(check_default_parameters_of_distances) {
  std::vector<std::pair<double, double> > diag = read_persistence_intervals_in_dimension("data/file_with_diagram");
  Persistence_landscape_on_grid p(diag, 0., 1., 100);

  std::vector<std::pair<double, double> > diag1 = read_persistence_intervals_in_dimension("data/file_with_diagram_1");
  Persistence_landscape_on_grid q(diag1, 0., 1., 100);

  double dist_numeric_limit_max = p.distance(q, std::numeric_limits<double>::max());
  double dist_infinity = p.distance(q, std::numeric_limits<double>::infinity());

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(dist_numeric_limit_max, dist_infinity);
}

BOOST_AUTO_TEST_CASE(check_computations_of_averages) {
  Persistence_landscape_on_grid p("data/file_with_diagram", 0., 1., 100);
  Persistence_landscape_on_grid q("data/file_with_diagram_1", 0., 1., 100);
  Persistence_landscape_on_grid av;
  av.compute_average({&p, &q});

  Persistence_landscape_on_grid template_average;
  template_average.load_landscape_from_file("data/average_on_a_grid");
  BOOST_CHECK(template_average == av);
}

BOOST_AUTO_TEST_CASE(check_computations_of_distances) {
  Persistence_landscape_on_grid p("data/file_with_diagram", 0., 1., 10000);
  Persistence_landscape_on_grid q("data/file_with_diagram_1", 0., 1., 10000);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.distance(q), 25.5779, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.distance(q, 2), 2.04891, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.distance(q, std::numeric_limits<double>::max()), 0.359, epsilon);
}

BOOST_AUTO_TEST_CASE(check_computations_of_scalar_product) {
  Persistence_landscape_on_grid p("data/file_with_diagram", 0., 1., 10000);
  Persistence_landscape_on_grid q("data/file_with_diagram_1", 0., 1., 10000);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_scalar_product(q), 0.754367, epsilon);
}

// Below I am storing the code used to generate tests for that functionality.
/*
        Persistence_landscape_on_grid l( "file_with_diagram_1" , 100 );
        l.print_to_file( "landscape_from_file_with_diagram_1" );

        Persistence_landscape_on_grid g;
        g.load_landscape_from_file( "landscape_from_file_with_diagram_1" );

        cerr << ( l == g );
        */

/*
Persistence_landscape_on_grid l( "file_with_diagram_1" , 100 );
cerr << l << endl;
cerr << l.compute_integral_of_landscape() << endl;
*/

/*
Persistence_landscape_on_grid p( "file_with_diagram_1" , 100 );
for ( size_t level = 0 ; level != 30 ; ++level )
{
        double integral = p.compute_integral_of_landscape( level );
        cerr << integral << endl;
}
*/

/*
Persistence_landscape_on_grid p( "file_with_diagram_1" , 100 );
for ( size_t power = 0 ; power != 5 ; ++power )
{
        double integral = p.compute_integral_of_landscape( (double)power );
        cerr << integral << endl;
}
*/

/*
Persistence_landscape_on_grid p( "file_with_diagram_1" , 100 );
double x = 0.0012321;
double dx = 0.05212;
for ( size_t i = 0 ; i != 10 ; ++i )
{
       cerr << p.compute_value_at_a_given_point(10,x) << endl;
       x += dx;
}
*/

/*
Persistence_landscape_on_grid p( "file_with_diagram_1",100 );
Persistence_landscape_on_grid second("file_with_diagram_1",100 );
Persistence_landscape_on_grid sum = p + second;
Persistence_landscape_on_grid difference = p - second;
Persistence_landscape_on_grid multiply_by_scalar = 10*p;
sum.print_to_file( "sum_on_grid_test" );
difference.print_to_file( "difference_on_grid_test" );
multiply_by_scalar.print_to_file( "multiply_by_scalar_on_grid_test" );
*/

/*
Persistence_landscape_on_grid p( "file_with_diagram_1" , 0 , 1 , 100 );
Persistence_landscape_on_grid second("file_with_diagram_1", 0 , 1 , 100 );
Persistence_landscape_on_grid sum = p + second;

cerr << "max : " << p.compute_maximum() << endl;
cerr << "1-norm : " << p.compute_norm_of_landscape(1) << endl;
cerr << "2-norm : " << p.compute_norm_of_landscape(2) << endl;
cerr << "3-norm : " << p.compute_norm_of_landscape(3) << endl;

cerr <<  compute_distance_of_landscapes_on_grid(p,sum,1) << endl;
cerr <<  compute_distance_of_landscapes_on_grid(p,sum,2) << endl;
cerr <<  compute_distance_of_landscapes_on_grid(p,sum,-1)  << endl;
*/

/*
Persistence_landscape_on_grid p( "file_with_diagram", 0,1,100 );
Persistence_landscape_on_grid q( "file_with_diagram_1", 0,1,100 );
Persistence_landscape_on_grid av;
av.compute_average( {&p,&q} );
av.print_to_file("average_on_a_grid");

Persistence_landscape_on_grid template_average;
template_average.load_landscape_from_file( "average_on_a_grid" );
if ( template_average == av )
{
        cerr << "OK OK \n";
}*/

/*
Persistence_landscape_on_grid p( "file_with_diagram" , 0,1,10000);
Persistence_landscape_on_grid q( "file_with_diagram_1" , 0,1,10000);
cerr <<  p.distance( &q )<< endl;
cerr <<  p.distance( &q , 2 ) << endl;
cerr <<  p.distance( &q , std::numeric_limits<double>::max() ) << endl;
*/

/*
        Persistence_landscape_on_grid p( "file_with_diagram", 0,1,10000 );
        Persistence_landscape_on_grid q( "file_with_diagram_1", 0,1,10000 );

        //std::vector< std::pair< double,double > > aa;
        //aa.push_back( std::make_pair( 0,1 ) );
        //Persistence_landscape_on_grid p( aa, 0,1,10 );
        //Persistence_landscape_on_grid q( aa, 0,1,10 );
        cerr <<  p.compute_scalar_product( &q ) << endl;
*/


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

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

//ComputeSVD algorithm test.
#include <gtest/gtest.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Testing/Utils/MatrixTestUtilities.h>
#include <Core/Algorithms/Math/ComputeSVD.h>
#include <Eigen/SVD>

using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::TestUtils;

namespace
{
    //Matrix for input.
    DenseMatrixHandle inputMatrix()
    {
        //Hard coded for testing.
        int column1 [12] = {5,4,7,8,10,10,4,6,8,7,9,6};
        int column2 [12] = {7,6,9,8,5,7,9,3,4,5,5,9};

        //Puts the columns into a matrix.
        DenseMatrixHandle inputM(boost::make_shared<DenseMatrix>(12,2));
        for (int i = 0; i < inputM->rows(); i++){
            (*inputM)(i,0) = column1[i];
            (*inputM)(i,1) = column2[i];
        }
        return inputM;
    }
}

//Checks if the outputs are correct.
//U,S,V: U: Left singular matrix, S: Singular values, V: Right singular matrix.
TEST(ComputeSVDtest, checkOutputs)
{
    ComputeSVDAlgo algo;

    DenseMatrixHandle m1(inputMatrix());
    DenseMatrixHandle LeftSingularMatrix_U;
    DenseMatrixHandle SingularValues_S;
    DenseMatrixHandle RightSingularMatrix_V;

    //Runs algorithm.
    algo.run(m1,LeftSingularMatrix_U,SingularValues_S,RightSingularMatrix_V);

    //Testing if the results are null.
    ASSERT_NE(nullptr,LeftSingularMatrix_U);
    ASSERT_NE(nullptr,SingularValues_S);
    ASSERT_NE(nullptr,RightSingularMatrix_V);

    //Check the dimensions of the matrices that were created for output.

    //Rows
    ASSERT_EQ(12,LeftSingularMatrix_U->rows());
    ASSERT_EQ(2,SingularValues_S->rows());
    ASSERT_EQ(2,RightSingularMatrix_V->rows());

    //Columns
    ASSERT_EQ(12,LeftSingularMatrix_U->cols());
    ASSERT_EQ(1,SingularValues_S->cols());
    ASSERT_EQ(2,RightSingularMatrix_V->cols());

    //Eigen does not create a diagonal matrix when it computes SVD, it just has a column with the singular values, so we must put it into a diagonal matrix to be able to do some matrix multiplication later.
    DenseMatrix sDiag = Eigen::MatrixXd::Constant(12,2,0);
    sDiag.diagonal() = SingularValues_S->col(0);

    //Multiplying back together and comparing to the centered matrix. They should be equal to each other with some tolerance.
    DenseMatrix product = (*LeftSingularMatrix_U) * sDiag * (*RightSingularMatrix_V).transpose();

    auto expected = *inputMatrix();

    //Comparing each element in the matrices.
    for (int i = 0; i < product.rows(); ++i) {
        for (int j = 0; j < product.cols(); ++j)
            ASSERT_NEAR(expected(i,j), product(i,j), 1e-5);
    }

}

//Tests for input with a dimension of zero.
TEST(ComputeSVDtest, ThrowsForZeroDimensionInput)
{
    ComputeSVDAlgo algo;

    DenseMatrixHandle m1(new DenseMatrix(5, 0));
    DenseMatrixHandle m2(new DenseMatrix(0, 5));
    DenseMatrixHandle m3(new DenseMatrix(0, 0));

    DenseMatrixHandle LeftSingularMatrix_U;
    DenseMatrixHandle SingularValues_S;
    DenseMatrixHandle RightSingularMatrix_V;

    //Runs algorithm and expects an error.
    EXPECT_ANY_THROW(algo.run(m1,LeftSingularMatrix_U,SingularValues_S,RightSingularMatrix_V));
    EXPECT_ANY_THROW(algo.run(m2,LeftSingularMatrix_U,SingularValues_S,RightSingularMatrix_V));
    EXPECT_ANY_THROW(algo.run(m3,LeftSingularMatrix_U,SingularValues_S,RightSingularMatrix_V));

}


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

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

#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Astrodynamics/Gravitation/triAxialEllipsoidGravity.h"

namespace tudat
{

namespace gravitation
{

//! Function to calculate (non-normalized) cosine spherical harmonic coefficient for a homogeneous
//! triaxial ellipsoid
double calculateCosineTermForTriaxialEllipsoidSphericalHarmonicGravity(
        const double aSquaredMinusCSquared, const double bSquaredMinusCSquared,
        const double referenceRadius, const int degree, const int order )
{
    // Initialize coefficient to zero
    double powerSeries = 0.0;

    // Only non-zero terms are for even degree and order
    if( !( ( degree % 2 != 0 || order % 2 != 0 ) ) )
    {
        using boost::math::factorial;

        // Calculate indices for algorithm
        int l = degree / 2;
        int m = order / 2;
        int maximumIndex = std::floor( ( l - m ) / 2 );

        // Evaluate single term in power series of final equation of Boyce (1997)
        for( int i = 0; i <= maximumIndex; i++ )
        {
            powerSeries +=
                    ( std::pow( ( aSquaredMinusCSquared - bSquaredMinusCSquared ) /
                                2.0, m + 2 * i ) *
                      std::pow( -( aSquaredMinusCSquared + bSquaredMinusCSquared ) /
                                2.0, l - m - 2 * i ) ) /
                    ( std::pow( 2.0, m + 2 * i ) * factorial< double >( l - m - 2 * i ) *
                      factorial< double >( m + i ) * factorial< double >( i ) );
        }

        // Calculate multiplier of power series in final equation of Boyce (1997)
        double multiplier = 3.0 / std::pow( referenceRadius, 2 * l ) *
                ( factorial< double >( l ) * factorial< double >( 2 * l - 2 * m ) ) /
                ( ( 2.0 * static_cast< double >( l ) + 3.0 ) * factorial< double >( 2 * l + 1 ) );
        if( order != 0 )
        {
            multiplier *= 2.0;
        }

        // Complete calculation of coefficient
        powerSeries *= multiplier;
    }
    return powerSeries;
}

//! Function to calculate triaxial ellipsoid reference radius
double calculateTriAxialEllipsoidReferenceRadius(
        const double axisA, const double axisB, const double axisC )
{
    return std::sqrt( 3.0 / ( 1.0 / ( axisA * axisA ) + 1.0 / ( axisB * axisB ) +
                              1.0 / ( axisC * axisC ) ) );
}

//! Function to calculate triaxial ellipsoid volume
double calculateTriAxialEllipsoidVolume(
        const double axisA, const double axisB, const double axisC )
{
    return 4.0 / 3.0 * mathematical_constants::PI * axisA * axisB * axisC;
}

//! Function to calculate (non-normalized) cosine spherical harmonic coefficients for a
//! homogeneous triaxial ellipsoid
Eigen::MatrixXd createTriAxialEllipsoidSphericalHarmonicCosineCoefficients(
        const double axisA, const double axisB, const double axisC,
        const int maximumDegree, const int maximumOrder )
{
    // Initialize vector to zeros
    Eigen::MatrixXd cosineCoefficients = Eigen::MatrixXd::Zero(
                maximumDegree + 1, maximumOrder + 1 );

    // Pre-calculate for single coefficient calculations.
    double aSquaredMinusCSquared = axisA * axisA - axisC * axisC;
    double bSquaredMinusCSquared = axisB * axisB - axisC * axisC;
    double referenceRadius =  calculateTriAxialEllipsoidReferenceRadius( axisA, axisB, axisC );

    // Iterate over all requested degrees.
    for( int i = 0; i <= maximumDegree; i++ )
    {
        // Iterate over all requested orders.
        for( int j = 0; ( ( j <= maximumOrder ) && ( j <= i ) ); j++ )
        {
            // Only even degree and order terms are non-zero
            if( ( i % 2 == 0 ) && ( j % 2 == 0 ) )
            {
                // Calculate coefficient at single degree and order.
                cosineCoefficients( i, j ) =
                        calculateCosineTermForTriaxialEllipsoidSphericalHarmonicGravity(
                            aSquaredMinusCSquared, bSquaredMinusCSquared, referenceRadius, i, j );
            }
        }
    }

    return cosineCoefficients;
}

//! Function to calculate (non-normalized) cosine and sine spherical harmonic coefficients for a
//! homogeneous triaxial ellipsoid
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > createTriAxialEllipsoidSphericalHarmonicCoefficients(
        const double axisA, const double axisB, const double axisC,
        const int maximumDegree, const int maximumOrder )
{
    // Calculate cosine coefficients and add sine matrix (all zeroes)
    return std::make_pair( createTriAxialEllipsoidSphericalHarmonicCosineCoefficients(
                               axisA, axisB, axisC, maximumDegree, maximumOrder ),
                           Eigen::MatrixXd::Zero( maximumDegree + 1, maximumOrder + 1 ) );
}

//! Function to calculate (normalized) cosine and sine spherical harmonic coefficients for a
//! homogeneous triaxial ellipsoid
std::pair< Eigen::MatrixXd, Eigen::MatrixXd >
createTriAxialEllipsoidNormalizedSphericalHarmonicCoefficients(
        const double axisA, const double axisB, const double axisC,
        const int maximumDegree, const int maximumOrder )
{
    // Calculate non-normalized coefficients
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > unNormalizedCoefficients =
            createTriAxialEllipsoidSphericalHarmonicCoefficients(
                axisA, axisB, axisC, maximumDegree, maximumOrder );

    // Retrieve cosine coefficients
    Eigen::MatrixXd normalizedCosineCoefficients = unNormalizedCoefficients.first;

    // Iterate over all degrees and orders and normalized coefficients
    for( int i = 2; i < normalizedCosineCoefficients.rows( ); i++ )
    {
        for( int j = 0; ( ( j < normalizedCosineCoefficients.cols( ) ) &&
                                   ( j <= i ) ); j++ )
        {
            normalizedCosineCoefficients( i, j ) = normalizedCosineCoefficients( i, j ) /
                    basic_mathematics::calculateLegendreGeodesyNormalizationFactor( i, j );
        }
    }

    // Return geodesy-normalized coefficients.
    return std::make_pair( normalizedCosineCoefficients, unNormalizedCoefficients.second );
}

}

}


#define BOOST_TEST_MODULE test_mat_vec_mul
#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mave/mave.hpp>
#include <tests/generate_random_matrices.hpp>
#include <tests/tolerance.hpp>

typedef boost::mpl::list<double, float> test_targets;

constexpr std::size_t N = 12000;

BOOST_AUTO_TEST_CASE_TEMPLATE(mat3x3_vec3, T, test_targets)
{
    std::mt19937 mt(123456789);

    const auto matrices = mave::test::generate_random_positive<mave::matrix<T, 3, 3>>(N, mt);
    const auto vectors  = mave::test::generate_random_positive<mave::vector<T, 3>>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto& m = matrices.at(i);
        const auto& v = vectors.at(i);

        const auto v2 = m * v;

        BOOST_TEST(v2[0] == m(0,0)*v[0] + m(0,1)*v[1] + m(0,2)*v[2], mave::test::tolerance<T>());
        BOOST_TEST(v2[1] == m(1,0)*v[0] + m(1,1)*v[1] + m(1,2)*v[2], mave::test::tolerance<T>());
        BOOST_TEST(v2[2] == m(2,0)*v[0] + m(2,1)*v[1] + m(2,2)*v[2], mave::test::tolerance<T>());

        BOOST_TEST(v.diagnosis());
        BOOST_TEST(v2.diagnosis());
    }
}


#include "geoutil.h"
#include "position.h"
#include <Eigen/Dense>
#include <cmath>

using namespace himan;
using namespace Eigen;

double geoutil::Distance(const point& a, const point& b, double r)
{
	const double alpha = std::pow(std::sin((b.Y() - a.Y()) / 360 * M_PI), 2) +
	               std::cos(a.Y() / 180 * M_PI) * std::cos(b.Y() / 180 * M_PI) *
	                   std::pow(std::sin((b.X() - a.X()) / 360 * M_PI), 2);
	return r * 2 * std::atan2(std::sqrt(alpha), std::sqrt(1 - alpha));
}

double geoutil::Area(const point& P1, const point& P2, const point& P3, double r)
{
	const position<double> p1(P1.Y() / 180 * M_PI, P1.Y() / 180 * M_PI, 0, earth_shape<double>(r));
	const position<double> p2(P2.Y() / 180 * M_PI, P2.X() / 180 * M_PI, 0, earth_shape<double>(r));
	const position<double> p3(P3.Y() / 180 * M_PI, P3.X() / 180 * M_PI, 0, earth_shape<double>(r));

	const Matrix<double, 3, 1> A(p1.Data());
	const Matrix<double, 3, 1> B(p2.Data());
	const Matrix<double, 3, 1> C(p3.Data());
	const double a = std::atan(A.cross(B).norm() / A.dot(B));
	const double b = std::atan(B.cross(C).norm() / B.dot(C));
	const double c = std::atan(C.cross(A).norm() / C.dot(A));
	const double s = (a + b + c) / 2;
	return 4 *
	       std::atan(
	           std::sqrt(std::tan(s / 2) * std::tan((s - a) / 2) * std::tan((s - b) / 2) * std::tan((s - c) / 2))) *
	       r * r;
}

double geoutil::Bearing(const point& a, const point& b)
{
	return std::atan2(std::sin((b.X() - a.X())/180*M_PI) * std::cos(b.Y()/180*M_PI) , std::cos(a.Y()/180*M_PI) * std::sin(b.Y()/180*M_PI) - std::sin(a.Y()/180*M_PI) * std::cos(b.Y()/180*M_PI) * std::cos((b.X() - a.X())/180*M_PI));
}

bool geoutil::InsideTriangle(const point& a, const point& b, const point& c, const point& p)
{
	const double epsilon = 1e-12;

	position<double> A(a.Y() / 180 * M_PI, a.X() / 180 * M_PI, 0, earth_shape<double>(1));
	position<double> B(b.Y() / 180 * M_PI, b.X() / 180 * M_PI, 0, earth_shape<double>(1));
	position<double> C(c.Y() / 180 * M_PI, c.X() / 180 * M_PI, 0, earth_shape<double>(1));

	position<double> P(p.Y() / 180 * M_PI, p.X() / 180 * M_PI, 0, earth_shape<double>(1));

	Matrix<double, 3, 1> v(P.Data());
	Matrix<double, 3, 3> M;
	M << A.X(), B.X(), C.X(), A.Y(), B.Y(), C.Y(), A.Z(), B.Z(), C.Z();

	ColPivHouseholderQR<Matrix<double, 3, 3>> dec(M);

	Matrix<double, 3, 1> beta = dec.solve(v);

	return beta(0) >= -epsilon && beta(1) >= -epsilon && beta(2) >= -epsilon;
}


/**	\file 	test_main.cpp
*	\brief	Testing Kernels in MLearnKernels.h
*/
// Test framework
#define CATCH_CONFIG_MAIN
#include <catch.hpp>
#include <test_common.h>

// MLearn
#include <MLearn/Core>
#include <MLearn/StochasticProcess/GaussianProcess/GP.h>
#include "test_gp_utils.h"

// Eigen
#include <Eigen/Core>

TEST_CASE("Test computation of covariance matrix"){
	typedef double FT;
	using namespace MLearn;
	using namespace TestUtils;
	using namespace SP;
	using namespace GP;

	uint N = 5;
	uint dim = 10;

	MLMatrix<FT> pts = MLMatrix<FT>::Random(dim, N);
	MLMatrix<FT> other_pts = MLMatrix<FT>::Random(dim, 2*N);

	SECTION("Test expected result with mock kernel"){
		MLMatrix<FT> covariance = MLMatrix<FT>::Zero(N, N);
		MockKernel K;

		compute_gp_covariance(pts, K, covariance);

		REQUIRE(diff_norm(pts.transpose()*pts, covariance) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
	}

	SECTION("Test compilation with a real kernel"){
		MLMatrix<FT> covariance = MLMatrix<FT>::Zero(N, N);
		Kernel< KernelType::LINEAR > K;
		compute_gp_covariance(pts, K, covariance);
	}

	SECTION("Test expected result with mock kernel and two set of points"){
		MockKernel K;
		MLMatrix<FT> covariance = MLMatrix<FT>::Zero(N, 2*N);

		compute_gp_covariance(pts, other_pts, K, covariance);

		REQUIRE(diff_norm(pts.transpose()*other_pts, covariance) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
	}

	SECTION("Test consistent results"){
		MockKernel K;
		MLMatrix<FT> covariance_1 = MLMatrix<FT>::Zero(N, N);
		MLMatrix<FT> covariance_2 = MLMatrix<FT>::Zero(N, N);

		compute_gp_covariance(pts, K, covariance_1);
		compute_gp_covariance(pts, pts, K, covariance_2);

		REQUIRE(diff_norm(covariance_1, covariance_2) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
	}
}

TEST_CASE("Test gaussian process class"){
	typedef double FT;
	using namespace MLearn;
	using namespace TestUtils;
	using namespace SP;
	using namespace GP;

	uint N = 3;
	uint dim = 10;

	MLMatrix<FT> pts = MLMatrix<FT>::Random(dim, N);
	MLMatrix<FT> covariance(N, N);
	MLVector<FT> mean = MLVector<FT>::Random(N);

	SECTION("Test class constructors and assign"){
		covariance << 4, 1,-1,
					  1, 2, 1,
					 -1, 1, 2;

		GaussianProcess<FT> ref_gp(mean, covariance);
		REQUIRE(diff_norm(mean, ref_gp.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(diff_norm(covariance, ref_gp.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		GaussianProcess<FT> copy_gp(ref_gp);
		REQUIRE(diff_norm(mean, copy_gp.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(diff_norm(covariance, copy_gp.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		GaussianProcess<FT> move_gp(std::move(copy_gp));
		REQUIRE(diff_norm(mean, move_gp.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(diff_norm(covariance, move_gp.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		GaussianProcess<FT> copy_assign_gp;
		copy_assign_gp = ref_gp;
		REQUIRE(diff_norm(mean, copy_assign_gp.mean()) ==
		 	Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(diff_norm(covariance, copy_assign_gp.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		GaussianProcess<FT> move_assign_gp;
		move_assign_gp = std::move(GaussianProcess<FT>(ref_gp));
		REQUIRE(diff_norm(mean, move_assign_gp.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(diff_norm(covariance, move_assign_gp.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

	}

	SECTION("Test class sampling"){
		uint N_samples = 7;

		Kernel< KernelType::LINEAR > K;
		compute_gp_covariance(pts, K, covariance);

		GaussianProcess<FT> gp(mean, covariance);

		MLMatrix<FT> samples = gp.sample(N_samples);
		REQUIRE(samples.rows() == N);
		REQUIRE(samples.cols() == N_samples);

		gp.set_covariance(MLMatrix<FT>::Zero(N, N));
		CHECK(diff_norm(gp.covariance(), covariance) > 0.0);

		gp.set_covariance(pts, K);
		REQUIRE(diff_norm(gp.covariance(), covariance) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		MLMatrix<FT> samples_static = gp.sample(mean, pts, K, N_samples);
		REQUIRE(samples_static.rows() == N);
		REQUIRE(samples_static.cols() == N_samples);

	}

	SECTION("Test confidence intervals"){
		GaussianProcess<FT>
			gp(MLVector<FT>::Zero(N), MLMatrix<FT>::Identity(N, N));
		FT prob = 0.95;
		MLMatrix<FT> conf_interval = gp.confidence_interval(prob);

		conf_interval = conf_interval.cwiseAbs();
		conf_interval.array() -= 1.96;

		REQUIRE(diff_norm(MLMatrix<FT>::Zero(2, N), conf_interval) ==
			Approx(0).margin(1e-4));

	}
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE bfgs_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/format.hpp>
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/adiis_costfunction.h"
#include "votca/xtp/bfgs_trm.h"
#include "votca/xtp/logger.h"
#include "votca/xtp/optimiser_costfunction.h"

using namespace votca::xtp;
using namespace votca;
using namespace std;

BOOST_AUTO_TEST_SUITE(bfgs_test)

BOOST_AUTO_TEST_CASE(parabola_test) {
  class parabola : public Optimiser_costfunction {

    double EvaluateCost(const Eigen::VectorXd& parameters) override {
      Eigen::VectorXd value = parameters;
      value(0) -= 2;
      double cost = value.cwiseAbs2().sum();
      return cost;
    }

    Eigen::VectorXd EvaluateGradient(
        const Eigen::VectorXd& parameters) override {
      Eigen::VectorXd gradient = 2 * parameters;
      gradient(0) -= 4;
      return gradient;
    }

    bool Converged(const Eigen::VectorXd&, double,
                   const Eigen::VectorXd& gradient) override {
      if (gradient.cwiseAbs().maxCoeff() < 1e-8) {
        return true;
      } else {
        return false;
      }
    }

    Index NumParameters() const override { return 5; }
  };

  parabola p5;
  BFGSTRM bfgstrm(p5);
  Logger log;
  bfgstrm.setLog(&log);
  bfgstrm.setNumofIterations(100);
  bfgstrm.setTrustRadius(0.1);
  bfgstrm.Optimize(5 * Eigen::VectorXd::Ones(5));

  Eigen::VectorXd ref = Eigen::VectorXd::Zero(5);
  ref(0) = 2;
  bool equal = bfgstrm.getParameters().isApprox(ref, 0.00001);
  if (!equal) {
    cout << "minimum found:" << endl;
    cout << bfgstrm.getParameters() << endl;
    cout << "minimum ref:" << endl;
    cout << ref << endl;
  } else {
    cout << bfgstrm.getIteration() << endl;
  }
  BOOST_CHECK_EQUAL(equal, 1);
}

BOOST_AUTO_TEST_CASE(booth_test) {
  class booth : public Optimiser_costfunction {

    double EvaluateCost(const Eigen::VectorXd& parameters) override {
      double x = parameters[0];
      double y = parameters[1];

      return (x + 2 * y - 7) * (x + 2 * y - 7) +
             (2 * x + y - 5) * (2 * x + y - 5);
    }

    Eigen::VectorXd EvaluateGradient(
        const Eigen::VectorXd& parameters) override {
      double x = parameters[0];
      double y = parameters[1];
      Eigen::VectorXd gradient = Eigen::VectorXd::Zero(2);
      gradient[0] = 2 * (5 * x + 4 * y - 17);
      gradient[1] = 2 * (4 * x + 5 * y - 19);
      return gradient;
    }

    bool Converged(const Eigen::VectorXd&, double,
                   const Eigen::VectorXd& gradient) override {
      if (gradient.cwiseAbs().maxCoeff() < 1e-8) {
        return true;
      } else {
        return false;
      }
    }

    Index NumParameters() const override { return 2; }
  };

  booth p2;
  BFGSTRM bfgstrm(p2);
  Logger log;
  bfgstrm.setLog(&log);
  bfgstrm.setNumofIterations(1000);
  bfgstrm.setTrustRadius(0.1);
  bfgstrm.Optimize(5 * Eigen::VectorXd::Ones(2));

  Eigen::VectorXd ref = Eigen::VectorXd::Zero(2);
  ref << 1, 3;
  bool equal = bfgstrm.getParameters().isApprox(ref, 0.00001);
  if (!equal) {
    cout << "minimum found:" << endl;
    cout << bfgstrm.getParameters() << endl;
    cout << "minimum ref:" << endl;
    cout << ref << endl;
  } else {
    cout << bfgstrm.getIteration() << endl;
  }
  BOOST_CHECK_EQUAL(equal, 1);
}

BOOST_AUTO_TEST_CASE(adiis_test) {
  Index size = 5;

  Eigen::VectorXd DiF = Eigen::VectorXd::Zero(size);
  DiF << 0.679243, 0.562675, 0.39399, -0.0258519, 0;
  Eigen::MatrixXd DiFj = Eigen::MatrixXd::Zero(size, size);
  DiFj << 0.613998, 0.192684, 0.0326914, -0.193661, 0, 0.192371, 0.0653754,
      0.0131825, -0.0631915, 0, 0.032739, 0.0131883, 0.0038493, -0.0110991, 0,
      -0.192873, -0.0631203, -0.0111063, 0.0633221, 0, 0, 0, 0, 0, 0;

  Logger log;

  ADIIS_costfunction a_cost = ADIIS_costfunction(DiF, DiFj);
  BFGSTRM optimizer = BFGSTRM(a_cost);
  optimizer.setNumofIterations(1000);
  optimizer.setTrustRadius(0.01);
  optimizer.setLog(&log);
  // Starting point: equal weights on all matrices
  Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(size, 1.0 / (double)size);
  optimizer.Optimize(coeffs);
  bool success = optimizer.Success();
  coeffs = optimizer.getParameters().cwiseAbs2();
  double xnorm = coeffs.sum();
  coeffs /= xnorm;

  BOOST_CHECK_EQUAL(success, true);
  Eigen::VectorXd ref = Eigen::VectorXd::Zero(size);
  ref << 0, 0, 0, 0.40826075912352, 0.59173924087648;
  bool equal = coeffs.isApprox(ref, 0.00001);
  if (!equal) {
    cout << "minimum found:" << endl;
    cout << coeffs << endl;
    cout << "minimum ref:" << endl;
    cout << ref << endl;
  } else {
    cout << optimizer.getIteration() << endl;
  }
  BOOST_CHECK_EQUAL(equal, 1);
}

BOOST_AUTO_TEST_SUITE_END()


// solving A * X = B
// A symmetric/hermitian positive definite
// factor (potrf()) and solve (potrs())

// #define BOOST_UBLAS_STRICT_HERMITIAN
// .. doesn't work (yet?)

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::size_t;
using std::cout;
using std::endl;

typedef std::complex<double> cmplx_t;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> cm_t;
#endif

#ifndef F_UPPER
typedef ublas::symmetric_adaptor<m_t, ublas::lower> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> herm_t;
#else
typedef ublas::symmetric_adaptor<m_t, ublas::upper> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::upper> herm_t;
#endif

int main() {

  // for more descriptive comments see ublas_posv.cc
  cout << endl;

  // symmetric
  cout << "real symmetric\n" << endl;

  size_t n = 5;
  m_t a (n, n);
  symm_t sa (a);
#ifdef F_UPPER
  init_symm (sa, 'u');
#else
  init_symm (sa, 'l');
#endif
  print_m (sa, "sa");
  cout << endl;

  size_t nrhs = 2;
  m_t x (n, nrhs);
#ifndef F_ROW_MAJOR
  m_t b (n, nrhs);
#else
  m_t b (nrhs, n);
#endif
  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  atlas::set (1., xc0);
  atlas::set (2., xc1);
#ifndef F_ROW_MAJOR
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
  atlas::symm (sa, x, b);
#else
  atlas::symm (CblasLeft, 1.0, sa, x, 0.0, b);
#endif
#else
  ublas::matrix_row<m_t> br0 (b, 0), br1 (b, 1);
  atlas::symv (sa, xc0, br0);
  atlas::symv (sa, xc1, br1);
#endif
  print_m (b, "b");
  cout << endl;

  int ierr = atlas::cholesky_factor (sa);  // potrf()
  if (!ierr) {
    atlas::cholesky_substitute (sa, b);  // potrs()
    print_m (b, "x");
  }
  cout << endl;

  /////////////////////////////////////////////////////////
  // hermitian
  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

  cm_t ca (3, 3);
  herm_t ha (ca);
  cm_t cx (3, 1);
#ifndef F_ROW_MAJOR
  cm_t cb (3, 1);
#else
  cm_t cb (1, 3);
#endif

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (3, 0);
  ha (1, 0) = cmplx_t (4, -2);
  ha (1, 1) = cmplx_t (5, 0);
  ha (2, 0) = cmplx_t (-7, -5);
  ha (2, 1) = cmplx_t (0, 3);
  ha (2, 2) = cmplx_t (2, 0);
#else
  ha (0, 0) = cmplx_t (3, 0);
  ha (0, 1) = cmplx_t (4, 2);
  ha (0, 2) = cmplx_t (-7, 5);
  ha (1, 1) = cmplx_t (5, 0);
  ha (1, 2) = cmplx_t (0, -3);
  ha (2, 2) = cmplx_t (2, 0);
#endif
  print_m (ha, "ha");
  cout << endl;

  ublas::matrix_column<cm_t> cx0 (cx, 0);
  atlas::set (cmplx_t (1, -1), cx0);
  print_m (cx, "cx");
  cout << endl;
#ifndef F_ROW_MAJOR
  ublas::matrix_column<cm_t> cb0 (cb, 0);
#else
  ublas::matrix_row<cm_t> cb0 (cb, 0);
#endif
  atlas::hemv (ha, cx0, cb0);
  print_m (cb, "cb");
  cout << endl;

  ierr = atlas::potrf (ha);
  if (ierr == 0) {
    atlas::potrs (ha, cb);
    print_m (cb, "cx");
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (25, 0);
  ha (1, 0) = cmplx_t (-5, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (2, 0) = cmplx_t (10, -5);
  ha (2, 1) = cmplx_t (4, 6);
  ha (2, 2) = cmplx_t (71, 0);
#else
  ha (0, 0) = cmplx_t (25, 0);
  ha (0, 1) = cmplx_t (-5, -5);
  ha (0, 2) = cmplx_t (10, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (1, 2) = cmplx_t (4, -6);
  ha (2, 2) = cmplx_t (71, 0);
#endif
  print_m (ha, "ha");
  cout << endl;

#ifndef F_ROW_MAJOR
  cm_t cb32 (3, 2);
  cb32 (0, 0) = cmplx_t (60, -55);
  cb32 (1, 0) = cmplx_t (34, 58);
  cb32 (2, 0) = cmplx_t (13, -152);
  cb32 (0, 1) = cmplx_t (70, 10);
  cb32 (1, 1) = cmplx_t (-51, 110);
  cb32 (2, 1) = cmplx_t (75, 63);
#else
  cm_t cb32 (2, 3);
  cb32 (0, 0) = cmplx_t (60, -55);
  cb32 (0, 1) = cmplx_t (34, 58);
  cb32 (0, 2) = cmplx_t (13, -152);
  cb32 (1, 0) = cmplx_t (70, 10);
  cb32 (1, 1) = cmplx_t (-51, 110);
  cb32 (1, 2) = cmplx_t (75, 63);
#endif
  print_m (cb32, "cb");
  cout << endl;

  ierr = atlas::potrf (ha);
  if (ierr == 0) {
    atlas::potrs (ha, cb32);
    print_m (cb32, "cx");
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

}


#include <cmath>
#include <iostream>
#include <boost/mpl/bool.hpp>
#include <boost/lambda/lambda.hpp>

double sc(double x)
{
    return sin(x) + cos(x);
}

struct sc_f
{
    double operator() (double x) const
    {
	return sin(x) + cos(x);
    }
};

class psc_f
{
  public:
    psc_f(double alpha) : alpha(alpha) {}

    double operator() (double x) const
    {
	return sin(alpha * x) + cos(x);
    }
  private:
    double alpha;
};

#if 0
// with function pointers
double fin_diff(double f(double), double x, double h)
{
    return ( f(x+h) - f(x) ) / h;
}
#endif

template <typename F, typename T>
inline T fin_diff(F f, const T& x, const T& h)
{
    return ( f(x+h) - f(x) ) / h;
}

template <typename F, typename T>
class derivative
{
  public:
    derivative(const F& f, const T& h) : f(f), h(h) {}

    T operator()(const T& x) const
    {
	return ( f(x+h) - f(x) ) / h;
    }
  private:
    const F& f;
    T        h;
};

template <typename F, typename T>
class second_derivative
{
  public:
    second_derivative(const F& f, const T& h) : h(h), fp(f, h) {}

    T operator()(const T& x) const
    {
	return ( fp(x+h) - fp(x) ) / h;
    }
  private:
    T        h;
    derivative<F, T> fp;
};

#if 0
template <typename F, typename T, unsigned N>
class nth_derivative
{
    using prev_derivative= nth_derivative<F, T, N-1>;
  public:
    nth_derivative(const F& f, const T& h) : h(h), fp(f, h) {}

    T operator()(const T& x) const
    {
	return ( fp(x+h) - fp(x) ) / h;
    }
  private:
    T        h;
    prev_derivative fp;
};

#else

template <typename F, typename T, unsigned N>
class nth_derivative
{
    using prev_derivative= nth_derivative<F, T, N-1>;
  public:
    nth_derivative(const F& f, const T& h) : h(h), fp(f, h) {}

    T operator()(const T& x) const
    {
	return N & 1 ? ( fp(x+h) - fp(x) ) / h
	             : ( fp(x) - fp(x-h) ) / h;
    }
  private:
    T        h;
    prev_derivative fp;
};

#if 0 // for meta-programming, maybe
template <typename F, typename T, unsigned N>
class nth_derivative
{
    using prev_derivative= nth_derivative<F, T, N-1>;
  public:
    nth_derivative(const F& f, const T& h) : h(h), fp(f, h) {}

    T operator()(const T& x) const
    {
	return diff(x, boost::mpl::bool_<N & 1>());
    }

  private:

    T diff(const T& x, boost::mpl::true_) const
    {
	return ( fp(x+h) - fp(x) ) / h;
    }

    T diff(const T& x, boost::mpl::false_) const
    {
	return ( fp(x) - fp(x-h) ) / h;
    }

    T        h;
    prev_derivative fp;
};
#endif

#endif

#if 0
template <typename F, typename T>
class nth_derivative<F, T, 1>
{
  public:
    nth_derivative(const F& f, const T& h) : f(f), h(h) {}

    T operator()(const T& x) const
    {
	return ( f(x+h) - f(x) ) / h;
    }
  private:
    const F& f;
    T        h;
};
#endif

template <typename F, typename T>
class nth_derivative<F, T, 1>
  : public derivative<F, T>
{
    using derivative<F, T>::derivative;

  // public:
  //   nth_derivative(const F& f, const T& h) : derivative<F, T>(f, h) {}
};

#if 0
template <typename F, typename T, unsigned N> // Not clever
nth_derivative<F, T, N>
make_nth_derivative(const F& f, const T& h)
{
    return nth_derivative<F, T, N>(f, h);
}
#endif

template <unsigned N, typename F, typename T>
nth_derivative<F, T, N>
make_nth_derivative(const F& f, const T& h)
{
    return nth_derivative<F, T, N>(f, h);
}

struct et {};

template <typename F>
void error(const F& f)
{
    et e= f;
}

int main()
{
    using namespace std;

    psc_f psc_o(1.0);
    cout << fin_diff(psc_o, 1., 0.001) << endl;
    cout << fin_diff(psc_f(2.0), 1., 0.001) << endl;
    cout << fin_diff(psc_f(2.0), 0., 0.001) << endl;
    cout << fin_diff(sc, 0., 0.001) << endl;

    using d_psc_f= derivative<psc_f, double>;
    using dd_psc_f= derivative<d_psc_f, double>;

    d_psc_f                                     d_psc_o(psc_o, 0.001);
    dd_psc_f                                     dd_psc_o(d_psc_o, 0.001);

    cout << "der. of sin(0) + cos(0) is " << d_psc_o(0.0) << '\n';
    cout << "2nd der. of sin(0) + cos(0) is " << dd_psc_o(0.0) << '\n';

    second_derivative<psc_f, double> dd_psc_2_o(psc_f(1.0), 0.001);
    cout << "2nd der. of sin(0) + cos(0) is " << dd_psc_2_o(0.0) << '\n';

    nth_derivative<psc_f, double, 2> dd_psc_3_o(psc_f(1.0), 0.001);
    cout << "2nd der. of sin(0) + cos(0) is " << dd_psc_3_o(0.0) << '\n';

    nth_derivative<psc_f, double, 6> d6_psc_o(psc_f(1.0), 0.00001);
    cout << "6th der. of sin(0) + cos(0) is " << d6_psc_o(0.0) << '\n';

    nth_derivative<psc_f, double, 12> d12_psc_o(psc_f(1.0), 0.00001);
    cout << "12th der. of sin(0) + cos(0) is " << d12_psc_o(0.0) << '\n';

    nth_derivative<psc_f, double, 22> d22_psc_o(psc_f(1.0), 0.00001);
    cout << "22nd der. of sin(0) + cos(0) is " << d22_psc_o(0.0) << '\n';

    // auto d7_psc_o= make_nth_derivative<psc_f, double, 7>(psc_o, 0.00001);

    // nth_derivative<psc_f, double, 7> d7_psc_o(psc_o, 0.00001);
    // auto d7_psc_o= nth_derivative<psc_f, double, 7>(psc_o, 0.00001);
    // nth_derivative<decltype(psc_o), decltype(0.00001), 7> d7_psc_o(psc_o, 0.00001);

    make_nth_derivative<7, psc_f, double>(psc_o, 0.00001);
    auto d7_psc_o= make_nth_derivative<7>(psc_o, 0.00001);

    cout << "7th der. of psc_o at x=3 is "
	 << make_nth_derivative<7>(psc_o, 0.00001)(3.0) << '\n';

    using boost::lambda::_1;

    (3.5 * _1 + 4.0) * _1 * _1;

    cout << "2nd der. of 3.5*x^3+4*x^2 at x=2 is "
	      << make_nth_derivative<2>((3.5 * _1 + 4.0) * _1 * _1, 0.0001)(2) << '\n';

    cout << "2nd der. of 3.5*x^3+4*x^2 at x=2 is "
	 << make_nth_derivative<2>([](double x){ return (3.5 * x + 4.0) * x * x; }, 0.0001)(2) << '\n';

    auto d7_cub_l= make_nth_derivative<7>([](double x){ return (3.5 * x + 4.0) * x * x; }, 0.0001);
    auto d7_psc_l= make_nth_derivative<7>([](double x){ return sin(2.5*x) + cos(x); }, 0.0001);

    // error((3.5 * _1 + _1) * _1 * _1);
    return 0;
}


//
// Copyright (c) 2018 INRIA
//

#include "utils/macros.hpp"
#include "pinocchio/math/sincos.hpp"
#include <cstdlib>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

template<typename Scalar>
void testSINCOS(int n)
{
  for(int k = 0; k < n; ++k)
  {
    Scalar sin_value, cos_value;
    Scalar alpha = (Scalar)std::rand()/(Scalar)RAND_MAX;
    pinocchio::SINCOS(alpha,&sin_value,&cos_value);

    Scalar sin_value_ref = std::sin(alpha),
           cos_value_ref = std::cos(alpha);

    BOOST_CHECK(sin_value == sin_value_ref);
    BOOST_CHECK(cos_value == cos_value_ref);
  }
}

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_sincos)
{
#ifndef NDEBUG
  const int n = 1e3;
#else
  const int n = 1e6;
#endif
  testSINCOS<float>(n);
  testSINCOS<double>(n);
  testSINCOS<long double>(n);
}

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
// chisq_summand.hpp                                                         //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_CHISQ_SUMMAND_FORMULA_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_CHISQ_SUMMAND_FORMULA_HPP_ER_2010
#include <boost/numeric/conversion/converter.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{

    template<typename T1,typename T2,typename T3>
    T1 chisq_summand_formula(
        const T2& expected_n,
        const T3& observed_n
    )
    {
        typedef boost::numeric::converter<T1,T2> conv2_;
        typedef boost::numeric::converter<T1,T3> conv3_;
        T1 summand = conv2_::convert( expected_n )
            - conv3_::convert( observed_n );
        summand *= summand;
        return summand / conv2_::convert( expected_n );
    }

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif