#ifndef BART_SRC_CALCULATOR_DRIFT_DIFFUSION_DRIFT_DIFFUSION_VECTOR_CALCULATOR_I_HPP_
#define BART_SRC_CALCULATOR_DRIFT_DIFFUSION_DRIFT_DIFFUSION_VECTOR_CALCULATOR_I_HPP_

#include <deal.II/base/tensor.h>

//! Calculators for the drift-diffusion term used by the drift-diffusion formulation.
namespace bart::calculator::drift_diffusion {

/*! \brief Interface for classes that calculate the drift-diffusion vector.
 *
 * This class has the single job of combining the terms of the drift-diffusion vector and returning the correct value.
 * The vector is calculated for a single degree of freedom and quadrature point within the cell. Therefore, the values
 * of the scalar flux and integrated angular flux are both scalar values.
 *
 * \f[
 * D(\phi, \vec{J}, \nabla\varphi, D) = \frac{1}{\phi}\left[\vec{J} - D\nabla\varphi\right]\;,
 * \f]
 *
 * where \f$\phi\f$ is the scalar flux, \f$J\f$ is the current, \f$\nabla\varphi\f$ is the gradient of the shape function,
 * and \f$D\f$ is the diffusion coefficent. The resulting return vector
 * will be of length \f$d \times 1\f$ where \f$d\f$ is the dimension of the problem.
 *
 * @tparam dim spatial dimension, required for return tensor and shape functions.
 *
 */
 template <int dim>
class DriftDiffusionVectorCalculatorI {
 public:
  using Tensor = typename dealii::Tensor<1, dim>;
  virtual auto DriftDiffusionVector(const double scalar_flux,
                                    const Tensor& current,
                                    const Tensor& shape_gradient,
                                    const double diffusion_coefficient) const -> Tensor = 0;
};

} // namespace bart::calculator::drift_diffusion

#endif //BART_SRC_CALCULATOR_DRIFT_DIFFUSION_DRIFT_DIFFUSION_VECTOR_CALCULATOR_I_HPP_


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

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h"
#include "Tudat/SimulationSetup/tudatEstimationHeader.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat;
using namespace gravitation;
using namespace aerodynamics;
using namespace ephemerides;
using namespace simulation_setup;
using namespace orbital_element_conversions;
using namespace unit_conversions;
using namespace orbit_determination;
using namespace acceleration_partials;
using namespace spice_interface;
using namespace orbit_determination;
using namespace estimatable_parameters;
using namespace electro_magnetism;

BOOST_AUTO_TEST_SUITE( test_spherical_harmonic_partials )

BOOST_AUTO_TEST_CASE( testSphericalHarmonicPartials )
{
    // Short-cuts.
    using namespace gravitation;

    const double gravitationalParameter = 3.986004418e14;
    const double planetaryRadius = 6378137.0;

    const Eigen::MatrixXd cosineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0,
              9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7,
              7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7,
              3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0,
              6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7,
              -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7
              ).finished( );
    const Eigen::MatrixXd sineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0,
              0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0,
              0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7,
              3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7,
              -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7
              ).finished( );

    Eigen::Vector3d position( 7.0e6, 8.0e6, 9.0e6 );
    Eigen::Vector3d nominalSphericalPosition = coordinate_conversions::
            convertCartesianToSpherical( position );
    nominalSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 -
            nominalSphericalPosition( 1 );

    std::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache
            = std::make_shared< basic_mathematics::SphericalHarmonicsCache >( 6, 6 );
    sphericalHarmonicsCache->update(
                nominalSphericalPosition( 0 ), std::sin( nominalSphericalPosition( 1 ) ), nominalSphericalPosition( 2 ),
                planetaryRadius );
    std::shared_ptr< basic_mathematics::LegendreCache > legendreCache = sphericalHarmonicsCache->getLegendreCache( );

    double currentLongitude = sphericalHarmonicsCache->getCurrentLongitude( );
    double currentPolynomialArgument = legendreCache->getCurrentPolynomialParameter( );

    Eigen::MatrixXd upPerturbedLegendrePolynomials = Eigen::MatrixXd( 6, 6 );
    Eigen::MatrixXd downPerturbedLegendrePolynomials = Eigen::MatrixXd( 6, 6 );

    Eigen::MatrixXd upPerturbedLegendrePolynomialPartials = Eigen::MatrixXd( 6, 6 );
    Eigen::MatrixXd downPerturbedLegendrePolynomialPartials = Eigen::MatrixXd( 6, 6 );

    Eigen::MatrixXd analyticalLegendrePolynomialPartials = Eigen::MatrixXd( 6, 6 );
    Eigen::MatrixXd analyticalLegendrePolynomialSecondPartials = Eigen::MatrixXd( 6, 6 );

    legendreCache->setComputeSecondDerivatives( 1 );
    legendreCache->update( currentPolynomialArgument + 0.1  );

    legendreCache->update( currentPolynomialArgument );

    for( unsigned int i = 0; i < 6; i++ )
    {
        for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
        {
            analyticalLegendrePolynomialPartials( i, j ) = legendreCache->getLegendrePolynomialDerivative( i, j );
            analyticalLegendrePolynomialSecondPartials( i, j ) = legendreCache->getLegendrePolynomialSecondDerivative( i, j );
        }

    }

    double polynomialArgumentPerturbation = 1.0E-6;
    {
        legendreCache->update( currentPolynomialArgument + polynomialArgumentPerturbation );
        for( unsigned int i = 0; i < 6; i++ )
        {
            for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
            {
                upPerturbedLegendrePolynomials( i, j ) = legendreCache->getLegendrePolynomial( i, j );
                upPerturbedLegendrePolynomialPartials( i, j ) = legendreCache->getLegendrePolynomialDerivative( i, j );
            }
        }

        legendreCache->update( currentPolynomialArgument - polynomialArgumentPerturbation );
        for( unsigned int i = 0; i < 6; i++ )
        {
            for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
            {
                downPerturbedLegendrePolynomials( i, j ) = legendreCache->getLegendrePolynomial( i, j );
                downPerturbedLegendrePolynomialPartials( i, j ) = legendreCache->getLegendrePolynomialDerivative( i, j );
            }
        }
    }

    Eigen::MatrixXd numericalLegendrePolynomialPartials =
            ( upPerturbedLegendrePolynomials - downPerturbedLegendrePolynomials ) /
            ( 2.0 * polynomialArgumentPerturbation );
    Eigen::MatrixXd numericalLegendrePolynomialSecondPartials =
            ( upPerturbedLegendrePolynomialPartials - downPerturbedLegendrePolynomialPartials ) /
            ( 2.0 * polynomialArgumentPerturbation );

    for( unsigned int i = 0; i < 6; i++ )
    {
        for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( numericalLegendrePolynomialPartials( i, j ) - analyticalLegendrePolynomialPartials( i, j ) ), 1.0E-8 );
            BOOST_CHECK_SMALL(
                        std::fabs( numericalLegendrePolynomialSecondPartials( i, j ) - analyticalLegendrePolynomialSecondPartials( i, j ) ), 1.0E-8 );
        }
    }

    std::vector< std::vector< Eigen::Vector3d > > sphericalPotentialGradients;

    std::vector< std::vector< Eigen::Matrix3d > > upPerturbedSphericalPotentialGradients;
    std::vector< std::vector< Eigen::Matrix3d > > downPerturbedSphericalPotentialGradients;

    std::vector< std::vector< Eigen::Matrix3d > > numericalSphericalPotentialHessian;
    std::vector< std::vector< Eigen::Matrix3d > > analyticalSphericalPotentialHessian;

    Eigen::Matrix3d normalization;
    normalization <<  nominalSphericalPosition( 0 ) * nominalSphericalPosition( 0 ), nominalSphericalPosition( 0 ), nominalSphericalPosition( 0 ),
            nominalSphericalPosition( 0 ), 1.0 , 1.0,
            nominalSphericalPosition( 0 ), 1.0, 1.0;
    for( unsigned int i = 0; i < 6; i++ )
    {
        std::vector< Eigen::Vector3d > singleTermPotentialGradients;
        singleTermPotentialGradients.resize( 6 );
        sphericalPotentialGradients.push_back( singleTermPotentialGradients );

        std::vector< Eigen::Matrix3d > singleTermPotentialPartials;
        singleTermPotentialPartials.resize( 6 );
        upPerturbedSphericalPotentialGradients.push_back( singleTermPotentialPartials );
        downPerturbedSphericalPotentialGradients.push_back( singleTermPotentialPartials );

        numericalSphericalPotentialHessian.push_back( singleTermPotentialPartials );
        analyticalSphericalPotentialHessian.push_back( singleTermPotentialPartials );

    }

    sphericalHarmonicsCache->update( position.norm( ), currentPolynomialArgument, currentLongitude, planetaryRadius );

    for( unsigned int i = 0; i < 6; i++ )
    {
        for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
        {
            sphericalPotentialGradients[ i ][ j ] = basic_mathematics::computePotentialGradient(
                        nominalSphericalPosition, gravitationalParameter / planetaryRadius, i, j,
                        cosineCoefficients( i, j ), sineCoefficients( i, j ), legendreCache->getLegendrePolynomial( i, j ),
                        legendreCache->getLegendrePolynomialDerivative( i, j ), sphericalHarmonicsCache );
        }
    }

    Eigen::Vector3d perturbedSphericalPosition;
    Eigen::Vector3d sphericalStatePerturbation;
    sphericalStatePerturbation << 10.0, 1.0E-7, 1.0E-8;

    for( unsigned parameter = 0; parameter < 3; parameter++ )
    {
        perturbedSphericalPosition = nominalSphericalPosition;
        perturbedSphericalPosition( parameter ) += sphericalStatePerturbation( parameter );

        sphericalHarmonicsCache->update(
                    perturbedSphericalPosition( 0 ), std::sin( perturbedSphericalPosition( 1 ) ),
                    perturbedSphericalPosition( 2 ), planetaryRadius );

        for( unsigned int i = 0; i < 6; i++ )
        {
            for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
            {
                upPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) = basic_mathematics::computePotentialGradient(
                            perturbedSphericalPosition, gravitationalParameter / planetaryRadius, i, j,
                            cosineCoefficients( i, j ), sineCoefficients( i, j ), legendreCache->getLegendrePolynomial( i, j ),
                            legendreCache->getLegendrePolynomialDerivative( i, j ), sphericalHarmonicsCache );
            }
        }

        perturbedSphericalPosition = nominalSphericalPosition;
        perturbedSphericalPosition( parameter ) -= sphericalStatePerturbation( parameter );

        sphericalHarmonicsCache->update(
                    perturbedSphericalPosition( 0 ), std::sin( perturbedSphericalPosition( 1 ) ),
                    perturbedSphericalPosition( 2 ), planetaryRadius );

        for( unsigned int i = 0; i < 6; i++ )
        {
            for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
            {
                downPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) = basic_mathematics::computePotentialGradient(
                            perturbedSphericalPosition, gravitationalParameter / planetaryRadius, i, j,
                            cosineCoefficients( i, j ), sineCoefficients( i, j ), legendreCache->getLegendrePolynomial( i, j ),
                            legendreCache->getLegendrePolynomialDerivative( i, j ), sphericalHarmonicsCache );
            }
        }

        for( unsigned int i = 0; i < 6; i++ )
        {
            for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
            {
                numericalSphericalPotentialHessian[ i ][ j ].block( 0, parameter, 3, 1 ) =
                        ( ( upPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) -
                            downPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) ) /
                          ( 2.0 * sphericalStatePerturbation( parameter ) ) );

            }

        }
    }

    for( unsigned int i = 1; i < 6; i++ )
    {
        for( unsigned int j = 0; ( j <= i && j < 6 ); j++ )
        {
            computePotentialSphericalHessian(
                        nominalSphericalPosition, gravitationalParameter / planetaryRadius, i, j,
                        cosineCoefficients( i, j ), sineCoefficients( i, j ), sphericalHarmonicsCache,
                        analyticalSphericalPotentialHessian[ i ][ j ] );

            analyticalSphericalPotentialHessian[ i ][ j ] = analyticalSphericalPotentialHessian[ i ][ j ].cwiseProduct(
                        normalization );
            numericalSphericalPotentialHessian[ i ][ j ] = numericalSphericalPotentialHessian[ i ][ j ].cwiseProduct(
                        normalization );
            for( unsigned int k = 0; k < 3; k++ )
            {
                for( unsigned int l = 0; l < 3; l++ )
                {
                    BOOST_CHECK_SMALL( analyticalSphericalPotentialHessian[ i ][ j ]( k, l ) -
                            numericalSphericalPotentialHessian[ i ][ j ]( k, l ), 2.5E-5 );
                }

            }

        }
    }

    Eigen::Matrix3d cumulativeSphericalHessian =  computeCumulativeSphericalHessian(
                nominalSphericalPosition, planetaryRadius, gravitationalParameter, cosineCoefficients, sineCoefficients,
                sphericalHarmonicsCache );

    Eigen::Matrix3d nominalGradientTransformationMatrix =
            coordinate_conversions::getSphericalToCartesianGradientMatrix( position );

    Eigen::Vector3d upPerturbedTotalGradient;
    Eigen::Vector3d downPerturbedTotalGradient;
    Eigen::Vector3d perturbedCartesianPosition;

    Eigen::Matrix3d numericalTotalSphericalGradient;

    sphericalStatePerturbation( 0 ) *= 10.0;
    sphericalStatePerturbation( 1 ) *= 100.0;
    sphericalStatePerturbation( 2 ) *= 1000.0;
    std::map< std::pair< int, int >, Eigen::Vector3d > dummyMap;

    for( unsigned parameter = 0; parameter < 3; parameter++ )
    {
        perturbedSphericalPosition = nominalSphericalPosition;
        perturbedSphericalPosition( parameter ) += sphericalStatePerturbation( parameter );

        perturbedSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - perturbedSphericalPosition( 1 );
        perturbedCartesianPosition = coordinate_conversions::convertSphericalToCartesian(
                    perturbedSphericalPosition );

        upPerturbedTotalGradient =
                coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) *
                computeGeodesyNormalizedGravitationalAccelerationSum(
                    perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients,
                    sphericalHarmonicsCache, dummyMap );

        perturbedSphericalPosition = nominalSphericalPosition;
        perturbedSphericalPosition( parameter ) -= sphericalStatePerturbation( parameter );

        perturbedSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - perturbedSphericalPosition( 1 );
        perturbedCartesianPosition = coordinate_conversions::convertSphericalToCartesian(
                    perturbedSphericalPosition );

        downPerturbedTotalGradient =
                coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) *
                computeGeodesyNormalizedGravitationalAccelerationSum(
                    perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients,
                    sphericalHarmonicsCache, dummyMap );

        numericalTotalSphericalGradient.block( 0, parameter, 3, 1 ) =
                ( upPerturbedTotalGradient - downPerturbedTotalGradient ) / ( 2.0 * sphericalStatePerturbation( parameter ) );

    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                cumulativeSphericalHessian, numericalTotalSphericalGradient, 1.0E-6 );

    Eigen::Vector3d cartesianStatePerturbation;
    cartesianStatePerturbation << 10.0, 10.0, 10.0;

    for( unsigned parameter = 0; parameter < 3; parameter++ )
    {
        perturbedCartesianPosition = position;
        perturbedCartesianPosition( parameter ) += cartesianStatePerturbation( parameter );

        upPerturbedTotalGradient =
                nominalGradientTransformationMatrix *
                coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) *
                computeGeodesyNormalizedGravitationalAccelerationSum(
                    perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients,
                    sphericalHarmonicsCache, dummyMap );

        perturbedCartesianPosition = position;
        perturbedCartesianPosition( parameter ) -= cartesianStatePerturbation( parameter );

        downPerturbedTotalGradient =
                nominalGradientTransformationMatrix *
                coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) *
                computeGeodesyNormalizedGravitationalAccelerationSum(
                    perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients,
                    sphericalHarmonicsCache, dummyMap );

        numericalTotalSphericalGradient.block( 0, parameter, 3, 1 ) =
                ( upPerturbedTotalGradient - downPerturbedTotalGradient ) / ( 2.0 * cartesianStatePerturbation( parameter ) );

    }

    Eigen::Matrix3d nominalGradientTransformationMatrixTranspose = nominalGradientTransformationMatrix.transpose( );
    Eigen::Matrix3d computedTotalSphericalGradient  =
            ( nominalGradientTransformationMatrix * cumulativeSphericalHessian  ) *
            nominalGradientTransformationMatrixTranspose;
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                computedTotalSphericalGradient,
                numericalTotalSphericalGradient, 1.0E-6 );

    for( unsigned parameter = 0; parameter < 3; parameter++ )
    {
        perturbedCartesianPosition = position;
        perturbedCartesianPosition( parameter ) += cartesianStatePerturbation( parameter );

        upPerturbedTotalGradient =
                computeGeodesyNormalizedGravitationalAccelerationSum(
                    perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients,
                    sphericalHarmonicsCache, dummyMap );

        perturbedCartesianPosition = position;
        perturbedCartesianPosition( parameter ) -= cartesianStatePerturbation( parameter );

        downPerturbedTotalGradient =
                computeGeodesyNormalizedGravitationalAccelerationSum(
                    perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients,
                    sphericalHarmonicsCache, dummyMap );

        numericalTotalSphericalGradient.block( 0, parameter, 3, 1 ) =
                ( upPerturbedTotalGradient - downPerturbedTotalGradient ) / ( 2.0 * cartesianStatePerturbation( parameter ) );

    }

    Eigen::Matrix3d totalGradientCartesianPartial =
            computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration(
                position, planetaryRadius, gravitationalParameter, cosineCoefficients, sineCoefficients,
                sphericalHarmonicsCache );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                totalGradientCartesianPartial, numericalTotalSphericalGradient, 1.0E-6 );
}

//! Function to get tidal deformation model for Earth
std::vector< std::shared_ptr< GravityFieldVariationSettings > > getEarthGravityFieldVariationSettings( )
{
    std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations;

    std::vector< std::string > deformingBodies;
    deformingBodies.push_back( "Moon" );

    std::vector< std::vector< std::complex< double > > > loveNumbers;

    std::vector< std::complex< double > > degreeTwoLoveNumbers_;
    degreeTwoLoveNumbers_.push_back( std::complex< double >( 0.29525, -0.00087 ) );
    degreeTwoLoveNumbers_.push_back( std::complex< double >( 0.29525, -0.00087 ) );
    degreeTwoLoveNumbers_.push_back( std::complex< double >( 0.29525, -0.00087 ) );
    std::vector< std::complex< double > > degreeThreeLoveNumbers_;
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    degreeThreeLoveNumbers_.push_back( std::complex< double >( 0.093, 0.0 ) );
    loveNumbers.push_back( degreeTwoLoveNumbers_ );
    loveNumbers.push_back( degreeThreeLoveNumbers_ );

    std::shared_ptr< GravityFieldVariationSettings > singleGravityFieldVariation =
            std::make_shared< BasicSolidBodyGravityFieldVariationSettings >( deformingBodies, loveNumbers,
                                                                             6378137.0 );
    gravityFieldVariations.push_back( singleGravityFieldVariation );
    return gravityFieldVariations;
}

BOOST_AUTO_TEST_CASE( testSphericalHarmonicAccelerationPartial )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Create empty bodies, earth and vehicle.
    std::shared_ptr< Body > earth = std::make_shared< Body >( );
    std::shared_ptr< Body > vehicle = std::make_shared< Body >( );

    const double gravitationalParameter = 3.986004418e14;
    const double planetaryRadius = 6378137.0;

    Eigen::MatrixXd cosineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0,
              9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7,
              7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7,
              3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0,
              6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7,
              -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7
              ).finished( );

    Eigen::MatrixXd sineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0,
              0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0,
              0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7,
              3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7,
              -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7
              ).finished( );

    std::shared_ptr< GravityFieldSettings > earthGravityFieldSettings =
            std::make_shared< SphericalHarmonicsGravityFieldSettings >
            ( gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, "IAU_Earth" );

    std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariationSettings =
            getEarthGravityFieldVariationSettings( );

    NamedBodyMap bodyMap;
    bodyMap[ "Earth" ] = earth;
    bodyMap[ "Vehicle" ] = vehicle;
    bodyMap[ "Moon" ] = createBodies( getDefaultBodySettings( { "Moon" } ) ).at( "Moon" );

    std::shared_ptr< ephemerides::SimpleRotationalEphemeris > simpleRotationalEphemeris =
            std::make_shared< ephemerides::SimpleRotationalEphemeris >(
                spice_interface::computeRotationQuaternionBetweenFrames( "ECLIPJ2000" , "IAU_Earth", 0.0 ),
                2.0 * mathematical_constants::PI / 86400.0,
                1.0E7,
                "ECLIPJ2000" , "IAU_Earth" );
    earth->setRotationalEphemeris( simpleRotationalEphemeris );

    std::shared_ptr< tudat::gravitation::TimeDependentSphericalHarmonicsGravityField > earthGravityField =
            std::dynamic_pointer_cast< gravitation::TimeDependentSphericalHarmonicsGravityField  >(
                tudat::simulation_setup::createGravityFieldModel(
                    earthGravityFieldSettings, "Earth", bodyMap, gravityFieldVariationSettings ) );
    earth->setGravityFieldModel( earthGravityField );
    bodyMap[ "Earth" ]->setGravityFieldVariationSet(
                createGravityFieldModelVariationsSet(
                    "Earth", bodyMap, gravityFieldVariationSettings ) );

    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Set current state of vehicle and earth.
    double testTime = 1.0E6;
    earth->setState( Eigen::Vector6d::Zero( ) );
    earth->setCurrentRotationToLocalFrameFromEphemeris( testTime );
    bodyMap[ "Moon" ] ->setState( tudat::spice_interface::getBodyCartesianStateAtEpoch( "Moon", "Earth", "ECLIPJ2000" ,"None", testTime ) );

    // Set Keplerian elements for Asterix.
    Eigen::Vector6d asterixInitialStateInKeplerianElements;
    asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3;
    asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
    asterixInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
    asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
            = unit_conversions::convertDegreesToRadians( 235.7 );
    asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
            = unit_conversions::convertDegreesToRadians( 23.4 );
    asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

    Eigen::Vector6d asterixInitialState = orbital_element_conversions::convertKeplerianToCartesianElements(
                asterixInitialStateInKeplerianElements, gravitationalParameter );

    vehicle->setState( asterixInitialState );

    // Create acceleration due to vehicle on earth.
    std::shared_ptr< SphericalHarmonicAccelerationSettings > accelerationSettings =
            std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 );
    std::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > gravitationalAcceleration =
            std::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >(
                createAccelerationModel( vehicle, earth, accelerationSettings, "Vehicle", "Earth" ) );

    gravitationalAcceleration->updateMembers( 0.0 );
    gravitationalAcceleration->getAcceleration( );

    // Declare numerical partials.
    Eigen::Matrix3d testPartialWrtEarthPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtEarthVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtVehiclePosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtVehicleVelocity = Eigen::Matrix3d::Zero( );

    // Declare perturbations in position for numerical partial/
    Eigen::Vector3d positionPerturbation;
    positionPerturbation << 10.0, 10.0, 10.0;
    Eigen::Vector3d velocityPerturbation;
    velocityPerturbation << 1.0E-3, 1.0E-3, 1.0E-3;

    // Create state access/modification functions for bodies.
    std::function< void( Eigen::Vector6d ) > earthStateSetFunction =
            std::bind( &Body::setState, earth, std::placeholders::_1  );
    std::function< void( Eigen::Vector6d ) > vehicleStateSetFunction =
            std::bind( &Body::setState, vehicle, std::placeholders::_1  );
    std::function< Eigen::Vector6d ( ) > earthStateGetFunction =
            std::bind( &Body::getState, earth );
    std::function< Eigen::Vector6d ( ) > vehicleStateGetFunction =
            std::bind( &Body::getState, vehicle );

    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back( std::make_shared< InitialRotationalStateEstimatableParameterSettings< double > >(
                                  "Earth", 0.0, "ECLIPJ2000" ) );
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Earth", gravitational_parameter) );
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Earth", constant_rotation_rate ) );
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Earth", rotation_pole_position ) );

    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  2, 0, 5, 4, "Earth", spherical_harmonics_cosine_coefficient_block ) );
    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  2, 1, 5, 4, "Earth", spherical_harmonics_sine_coefficient_block ) );

    parameterNames.push_back( std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 2, std::vector< int >{ 2, 0, 1 }, "", false ) );
    parameterNames.push_back( std::make_shared< FullDegreeTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 2, "Moon", true ) );
    parameterNames.push_back( std::make_shared< FullDegreeTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 3, "", false ) );
    parameterNames.push_back( std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >(
                                  "Earth", 3, std::vector< int >{ 0, 3 }, "", true ) );

    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parameterSet =
            createParametersToEstimate( parameterNames, bodyMap );

    // Check if incompatible tidal parameters correctly throw an error
    {
        bool isExceptionCaught = false;
        std::vector< std::shared_ptr< EstimatableParameterSettings > > wrongParameterNames;
        wrongParameterNames.resize( 1 );
        wrongParameterNames[ 0 ] = std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >(
                    "Earth", 2, std::vector< int >{ 2, 0, 1 }, "Sun", false );
        try
        {
            std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parameterSet =
                    createParametersToEstimate( wrongParameterNames, bodyMap );
        }
        catch( std::runtime_error )
        {
            isExceptionCaught = true;
        }
        BOOST_CHECK_EQUAL( isExceptionCaught, true );

        std::vector< std::string > deformingBodyNames;
        deformingBodyNames.push_back( "Moon" );
        deformingBodyNames.push_back( "Sun" );

        wrongParameterNames[ 0 ] = std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >(
                    "Earth", 2, std::vector< int >{ 2, 0, 1 }, deformingBodyNames, false );
        isExceptionCaught = false;
        try
        {
            std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parameterSet =
                    createParametersToEstimate( wrongParameterNames, bodyMap );
        }
        catch( std::runtime_error )
        {
            isExceptionCaught = true;
        }
        BOOST_CHECK_EQUAL( isExceptionCaught, true );

        wrongParameterNames[ 0 ] = std::make_shared< FullDegreeTidalLoveNumberEstimatableParameterSettings >(
                    "Earth", 2, deformingBodyNames, true );
        isExceptionCaught = false;
        try
        {
            std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parameterSet =
                    createParametersToEstimate( wrongParameterNames, bodyMap );
        }
        catch( std::runtime_error )
        {
            isExceptionCaught = true;
        }
        BOOST_CHECK_EQUAL( isExceptionCaught, true );

        wrongParameterNames[ 0 ] = std::make_shared< FullDegreeTidalLoveNumberEstimatableParameterSettings >(
                    "Earth", 3, "Sun", false );
        isExceptionCaught = false;
        try
        {
            std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parameterSet =
                    createParametersToEstimate( wrongParameterNames, bodyMap );
        }
        catch( std::runtime_error )
        {
            isExceptionCaught = true;
        }
        BOOST_CHECK_EQUAL( isExceptionCaught, true );
    }

    // Create acceleration partial object.
    std::shared_ptr< SphericalHarmonicsGravityPartial > accelerationPartial =
            std::dynamic_pointer_cast< SphericalHarmonicsGravityPartial > (
                createAnalyticalAccelerationPartial(
                    gravitationalAcceleration, std::make_pair( "Vehicle", vehicle ), std::make_pair( "Earth", earth ),
                    bodyMap, parameterSet ) );

    accelerationPartial->update( testTime );

    Eigen::MatrixXd partialWrtVehiclePosition = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtPositionOfAcceleratedBody( partialWrtVehiclePosition.block( 0, 0, 3, 3 ) );
    Eigen::MatrixXd partialWrtVehicleVelocity = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtVelocityOfAcceleratedBody( partialWrtVehicleVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );
    Eigen::MatrixXd partialWrtEarthPosition = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtPositionOfAcceleratingBody( partialWrtEarthPosition.block( 0, 0, 3, 3 ) );
    Eigen::MatrixXd partialWrtEarthVelocity = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtVelocityOfAcceleratingBody( partialWrtEarthVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );

    Eigen::MatrixXd partialWrtEarthOrientation = Eigen::MatrixXd::Zero( 3, 7 );
    accelerationPartial->wrtNonTranslationalStateOfAdditionalBody(
                partialWrtEarthOrientation.block( 0, 0, 3, 7 ), std::make_pair( "Earth", "" ), propagators::rotational_state );

    // Calculate numerical partials.
    testPartialWrtVehiclePosition = calculateAccelerationWrtStatePartials(
                vehicleStateSetFunction, gravitationalAcceleration, vehicle->getState( ), positionPerturbation, 0 );
    testPartialWrtVehicleVelocity = calculateAccelerationWrtStatePartials(
                vehicleStateSetFunction, gravitationalAcceleration, vehicle->getState( ), velocityPerturbation, 3 );

    testPartialWrtEarthPosition = calculateAccelerationWrtStatePartials(
                earthStateSetFunction, gravitationalAcceleration, earth->getState( ), positionPerturbation, 0 );
    testPartialWrtEarthVelocity = calculateAccelerationWrtStatePartials(
                earthStateSetFunction, gravitationalAcceleration, earth->getState( ), velocityPerturbation, 3 );

    Eigen::Vector4d orientationPerturbation;
    orientationPerturbation << 1.0E-8, 1.0E-8, 1.0E-8, 1.0E-8;

    std::function< void( Eigen::Vector7d ) > earthRotationalStateSetFunction =
            std::bind( &Body::setCurrentRotationalStateToLocalFrame, earth, std::placeholders::_1 );
    std::vector< Eigen::Vector4d > appliedQuaternionPerturbation;
    Eigen::MatrixXd accelerationDeviations = calculateAccelerationDeviationDueToOrientationChange(
                earthRotationalStateSetFunction, gravitationalAcceleration, earth->getRotationalStateVector( ), orientationPerturbation,
                appliedQuaternionPerturbation );

    // Calculate numerical partials.
    std::map< int, std::shared_ptr< EstimatableParameter< double > > > doubleParameters =
            parameterSet->getDoubleParameters( );
    std::map< int, std::shared_ptr< EstimatableParameter< double > > >::iterator doubleParametersIterator =
            doubleParameters.begin( );

    Eigen::Vector3d testPartialWrtEarthGravitationalParameter = calculateAccelerationWrtParameterPartials(
                doubleParametersIterator->second, gravitationalAcceleration, 1.0E12 );
    Eigen::Vector3d partialWrtEarthGravitationalParameter = accelerationPartial->wrtParameter(
                doubleParametersIterator->second );
    doubleParametersIterator++;

    Eigen::Vector3d partialWrtEarthRotationRate = accelerationPartial->wrtParameter(
                doubleParametersIterator->second );
    Eigen::Vector3d testPartialWrtEarthRotationRate = calculateAccelerationWrtParameterPartials(
                doubleParametersIterator->second, gravitationalAcceleration, 1.0E-12, &emptyFunction, testTime, std::bind(
                    &Body::setCurrentRotationToLocalFrameFromEphemeris, earth, std::placeholders::_1 ) );

    std::map< int, std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > vectorParameters =
            parameterSet->getVectorParameters( );
    std::map< int, std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > >::iterator vectorParametersIterator =
            vectorParameters.begin( );
    Eigen::MatrixXd partialWrtPolePosition = accelerationPartial->wrtParameter(
                vectorParametersIterator->second );
    Eigen::MatrixXd testPartialWrtPosition = calculateAccelerationWrtParameterPartials(
                vectorParametersIterator->second, gravitationalAcceleration, Eigen::Vector2d::Constant( 1.0E-6 ),
                &emptyFunction, testTime, std::bind(
                    &Body::setCurrentRotationToLocalFrameFromEphemeris, earth, std::placeholders::_1 ) );
    vectorParametersIterator++;

    std::function< void( ) > sphericalHarmonicFieldUpdate =
            std::bind( &tudat::gravitation::TimeDependentSphericalHarmonicsGravityField::update,
                       std::dynamic_pointer_cast< TimeDependentSphericalHarmonicsGravityField >( earthGravityField ), testTime );

    Eigen::MatrixXd partialWrtCosineCoefficients = accelerationPartial->wrtParameter(
                vectorParametersIterator->second );
    Eigen::MatrixXd testPartialWrtCosineCoefficients = calculateAccelerationWrtParameterPartials(
                vectorParametersIterator->second, gravitationalAcceleration, vectorParametersIterator->second->getParameterValue( ) * 1.0E-2,
                sphericalHarmonicFieldUpdate );
    vectorParametersIterator++;

    accelerationPartial->getParameterPartialFunction(  vectorParametersIterator->second );
    Eigen::MatrixXd partialWrtSineCoefficients = accelerationPartial->wrtParameter(
                vectorParametersIterator->second );
    Eigen::MatrixXd testPartialWrtSineCoefficients = calculateAccelerationWrtParameterPartials(
                vectorParametersIterator->second, gravitationalAcceleration, vectorParametersIterator->second->getParameterValue( ) * 1.0E-2,
                sphericalHarmonicFieldUpdate );
    vectorParametersIterator++;

    Eigen::MatrixXd partialWrtDegreeTwoLoveNumberAtSeparateOrders = accelerationPartial->wrtParameter(
                vectorParametersIterator->second );
    Eigen::MatrixXd testPartialWrtDegreeTwoOrderTwoLoveNumberAtSeparateOrders = calculateAccelerationWrtParameterPartials(
                vectorParametersIterator->second, gravitationalAcceleration, Eigen::VectorXd::Constant( 3, 1.0 ), sphericalHarmonicFieldUpdate );
    vectorParametersIterator++;

    Eigen::MatrixXd partialWrtComplexDegreeTwoLoveNumber = accelerationPartial->wrtParameter(
                vectorParametersIterator->second );
    Eigen::MatrixXd testPartialWrtComplexDegreeTwoLoveNumber = calculateAccelerationWrtParameterPartials(
                vectorParametersIterator->second, gravitationalAcceleration, Eigen::VectorXd::Constant( 2, 1.0 ), sphericalHarmonicFieldUpdate );
    vectorParametersIterator++;

    Eigen::MatrixXd partialWrtDegreeThreeLoveNumber = accelerationPartial->wrtParameter(
                vectorParametersIterator->second );
    Eigen::MatrixXd testPartialWrtDegreeThreeLoveNumber = calculateAccelerationWrtParameterPartials(
                vectorParametersIterator->second, gravitationalAcceleration, Eigen::VectorXd::Constant( 1, 10.0 ), sphericalHarmonicFieldUpdate );
    vectorParametersIterator++;

    Eigen::MatrixXd partialWrtComplexDegreeThreeLoveNumberAtSeparateOrder = accelerationPartial->wrtParameter(
                vectorParametersIterator->second );
    Eigen::MatrixXd testPartialWrtComplexDegreeThreeLoveNumberAtSeparateOrder = calculateAccelerationWrtParameterPartials(
                vectorParametersIterator->second, gravitationalAcceleration, Eigen::VectorXd::Constant( 4, 10.0 ), sphericalHarmonicFieldUpdate );

    Eigen::VectorXd nominalTidalParameter = vectorParametersIterator->second->getParameterValue( );

    vectorParametersIterator->second->setParameterValue( nominalTidalParameter + Eigen::VectorXd::Constant( nominalTidalParameter.rows( ), 1.0 ) );
    earthGravityField->update( testTime );
    Eigen::MatrixXd upperturbedCosineCoefficients =
            earthGravityField->getCosineCoefficients( ).block( 0, 0, 3, 3 );
    Eigen::MatrixXd upperturbedSineCoefficients =
            earthGravityField->getSineCoefficients( ).block( 0, 0, 3, 3 );

    vectorParametersIterator->second->setParameterValue( nominalTidalParameter - Eigen::VectorXd::Constant( nominalTidalParameter.rows( ), 1.0 ) );
    earthGravityField->update( testTime );
    Eigen::MatrixXd downperturbedCosineCoefficients =
            earthGravityField->getCosineCoefficients( ).block( 0, 0, 3, 3 );
    Eigen::MatrixXd downperturbedSineCoefficients =
            earthGravityField->getSineCoefficients( ).block( 0, 0, 3, 3 );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtVehiclePosition, partialWrtVehiclePosition, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtVehicleVelocity, partialWrtVehicleVelocity, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthPosition, partialWrtEarthPosition, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthVelocity, partialWrtEarthVelocity, 1.0E-6 );

    //    // Compare numerical and analytical results.
    for( int index = 1; index < 4; index++ )
    {
        Eigen::Vector3d numericalChangeInAcceleration = accelerationDeviations.block( 0, index - 1, 3, 1 );
        Eigen::Vector3d analyticalChangeInAcceleration =
                partialWrtEarthOrientation.block( 0, 0, 3, 1 ) * appliedQuaternionPerturbation[ index ]( 0 ) +
                partialWrtEarthOrientation.block( 0, index, 3, 1 ) * appliedQuaternionPerturbation[ index ]( index );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalChangeInAcceleration,
                                           analyticalChangeInAcceleration, 1.0E-6 );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthGravitationalParameter, partialWrtEarthGravitationalParameter, 1.0E-12 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthRotationRate, partialWrtEarthRotationRate, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPosition, partialWrtPolePosition, 1.0E-6 );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtCosineCoefficients, partialWrtCosineCoefficients, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSineCoefficients, partialWrtSineCoefficients, 1.0E-6 );

    BOOST_CHECK_EQUAL( testPartialWrtCosineCoefficients.cols( ), 17 );
    BOOST_CHECK_EQUAL( testPartialWrtSineCoefficients.cols( ), 13 );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtDegreeTwoLoveNumberAtSeparateOrders, testPartialWrtDegreeTwoOrderTwoLoveNumberAtSeparateOrders, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtComplexDegreeTwoLoveNumber, testPartialWrtComplexDegreeTwoLoveNumber, 1.0E-6 );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtDegreeThreeLoveNumber, testPartialWrtDegreeThreeLoveNumber, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtComplexDegreeThreeLoveNumberAtSeparateOrder, testPartialWrtComplexDegreeThreeLoveNumberAtSeparateOrder, 1.0E-6 );

}

//! Unit test to check working onf spherical harmonic state partial for synchronously rotating body (and rotation depending on state)
BOOST_AUTO_TEST_CASE( testSphericalHarmonicAccelerationPartialWithSynchronousRotation )
{
    // Define bodies in simulation
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Earth" );
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Moon" );

    // Create bodies needed in simulation
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames );
    bodySettings[ "Earth" ]->rotationModelSettings =
            std::make_shared< SynchronousRotationModelSettings >(
                "Moon", "ECLIPJ2000", "IAU_Earth" );
    NamedBodyMap bodyMap = createBodies( bodySettings );
    std::shared_ptr< tudat::simulation_setup::Body > earth = bodyMap.at( "Earth" );
    std::shared_ptr< tudat::simulation_setup::Body > moon = bodyMap.at( "Moon" );
    std::dynamic_pointer_cast< tudat::ephemerides::SynchronousRotationalEphemeris >(
                earth->getRotationalEphemeris( ) )->setIsBodyInPropagation( 1 );
    setGlobalFrameBodyEphemerides( bodyMap, "Earth", "ECLIPJ2000" );

    // Set translational and rotational state of bodies
    double testTime = 1.0E6;
    earth->setStateFromEphemeris( testTime );
    Eigen::Vector6d moonState = moon->getStateInBaseFrameFromEphemeris( testTime );
    moon->setState( moonState * 0.1 );

    earth->setCurrentRotationToLocalFrameFromEphemeris( testTime );
    moon->setCurrentRotationToLocalFrameFromEphemeris( testTime );

    // Create acceleration model
    std::shared_ptr< SphericalHarmonicAccelerationSettings > accelerationSettings =
            std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 );
    std::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > gravitationalAcceleration =
            std::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >(
                createAccelerationModel( moon, earth, accelerationSettings, "Moon", "Earth" ) );
    gravitationalAcceleration->updateMembers( 0.0 );

    // Declare numerical partials.
    Eigen::Matrix3d testPartialWrtEarthPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtEarthVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtMoonPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtMoonVelocity = Eigen::Matrix3d::Zero( );

    // Declare perturbations in position for numerical partial/
    Eigen::Vector3d positionPerturbation;
    positionPerturbation << 1000.0, 1000.0, 1000.0;
    Eigen::Vector3d velocityPerturbation;
    velocityPerturbation << 1.0, 1.0E-1, 1.0;

    // Create state access/modification functions for bodies.
    std::function< void( Eigen::Vector6d ) > earthStateSetFunction =
            std::bind( &Body::setState, earth, std::placeholders::_1  );
    std::function< void( Eigen::Vector6d ) > moonStateSetFunction =
            std::bind( &Body::setState, moon, std::placeholders::_1  );
    std::function< Eigen::Vector6d ( ) > earthStateGetFunction =
            std::bind( &Body::getState, earth );
    std::function< Eigen::Vector6d ( ) > moonStateGetFunction =
            std::bind( &Body::getState, moon );

    // Define estimated parameters
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back( std::make_shared< InitialRotationalStateEstimatableParameterSettings< double > >(
                                  "Earth", 0.0, "ECLIPJ2000" ) );
    parameterNames.push_back( std::make_shared< InitialRotationalStateEstimatableParameterSettings< double > >(
                                  "Moon", 0.0, "ECLIPJ2000" ) );
    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parameterSet =
            createParametersToEstimate( parameterNames, bodyMap );

    // Create acceleration partial object.
    std::shared_ptr< SphericalHarmonicsGravityPartial > accelerationPartial =
            std::dynamic_pointer_cast< SphericalHarmonicsGravityPartial > (
                createAnalyticalAccelerationPartial(
                    gravitationalAcceleration, std::make_pair( "Moon", moon ), std::make_pair( "Earth", earth ),
                    bodyMap, parameterSet ) );
    accelerationPartial->update( testTime );

    // Calculate analytical partials.
    Eigen::MatrixXd partialWrtMoonPosition = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtPositionOfAcceleratedBody( partialWrtMoonPosition.block( 0, 0, 3, 3 ) );

    Eigen::MatrixXd partialWrtMoonVelocity = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtVelocityOfAcceleratedBody( partialWrtMoonVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );

    Eigen::MatrixXd partialWrtEarthPosition = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtPositionOfAcceleratingBody( partialWrtEarthPosition.block( 0, 0, 3, 3 ) );

    Eigen::MatrixXd partialWrtEarthVelocity = Eigen::Matrix3d::Zero( );
    accelerationPartial->wrtVelocityOfAcceleratingBody( partialWrtEarthVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );

    // Calculate numerical partials.
    std::function< void( ) > updateFunction =
            std::bind( &Body::setCurrentRotationToLocalFrameFromEphemeris, bodyMap.at( "Earth" ), testTime );

    testPartialWrtMoonPosition = calculateAccelerationWrtStatePartials(
                moonStateSetFunction, gravitationalAcceleration, moon->getState( ), positionPerturbation, 0,
                updateFunction );
    testPartialWrtMoonVelocity = calculateAccelerationWrtStatePartials(
                moonStateSetFunction, gravitationalAcceleration, moon->getState( ), velocityPerturbation, 3,
                updateFunction );

    testPartialWrtEarthPosition = calculateAccelerationWrtStatePartials(
                earthStateSetFunction, gravitationalAcceleration, earth->getState( ), positionPerturbation, 0,
                updateFunction );
    testPartialWrtEarthVelocity = calculateAccelerationWrtStatePartials(
                earthStateSetFunction, gravitationalAcceleration, earth->getState( ), velocityPerturbation, 3,
                updateFunction );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMoonPosition, partialWrtMoonPosition, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMoonVelocity, partialWrtMoonVelocity, 1.0E-4 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthPosition, partialWrtEarthPosition, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthVelocity, partialWrtEarthVelocity, 1.0E-4 );

}
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


// Copyright (C) 2001-2003
// William E. Kempf
//
//  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/thread/thread.hpp>
#include <boost/ref.hpp>
#include <iostream>

class factorial
{
public:
    factorial(int x) : x(x), res(0) { }
    void operator()() { res = calculate(x); }
    int result() const { return res; }

private:
    int calculate(int x) { return x <= 1 ? 1 : x * calculate(x-1); }

private:
    int x;
    int res;
};

int main()
{
    factorial f(10);
    boost::thread thrd(boost::ref(f));
    thrd.join();
    std::cout << "10! = " << f.result() << std::endl;
}


#pragma once
#pragma clang diagnostic ignored "-Woverloaded-shift-op-parentheses"
#include "doctest.h"
using doctest::Approx;
#include <armadillo>

bool equal(const arma::vec& v1, const arma::vec& v2, const double tolerance = 1e-9);

bool equal(const arma::mat& v1, const arma::mat& v2, const double tolerance = 1e-9);


/*
 * GreedyPacker.cpp
 *
 *  Created on: Nov 2, 2011
 *      Author: dkoes
 */

#include "GreedyPacker.h"
#include <boost/multi_array.hpp>

using namespace lemon;

typedef bool (*ICDComp)(const IntraClusterDist& lhs,
		const IntraClusterDist& rhs);
static bool reverseDist(const IntraClusterDist& lhs,
		const IntraClusterDist& rhs)
{
	return lhs.dist > rhs.dist;
}

//a priority q for distances, roll our own to support more efficient
//removal of clusters and re-use of distances array
class DistancePQ
{
	vector<IntraClusterDist>& Q;

public:
	DistancePQ(vector<IntraClusterDist>& q) :
			Q(q)
	{
		make_heap(Q.begin(), Q.end(), reverseDist);
	}

	bool empty() const
	{
		return Q.size() == 0;
	}

	const IntraClusterDist& top() const
	{
		return Q[0];
	}

	void push(const IntraClusterDist& c)
	{
		Q.push_back(c);
		push_heap(Q.begin(), Q.end(), reverseDist);
	}

	void pop()
	{
		pop_heap(Q.begin(), Q.end(), reverseDist);
		Q.pop_back();
	}

	//remove all references to clusters i and j; this is linear
	void removeIJ(unsigned i, unsigned j)
	{
		unsigned c = 0;
		unsigned end = Q.size();
		while (c < end)
		{
			if (Q[c].i != i && Q[c].i != j && Q[c].j != i && Q[c].j != j)
			{
				c++;
			}
			else
			{
				end--;
				swap(Q[c], Q[end]);
			}
		}

		Q.erase(Q.begin() + end, Q.end());
		make_heap(Q.begin(), Q.end(), reverseDist);
	}

};

//keep track of what clusters are connected to what
class ClusterConnections
{
	vector< vector<unsigned> > table;
public:

	//assumes distances are distinct edges
	ClusterConnections(const vector<IntraClusterDist>& distances)
	{
		for(unsigned i = 0, n = distances.size(); i < n; i++)
		{
			unsigned I = distances[i].i;
			unsigned J = distances[i].j;

			unsigned m = max(I,J);
			if(table.size() <= m)
				table.resize(m+1);

			table[I].push_back(J);
			table[J].push_back(I);
		}

		for(unsigned i = 0, n = table.size(); i < n; i++)
		{
			sort(table[i].begin(), table[i].end());
		}
	}

	//return all clusters connected to c
	const vector<unsigned>& neighbors(unsigned c) const
	{
		assert(c < table.size());
		return table[c];
	}

	//merge i and j into a new cluster c
	//i and j go away, c is connected to everything they are connected to
	void merge(unsigned i, unsigned j, unsigned c)
	{
		if(table.size() <= c)
			table.resize(c+1);

		table[c].insert(table[c].end(), table[i].begin(), table[i].end());
		table[c].insert(table[c].end(), table[j].begin(), table[j].end());

		table[i].clear();
		table[j].clear();

		//uniquify
		sort(table[c].begin(), table[c].end());
		vector<unsigned>::iterator newend = unique(table[c].begin(), table[c].end());
		table[c].erase(newend, table[c].end());
	}
};

void GreedyPacker::pack(const DataViewer* dv,
		vector<Cluster>& finalclusters) const
{
	if (dv->size() == 0)
		return;

	vector<unsigned> indices(dv->size());
	for (unsigned i = 0, n = indices.size(); i < n; i++)
		indices[i] = i;

	//start with each object in its own cluster
	unsigned N = indices.size();
	vector<Cluster> clusters;
	clusters.reserve(N * 2);
	clusters.resize(N);
	for (unsigned i = 0, n = indices.size(); i < n; i++)
	{
		unsigned index = indices[i];
		clusters[i].setToSingleton(index, dv->getMIV(index), dv->getMSV(index));
	}

	vector<IntraClusterDist> distances;
	DCache *dcacheptr = NULL;

	computeDistanceVector(dv, clusters, distances, dcacheptr);

	DistancePQ pQ(distances); //manipulates a reference of distances

	//keep track of what clusters are connected for the knn case
	//this isn't really necessary for non-KNN, but that's a curiousity anyway
	ClusterConnections connections(distances);

	unsigned max = packSize;
	while (!pQ.empty())
	{
		//get the smallest distance
		unsigned i = pQ.top().i;
		unsigned j = pQ.top().j;
		pQ.pop();

		if (clusters[i].isValid() && clusters[j].isValid())
		{
			if (clusters[i].size() + clusters[j].size() <= max)
			{
				unsigned c = clusters.size();
				//merge, create a new cluster
				clusters.push_back(Cluster());
				clusters.back().mergeInto(clusters[i], clusters[j]);

				connections.merge(i, j, c);
				pQ.removeIJ(i, j);
				//now compute the distance between this cluster and all remaining clusters
				//and add to priority Q
				const vector<unsigned>& neighs = connections.neighbors(c);
				for (unsigned nbr = 0, n = neighs.size(); nbr < n; nbr++)
				{
					unsigned d = neighs[nbr];
					if (clusters[d].isValid())
					{
						float dist = clusterDistance(dv, clusters.back(),
								clusters[d], *dcacheptr);
						pQ.push(IntraClusterDist(c, d, dist));
					}
				}
			}
		}
	}

	delete dcacheptr;
	dcacheptr = NULL;

	//remove empty clusters
	finalclusters.clear();
	finalclusters.reserve(ceil(N / (double) packSize));
	for (unsigned i = 0, n = clusters.size(); i < n; i++)
	{
		if (clusters[i].isValid())
		{
			finalclusters.push_back(Cluster());
			finalclusters.back().moveInto(clusters[i]);
		}
	}
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_NEG_BINOMIAL_2_LOG_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_NEG_BINOMIAL_2_LOG_RNG_HPP

#include <boost/math/special_functions/digamma.hpp>
#include <boost/random/negative_binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_less.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/prob/gamma_rng.hpp>
#include <stan/math/prim/scal/prob/poisson_rng.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>

namespace stan {
namespace math {

template <class RNG>
inline int neg_binomial_2_log_rng(double eta, double phi, RNG& rng) {
  using boost::gamma_distribution;
  using boost::random::negative_binomial_distribution;
  using boost::random::poisson_distribution;
  using boost::variate_generator;

  static const char* function = "neg_binomial_2_log_rng";

  check_finite(function, "Log-location parameter", eta);
  check_positive_finite(function, "Precision parameter", phi);

  double exp_eta_div_phi = std::exp(eta) / phi;

  // gamma_rng params must be positive and finite
  check_positive_finite(function,
                        "Exponential of the log-location parameter "
                        "divided by the precision parameter",
                        exp_eta_div_phi);

  double rng_from_gamma = variate_generator<RNG&, gamma_distribution<> >(
      rng, gamma_distribution<>(phi, exp_eta_div_phi))();

  // same as the constraints for poisson_rng
  check_less(function, "Random number that came from gamma distribution",
             rng_from_gamma, POISSON_MAX_RATE);
  check_not_nan(function, "Random number that came from gamma distribution",
                rng_from_gamma);
  check_nonnegative(function, "Random number that came from gamma distribution",
                    rng_from_gamma);

  return variate_generator<RNG&, poisson_distribution<> >(
      rng, poisson_distribution<>(rng_from_gamma))();
}

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


//==============================================================================
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/exponential/include/functions/pow.hpp>
#include <nt2/include/functions/rec.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/sdk/meta/as_integer.hpp>

#include <nt2/include/constants/eight.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL(pow, NT2_SIMD_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  typedef typename nt2::meta::call<pow_(vT,vT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(), nt2::Inf<vT>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<vT>(), nt2::Minf<vT>()), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(), nt2::Minf<vT>()), nt2::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<vT>(), nt2::Nan<vT>()), nt2::Nan<r_t>(), 0);
#endif
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<vT>(), nt2::Mone<vT>()), nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<vT>(), nt2::One<vT>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<vT>(), nt2::Zero<vT>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::splat<vT>(-1),nt2::splat<vT>(5)), nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::splat<vT>(-1),nt2::splat<vT>(6)), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<vT>(), nt2::One<vT>()), nt2::Zero<r_t>(), 0);
}

NT2_TEST_CASE_TPL(pow_real_int, NT2_SIMD_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::as_integer<vT>::type          ivT;
  typedef typename nt2::meta::call<pow_(vT,ivT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(),3), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<vT>(),3), nt2::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<vT>(),3), nt2::Nan<r_t>(), 0);
#endif
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(),3), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(),4), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<vT>(),3), nt2::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<vT>(),4), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<vT>(),3), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<vT>(),4), nt2::Nan<r_t>(), 0);
#endif

  NT2_TEST_ULP_EQUAL(pow(nt2::Two<vT>(),-3), nt2::rec(nt2::Eight<r_t>()),  0);
}

NT2_TEST_CASE_TPL(pow_int, NT2_SIMD_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<pow_(vT,iT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<vT>(),3),nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<vT>(),3), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Two <vT>(),3),nt2::Eight<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<vT>(),0), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<vT>(),3), nt2::Zero<r_t>(), 0);

  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<vT>(),4), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<vT>(),4), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<vT>(),4), nt2::Zero<r_t>(), 0);
}

NT2_TEST_CASE_TPL(pow_real_int_vector, NT2_SIMD_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::as_integer<vT>::type          ivT;
  typedef typename nt2::meta::call<pow_(vT,ivT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(),nt2::Three<ivT>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<vT>(),nt2::Three<ivT>()), nt2::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<vT>(),nt2::Three<ivT>()), nt2::Nan<r_t>(), 0);
#endif
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(),nt2::Three<ivT>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<vT>(),nt2::Four<ivT>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<vT>(),nt2::Three<ivT>()), nt2::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<vT>(),nt2::Four<ivT>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<vT>(),nt2::Three<ivT>()), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<vT>(),nt2::Four<ivT>()), nt2::Nan<r_t>(), 0);
#endif

  NT2_TEST_ULP_EQUAL(pow(nt2::Two<vT>(),-nt2::Three<vT>()), nt2::rec(nt2::Eight<r_t>()),  0);
}

NT2_TEST_CASE_TPL(pow_int_vector, NT2_SIMD_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::as_integer<vT>::type          ivT;
  typedef typename nt2::meta::call<pow_(vT,ivT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(pow<3>(nt2::Mone<vT>()), nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow<3>(nt2::One<vT>() ), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow<3>(nt2::Two <vT>()), nt2::Eight<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow<0>(nt2::Zero<vT>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow<3>(nt2::Zero<vT>()), nt2::Zero<r_t>(), 0);

  NT2_TEST_ULP_EQUAL(pow<4>(nt2::Mone<vT>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow<4>(nt2::One<vT>() ), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow<4>(nt2::Zero<vT>()), nt2::Zero<r_t>(), 0);
}

NT2_TEST_CASE_TPL(pow_negint_vector, NT2_SIMD_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::as_integer<vT>::type          ivT;
  typedef typename nt2::meta::call<pow_(vT,ivT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  NT2_TEST_ULP_EQUAL(pow<-3>(nt2::Two<vT>()), nt2::rec(nt2::Eight<r_t>()),  0);
}


#include <iostream>
#include "atkui/framework.h"
#include "atkmath/matrix3.h"
#include <Eigen/Dense>
using namespace glm;

class Unique : public atkui::Framework
{
public:
   Unique() : atkui::Framework(atkui::Orthographic)
   {
   }
   virtual void setup()
   {
      _mouseX = width() * 0.5;
      _mouseY = height() * 0.5;
      movingTarget = false;

      for (int i = 5; i < width(); i += 10)
      {
         for (int j = 0; j < height(); j += 15)
         {
            cuboidPositions.push_back(vec3(i, j, 0));
         }
      }
   }

   virtual void scene()
   {
      for (int i = 0; i < cuboidPositions.size(); i += 1)
      {
         setColor(vec3(0.5, 0.8, 0.5));
         vec3 cuboid = cuboidPositions[i];
         if (movingTarget && pow(cuboidPositions[i][0] - _mouseX, 2) + pow(cuboidPositions[i][1] - _mouseY, 2) <= pow(100, 2))
         {
            float theta = atan2(_mouseY - cuboid[1], _mouseX - cuboid[0]);
            push();
            translate(cuboid);
            rotate(theta, vec3(0, 0, 1));
            drawCube(vec3(0), vec3(10, 1, 0));
            pop();
         }
         else
         {
            drawCube(cuboid, vec3(10, 1, 0));
         }
      }

      vec3 target = vec3(_mouseX, _mouseY, 0);
      setColor(vec3(1, 0, 0));
      drawSphere(target, targetRadius);
   }

   void mouseMotion(int pX, int pY, int dx, int dy)
   {
      if (movingTarget)
      {
         _mouseX = pX;
         _mouseY = height() - pY;
      }
   }

   void mouseDown(int button, int mods)
   {
      vec2 currentPosition = this->mousePosition();
      if (button == GLFW_MOUSE_BUTTON_LEFT && length(currentPosition - vec2(_mouseX, height() - _mouseY)) <= targetRadius)
      {
         movingTarget = true;
      }
   }

   void mouseUp(int button, int mods)
   {
      movingTarget = false;
   }

private:
   int _mouseX;
   int _mouseY;
   bool movingTarget; // whether the target is being moved
   std::vector<glm::vec3> cuboidPositions;
   const double targetRadius = 10;
};

int main(int argc, char **argv)
{
   Unique viewer;
   viewer.run();
}


#include "Demos/Utils/Config.h"
#include "GL/glew.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Utils/TimeManager.h"
#include <Eigen/Dense>
#include "FluidModel.h"
#include "TimeStepFluidModel.h"
#include <iostream>

// Enable memory leak detection
#ifdef _DEBUG
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void createBreakingDam();
void addWall(const Eigen::Vector3f &minX, const Eigen::Vector3f &maxX, std::vector<Eigen::Vector3f> &boundaryParticles);
void initBoundaryData(std::vector<Eigen::Vector3f> &boundaryParticles);
void render ();
void cleanup();
void reset();
void hsvToRgb(float h, float s, float v, float *rgb);
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);
void createSphereBuffers(float radius, int resolution);
void renderSphere(const Eigen::Vector3f &x, const float color[]);
void releaseSphereBuffers();
void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);
void TW_CALL setViscosity(const void *value, void *clientData);
void TW_CALL getViscosity(void *value, void *clientData);

FluidModel model;
TimeStepFluidModel simulation;

const float particleRadius = 0.025f;
const unsigned int width = 15;
const unsigned int depth = 15;
const unsigned int height = 20;
const float containerWidth = (width + 1)*particleRadius*2.0f * 5.0f;
const float containerDepth = (depth + 1)*particleRadius*2.0f;
const float containerHeight = 4.0f;
bool doPause = true;
std::vector<unsigned int> selectedParticles;
Eigen::Vector3f oldMousePos;
// initiate buffers
GLuint elementbuffer;
GLuint normalbuffer;
GLuint vertexbuffer;
int vertexBufferSize = 0;
GLint context_major_version, context_minor_version;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	// OpenGL
	MiniGL::init (argc, argv, 1024, 768, 0, 0, "Fluid demo");
	MiniGL::initLights ();
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setSelectionFunc(selection);

	MiniGL::getOpenGLVersion(context_major_version, context_minor_version);

	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3f (0.0, -3.0, 8.0), Vector3f (0.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_FLOAT, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &simulation, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "Viscosity", TW_TYPE_FLOAT, setViscosity, getViscosity, &model, " label='Viscosity'  min=0.0 max = 0.5 step=0.001 precision=4 group=Simulation ");

	buildModel();

	if (context_major_version >= 3)
		createSphereBuffers((float)particleRadius, 8);

	glutMainLoop ();

	cleanup ();

	return 0;
}

void cleanup()
{
	delete TimeManager::getCurrent();
	if (context_major_version >= 3)
		releaseSphereBuffers();
}

void reset()
{
	model.reset();
	simulation.reset(model);
	TimeManager::getCurrent()->setTime(0.0);
}

void mouseMove(int x, int y)
{
	Eigen::Vector3f mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Eigen::Vector3f diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const float h = tm->getTimeStepSize();

	ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		pd.getVelocity(selectedParticles[j]) += 5.0*diff/h;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
	std::vector<unsigned int> hits;
	selectedParticles.clear();
	ParticleData &pd = model.getParticles();
	Selection::selectRect(start, end, &pd.getPosition(0), &pd.getPosition(pd.size() - 1), selectedParticles);
	if (selectedParticles.size() > 0)
		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
	else
		MiniGL::setMouseMoveFunc(-1, NULL);

	MiniGL::unproject(end[0], end[1], oldMousePos);
}

void timeStep ()
{
	if (doPause)
		return;

	// Simulation code
	for (unsigned int i = 0; i < 4; i++)
		simulation.step(model);
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (0.0025f);

	createBreakingDam();
}

void hsvToRgb(float h, float s, float v, float *rgb)
{
	int i = (int)floor(h * 6);
	float f = h * 6 - i;
	float p = v * (1 - s);
	float q = v * (1 - f * s);
	float t = v * (1 - (1 - f) * s);

	switch (i % 6)
	{
	case 0: rgb[0] = v, rgb[1] = t, rgb[2] = p; break;
	case 1: rgb[0] = q, rgb[1] = v, rgb[2] = p; break;
	case 2: rgb[0] = p, rgb[1] = v, rgb[2] = t; break;
	case 3: rgb[0] = p, rgb[1] = q, rgb[2] = v; break;
	case 4: rgb[0] = t, rgb[1] = p, rgb[2] = v; break;
	case 5: rgb[0] = v, rgb[1] = p, rgb[2] = q; break;
	}
}

void render ()
{
	MiniGL::coordinateSystem();

	// Draw simulation model

	const ParticleData &pd = model.getParticles();
	const unsigned int nParticles = pd.size();

	float surfaceColor[4] = { 0.2f, 0.6f, 0.8f, 1 };
	float speccolor[4] = { 1.0, 1.0, 1.0, 1.0 };
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, surfaceColor);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, surfaceColor);
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, speccolor);
	glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 100.0);
	glColor3fv(surfaceColor);

	glPointSize(4.0f);

	const float supportRadius = model.getSupportRadius();
	float vmax = 0.4f*2.0f*supportRadius / TimeManager::getCurrent()->getTimeStepSize();
	float vmin = 0.0f;

	if (context_major_version > 3)
	{
		for (unsigned int i = 0; i < nParticles; i++)
		{
			float v = pd.getVelocity(i).norm();
			v = 0.5f*((v - vmin) / (vmax - vmin));
			v = min(128.0f*v*v, 0.5f);
			float fluidColor[4] = { 0.2f, 0.2f, 0.2f, 1.0f };
			hsvToRgb(0.55f, 1.0f, 0.5f + (float)v, fluidColor);
			renderSphere(pd.getPosition(i), fluidColor);
		}

// 		for (unsigned int i = 0; i < model.numBoundaryParticles(); i++)
// 			renderSphere(model.getBoundaryX(i), surfaceColor);
	}
	else
	{
		glDisable(GL_LIGHTING);
		glBegin(GL_POINTS);
		for (unsigned int i = 0; i < nParticles; i++)
		{
			float v = pd.getVelocity(i).norm();
			v = 0.5f*((v - vmin) / (vmax - vmin));
			v = min(128.0f*v*v, 0.5f);
			float fluidColor[4] = { 0.2f, 0.2f, 0.2f, 1.0f };
			hsvToRgb(0.55f, 1.0f, 0.5f + (float)v, fluidColor);

			glColor3fv(fluidColor);
			glVertex3fv(&pd.getPosition(i)[0]);
		}
		glEnd();

		// 	glBegin(GL_POINTS);
		// 	for (unsigned int i = 0; i < model.numBoundaryParticles(); i++)
		// 	{
		// 		glColor3fv(surfaceColor);
		// 		glVertex3fv(&model.getBoundaryX(i)[0]);
		// 	}
		// 	glEnd();

		glEnable(GL_LIGHTING);
	}

	float red[4] = { 0.8f, 0.0f, 0.0f, 1 };
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		MiniGL::drawSphere(pd.getPosition(selectedParticles[j]), 0.08f, red);
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

/** Create a breaking dam scenario
*/
void createBreakingDam()
{
	std::cout << "Initialize fluid particles\n";
	const float diam = 2.0f*particleRadius;
	const float startX = -0.5f*containerWidth + diam;
	const float startY = diam;
	const float startZ = -0.5f*containerDepth + diam;
	const float yshift = sqrt(3.0f) * particleRadius;

	std::vector<Eigen::Vector3f> fluidParticles;
	fluidParticles.resize(width*height*depth);

	#pragma omp parallel default(shared)
	{
		#pragma omp for schedule(static)
		for (int i = 0; i < (int)width; i++)
		{
			for (unsigned int j = 0; j < height; j++)
			{
				for (unsigned int k = 0; k < depth; k++)
				{
					fluidParticles[i*height*depth + j*depth + k] = diam*Eigen::Vector3f((float)i, (float)j, (float)k) + Eigen::Vector3f(startX, startY, startZ);
				}
			}
		}
	}

	model.setParticleRadius(particleRadius);

	std::vector<Eigen::Vector3f> boundaryParticles;
	initBoundaryData(boundaryParticles);

	model.initModel((unsigned int)fluidParticles.size(), fluidParticles.data(), (unsigned int)boundaryParticles.size(), boundaryParticles.data());

	std::cout << "Number of particles: " << width*height*depth << "\n";
}

void addWall(const Eigen::Vector3f &minX, const Eigen::Vector3f &maxX, std::vector<Eigen::Vector3f> &boundaryParticles)
{
	const float particleDistance = 2.0f*model.getParticleRadius();

	const Eigen::Vector3f diff = maxX - minX;
	const unsigned int stepsX = (unsigned int)(diff[0] / particleDistance) + 1u;
	const unsigned int stepsY = (unsigned int)(diff[1] / particleDistance) + 1u;
	const unsigned int stepsZ = (unsigned int)(diff[2] / particleDistance) + 1u;

	const unsigned int startIndex = (unsigned int) boundaryParticles.size();
	boundaryParticles.resize(startIndex + stepsX*stepsY*stepsZ);

	#pragma omp parallel default(shared)
	{
		#pragma omp for schedule(static)
		for (int j = 0; j < (int)stepsX; j++)
		{
			for (unsigned int k = 0; k < stepsY; k++)
			{
				for (unsigned int l = 0; l < stepsZ; l++)
				{
					const Eigen::Vector3f currPos = minX + Eigen::Vector3f(j*particleDistance, k*particleDistance, l*particleDistance);
					boundaryParticles[startIndex + j*stepsY*stepsZ + k*stepsZ + l] = currPos;
				}
			}
		}
	}
}

void initBoundaryData(std::vector<Eigen::Vector3f> &boundaryParticles)
{
	const float x1 = -containerWidth / 2.0f;
	const float x2 = containerWidth / 2.0f;
	const float y1 = 0.0f;
	const float y2 = containerHeight;
	const float z1 = -containerDepth / 2.0f;
	const float z2 = containerDepth / 2.0f;

	const float diam = 2.0f*particleRadius;

	// Floor
	addWall(Eigen::Vector3f(x1, y1, z1), Eigen::Vector3f(x2, y1, z2), boundaryParticles);
	// Top
	addWall(Eigen::Vector3f(x1, y2, z1), Eigen::Vector3f(x2, y2, z2), boundaryParticles);
	// Left
	addWall(Eigen::Vector3f(x1, y1, z1), Eigen::Vector3f(x1, y2, z2), boundaryParticles);
	// Right
	addWall(Eigen::Vector3f(x2, y1, z1), Eigen::Vector3f(x2, y2, z2), boundaryParticles);
	// Back
	addWall(Eigen::Vector3f(x1, y1, z1), Eigen::Vector3f(x2, y2, z1), boundaryParticles);
	// Front
	addWall(Eigen::Vector3f(x1, y1, z2), Eigen::Vector3f(x2, y2, z2), boundaryParticles);
}

void createSphereBuffers(float radius, int resolution)
{
	float PI = static_cast<float>(M_PI);
	// vectors to hold our data
	// vertice positions
	std::vector<Eigen::Vector3f> v;
	// normals
	std::vector<Eigen::Vector3f> n;
	std::vector<unsigned short> indices;

	// initiate the variable we are going to use
	float X1, Y1, X2, Y2, Z1, Z2;
	float inc1, inc2, inc3, inc4, radius1, radius2;

	for (int w = 0; w < resolution; w++)
	{
		for (int h = (-resolution / 2); h < (resolution / 2); h++)
		{
			inc1 = (w / (float)resolution) * 2 * PI;
			inc2 = ((w + 1) / (float)resolution) * 2 * PI;
			inc3 = (h / (float)resolution)*PI;
			inc4 = ((h + 1) / (float)resolution)*PI;

			X1 = sin(inc1);
			Y1 = cos(inc1);
			X2 = sin(inc2);
			Y2 = cos(inc2);

			// store the upper and lower radius, remember everything is going to be drawn as triangles
			radius1 = radius*cos(inc3);
			radius2 = radius*cos(inc4);

			Z1 = radius*sin(inc3);
			Z2 = radius*sin(inc4);

			// insert the triangle coordinates
			v.push_back(Eigen::Vector3f(radius1*X1, Z1, radius1*Y1));
			v.push_back(Eigen::Vector3f(radius1*X2, Z1, radius1*Y2));
			v.push_back(Eigen::Vector3f(radius2*X2, Z2, radius2*Y2));

			indices.push_back((unsigned short)v.size() - 3);
			indices.push_back((unsigned short)v.size() - 2);
			indices.push_back((unsigned short)v.size() - 1);

			v.push_back(Eigen::Vector3f(radius1*X1, Z1, radius1*Y1));
			v.push_back(Eigen::Vector3f(radius2*X2, Z2, radius2*Y2));
			v.push_back(Eigen::Vector3f(radius2*X1, Z2, radius2*Y1));

			indices.push_back((unsigned short)v.size() - 3);
			indices.push_back((unsigned short)v.size() - 2);
			indices.push_back((unsigned short)v.size() - 1);

			// insert the normal data
			n.push_back(Eigen::Vector3f(X1, Z1, Y1));
			n.push_back(Eigen::Vector3f(X2, Z1, Y2));
			n.push_back(Eigen::Vector3f(X2, Z2, Y2));
			n.push_back(Eigen::Vector3f(X1, Z1, Y1));
			n.push_back(Eigen::Vector3f(X2, Z2, Y2));
			n.push_back(Eigen::Vector3f(X1, Z2, Y1));
		}
	}

	for (unsigned int i = 0; i < n.size(); i++)
		n[i].normalize();

	glGenBuffersARB(1, &vertexbuffer);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, vertexbuffer);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, v.size() * sizeof(Eigen::Vector3f), &v[0], GL_STATIC_DRAW);

	glGenBuffersARB(1, &normalbuffer);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, normalbuffer);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, n.size() * sizeof(Eigen::Vector3f), &n[0], GL_STATIC_DRAW);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	// Generate a buffer for the indices as well
	glGenBuffersARB(1, &elementbuffer);
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, elementbuffer);
	glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB, indices.size() * sizeof(unsigned short), &indices[0], GL_STATIC_DRAW);
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0);

	// store the number of indices for later use
	vertexBufferSize = (unsigned int)indices.size();

	// clean up after us
	indices.clear();
	n.clear();
	v.clear();
}

void renderSphere(const Eigen::Vector3f &x, const float color[])
{
	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);

	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, vertexbuffer);
	glVertexPointer(3, GL_FLOAT, 0, 0);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, normalbuffer);
	glNormalPointer(GL_FLOAT, 0, 0);

	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, elementbuffer);

	glPushMatrix();
	glTranslated(x[0], x[1], x[2]);
	glDrawElements(GL_TRIANGLES, (GLsizei)vertexBufferSize, GL_UNSIGNED_SHORT, 0);
	glPopMatrix();
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_VERTEX_ARRAY);
}

void releaseSphereBuffers()
{
	if (elementbuffer != 0)
	{
		glDeleteBuffersARB(1, &elementbuffer);
		elementbuffer = 0;
	}
	if (normalbuffer != 0)
	{
		glDeleteBuffersARB(1, &normalbuffer);
		normalbuffer = 0;
	}
	if (vertexbuffer != 0)
	{
		glDeleteBuffersARB(1, &vertexbuffer);
		vertexbuffer = 0;
	}
}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(float *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepFluidModel*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepFluidModel*)clientData)->getVelocityUpdateMethod();
}

void TW_CALL setViscosity(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	((FluidModel*)clientData)->setViscosity(val);
}

void TW_CALL getViscosity(void *value, void *clientData)
{
	*(float *)(value) = ((FluidModel*)clientData)->getViscosity();
}


/*

 [begin_description]
 Test case for issue 147
 [end_description]

 Copyright 2011-2015 Karsten Ahnert
 Copyright 2011-2015 Mario Mulansky

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

// disable checked iterator warning for msvc

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_regression_147

#include <utility>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef double state_type;

void rhs( const state_type &x , state_type &dxdt , const double t )
{
    dxdt = 1;
}

template<class Stepper, class InitStepper>
struct perform_init_test
{
    void operator()( void )
    {
        double t = 0;
        const double dt = 0.1;

        state_type x = 0;

        Stepper stepper;
        InitStepper init_stepper;
        stepper.initialize( init_stepper, rhs, x, t, dt );

        // ab-stepper needs order-1 init steps: t and x should be (order-1)*dt
        BOOST_CHECK_CLOSE( t , (stepper.order()-1)*dt , 1E-16 );
        BOOST_CHECK_CLOSE( x, ( stepper.order() - 1 ) * dt, 2E-14 );
    }
};

typedef mpl::vector<
    euler< state_type > ,
    modified_midpoint< state_type > ,
    runge_kutta4< state_type > ,
    runge_kutta4_classic< state_type > ,
    runge_kutta_cash_karp54_classic< state_type > ,
    runge_kutta_cash_karp54< state_type > ,
    runge_kutta_dopri5< state_type > ,
    runge_kutta_fehlberg78< state_type >
    > runge_kutta_steppers;

BOOST_AUTO_TEST_SUITE( regression_147_test )

BOOST_AUTO_TEST_CASE_TEMPLATE( init_test , InitStepper,
                               runge_kutta_steppers )
{
    perform_init_test< adams_bashforth<4, state_type>, InitStepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "DisksViewer.hpp"

//#include <Siconos/io/Register.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>

// FMatrix is forwarded
#include <SpaceFilter_impl.hpp>

using namespace qglviewer;

void DisksViewer::init()
{

  // siconos setup
  Siconos_.reset(new Disks());

  BodiesViewer::init();

  // 2D setup
  glDisable(GL_LIGHTING);
  camera()->setPosition(Vec(0.0, 0.0, 1.0));
  camera()->lookAt(sceneCenter());
  camera()->setType(Camera::ORTHOGRAPHIC);
  camera()->showEntireScene();
  constraint_.reset(new WorldConstraint());
  constraint_->setRotationConstraintType(AxisPlaneConstraint::FORBIDDEN);
  camera()->frame()->setConstraint(&*constraint_);

}

void DisksViewer::draw()
{

  int i;

  char qs[6];

  float lbd, w;

  float lbdmax = 0.;

  SP::InteractionsGraph I1;
  SP::Interaction interaction;
  SP::Relation relation;

  if (Siconos_->simulation()->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() > 1)
  {
    I1 = Siconos_->simulation()->indexSet(1);

    // calibration
    InteractionsGraph::VIterator ui, uiend;
    for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui)
    {
      lbdmax = fmax(I1->bundle(*ui)->lambdaOld(1)->getValue(0), lbdmax);
    }

    for (boost::tie(ui, uiend) = I1->vertices(); ui != uiend; ++ui)
    {
      interaction = I1->bundle(*ui);
      relation = interaction->relation();

      lbd = interaction->lambdaOld(1)->getValue(0);

      // screen width of interaction
      w = lbd / (2 * fmax(lbdmax, 1.)) + .03;

      // disk/disk

      SP::DynamicalSystem d1 = I1->properties(*ui).source;
      SP::DynamicalSystem d2 = I1->properties(*ui).target;

      SP::SiconosVector q1 = ask<ForPosition>(*d1);

      float x1 = (*q1)(0);
      float y1 = (*q1)(1);
      float r1 = ask<ForRadius>(*d1);

      if (d1 != d2)
      {
        SP::SiconosVector q2 = ask<ForPosition>(*d2);
        float x2 = (*q2)(0);
        float y2 = (*q2)(1);
        float r2 = ask<ForRadius>(*d2);

        float d = hypotf(x1 - x2, y1 - y2);

        glPushMatrix();

        glColor3f(.0f, .0f, .0f);
        drawRec(x1, y1, x1 + (x2 - x1)*r1 / d, y1 + (y2 - y1)*r1 / d, w);
        drawRec(x2, y2, x2 + (x1 - x2)*r2 / d, y2 + (y1 - y2)*r2 / d, w);

        glPopMatrix();
      }

      else
      {
        SP::SiconosMatrix jachq = ask<ForJachq>(*relation);
        double jx = jachq->getValue(0, 0);
        double jy = jachq->getValue(0, 1);
        double dj = hypot(jx, jy);

        glPushMatrix();

        glColor3f(.0f, .0f, .0f);
        drawRec(x1, y1, x1 - r1 * jx / dj, y1 - r1 * jy / dj, w);
        glPopMatrix();
      }
    }
  }

  for (unsigned int i = 0; i < GETNDS(Siconos_); i++)
  {
    if (shapes_[i]->selected())
    {
      drawSelectedQGLShape(*shapes_[i]);
    }
    else
    {
      drawQGLShape(*shapes_[i]);
    }
  }

  glColor3f(.45, .45, .45);
  glLineWidth(1.);
  drawGrid(100, 200);
  setGridIsDrawn();

  glColor3f(.1, .1, .3);
  drawVec(-100, 0, 100, 0);
  drawVec(0, -100, 0, 100);

  glColor3f(0, 0, 1);
  glLineWidth(4.);

  if (Siconos_->plans())
  {
    for (unsigned int i = 0 ; i < Siconos_->plans()->size(0) ; ++i)
    {
      double A = (*Siconos_->plans())(i, 0);
      double B = (*Siconos_->plans())(i, 1);
      //double C = (*Siconos_->plans())(i,2);
      double xc = (*Siconos_->plans())(i, 3);
      double yc = (*Siconos_->plans())(i, 4);
      double w = fmin(1e10, (*Siconos_->plans())(i, 5));
      double H = hypot(A, B);

      if (w == 0) w = 1e10;

      //      assert ( fabs(A*xc + B*yc + C) <= std::numeric_limits<double>::epsilon() );

      drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H);
      drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H);
    }
  }

  if (Siconos_->movingPlans())
  {
    double time = Siconos_->simulation()->getTk();
    for (unsigned int i = 0 ; i < Siconos_->movingPlans()->size1() ; ++i)
    {
      double A = (*Siconos_->movingPlans())(i, 0)(time);
      double B = (*Siconos_->movingPlans())(i, 1)(time);
      double C = (*Siconos_->movingPlans())(i, 2)(time);
      double w = 1e10;
      double H = hypot(A, B);
      double xc = 0.;
      double yc = 0.;

      if (fabs(C) > std::numeric_limits<double>::epsilon())
      {
        if (A == 0)
          // By+C=0
        {
          yc = -C / B;
        }
        else if (B == 0)
          // Ax+C=0
        {
          xc = -C / A;
        }
        else
          // Ax+By+C=0
        {
          if (xc != 0)
            yc = - (A * xc + C) / B;
          else
            xc = - (B * yc + C) / A;
        }
      }

      drawVec(xc, yc, xc - 0.5 * w * B / H, yc + 0.5 * w * A / H);
      drawVec(xc, yc, xc + 0.5 * w * B / H, yc - 0.5 * w * A / H);
    }
  }
  glColor3f(.1, .1, .1);
  glLineWidth(4.);
  QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(1, 0, .1), .01, 3);
  QGLViewer::drawArrow(qglviewer::Vec(0, 0, .1), qglviewer::Vec(0, 1, .1), .01, 3);

  glLineWidth(1.);
  for (i = -100; i <= 100; i += 5)
  {
    sprintf(qs, "%d", i);
    //    print((float)i,-.8,qs,small_text);
    //print(-.8,(float)i,qs,small_text);
    drawVec((float)i, -.2, (float)i, .2);
    drawVec(-.2, (float)i, .2, (float)i);
  }
  for (i = -100; i <= 100; i++)
  {
    drawVec((float)i, -.1, (float)i, .1);
    drawVec(-.1, (float)i, .1, (float)i);
  }
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/detail/canonical_constant.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/comparable.hpp>
#include <laws/constant.hpp>
#include <laws/euclidean_ring.hpp>
#include <laws/group.hpp>
#include <laws/logical.hpp>
#include <laws/monoid.hpp>
#include <laws/orderable.hpp>
#include <laws/ring.hpp>
namespace hana = boost::hana;

template <typename T, T v>
struct canonical {
    static constexpr T value = v;
    using hana_tag = hana::detail::CanonicalConstant<T>;
};

int main() {
    auto ints = hana::make_tuple(
        canonical<int, -10>{}, canonical<int, -2>{}, canonical<int, 0>{},
        canonical<int, 1>{}, canonical<int, 3>{}, canonical<int, 4>{}
    );

    auto bools = hana::make_tuple(canonical<bool, true>{}, canonical<bool, false>{});

    // Constant
    hana::test::TestConstant<hana::detail::CanonicalConstant<int>>{ints, hana::tuple_t<int, long, long long>};
    hana::test::TestConstant<hana::detail::CanonicalConstant<bool>>{bools, hana::tuple_t<bool>};

    // Monoid, Group, Ring, EuclideanRing
    hana::test::TestMonoid<hana::detail::CanonicalConstant<int>>{ints};
    hana::test::TestGroup<hana::detail::CanonicalConstant<int>>{ints};
    hana::test::TestRing<hana::detail::CanonicalConstant<int>>{ints};
    hana::test::TestEuclideanRing<hana::detail::CanonicalConstant<int>>{ints};

    // Logical
    {
        auto ints = hana::make_tuple(
            canonical<int, -2>{}, canonical<int, 0>{},
            canonical<int, 1>{}, canonical<int, 3>{}
        );
        hana::test::TestLogical<hana::detail::CanonicalConstant<int>>{ints};
        hana::test::TestLogical<hana::detail::CanonicalConstant<bool>>{bools};
    }

    // Comparable and Orderable
    hana::test::TestComparable<hana::detail::CanonicalConstant<int>>{ints};
    hana::test::TestOrderable<hana::detail::CanonicalConstant<int>>{ints};
}


/*
 * camera_calibration.hpp
 *
 *  Created on: Jun 26, 2018
 *      Author: atabb
 */

#ifndef CAMERA_CALIBRATION_HPP_
#define CAMERA_CALIBRATION_HPP_

#include <iostream>

#include <string>
#include <fstream>
#include <sstream>
#include <map>
#include <list>
#include <vector>
#include <stdlib.h>
#include <algorithm>
#include <inttypes.h>
#include <parallel/algorithm>
#include <iostream>
#include <algorithm>
#include <vector>
#include <set>
#include <cmath>
#include <omp.h>

#include <opencv2/core/core.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "Includes.hpp"

using namespace Eigen;
using namespace cv;
using std::vector;
using std::ifstream;
using std::string;

using std::set;
using std::max;
using std::min;
using std::cin;
using std::cout;
using std::endl;
using std::string;
using std::list;
using std::vector;
using std::map;
using std::pair;

// double to single mapping -- take 0 9 to number

class PatternsCreated{
public:
	vector<vector<int> > double_to_single;
	vector< cv::Point3f> three_d_points;
	vector< cv::Point3f> three_d_points_internal;
	Ptr<aruco::Dictionary> dictionary;
	vector< vector<int> > display_colors;
	vector<pair<int, int> > min_max_id_pattern;
	vector<pair<int, int> > min_max_id_squares;
	bool single_aruco_markers;
	vector<int> single_aruco_ids;
	int max_internal_patterns;
	int internalx, internaly;

	PatternsCreated(string read_dir, string write_directory, bool aruco_markers);

	int MappingArucoIDToPatternNumber(int id);

	Scalar Color(int index);

	int NumberMarkers();
	int NumberSquares();
	int NumberPatterns();

	void SetNumberMarkers(int n);
	void SetNumberPatterns(int n);

protected:
	int number_patterns;
	int number_total_squares;
	int int_number_markers;

};

class CameraCali{
public:

	vector<vector<bool> > patterns_present; //aruco
	vector<MatrixXd> two_d_point_coordinates_dense;
	vector<vector<int> > points_per_board;
	vector<vector<bool> > boards_detected;
	vector< vector<double> > reproj_error_per_board;

	vector<vector<bool> > has_calibration_estimate;
	vector<string> im_names;
	vector<string> im_short_names;

	vector<MatrixXd> internal_two_d_point_coordinates_dense;
	int count_internal_ids_present;
	vector<bool> id_bool;
	vector<double> focal_lengths;
	vector<double> pix_widths;
	double sensor_width;
	double pixel_width;
	int rows;
	int cols;

	vector<Mat> images; // the calibration images in the set
	int number_external_images_max;
	vector<Mat> reproject_cam_cali_images;

	Matrix3d internal_parameters;
	VectorXd distortion;
	vector<vector< Matrix4d> > external_parameters;

	PatternsCreated* P_class;

	vector<int> internal_pattern_indices;
	vector<MatrixXd> two_d_point_coordinates_segmentation_external;
	vector<MatrixXd> two_d_point_coordinates_segmentation_internal;

	CameraCali(string read_dir, PatternsCreated* P);

	void ReadExifInformationForAllImages(string image_read_dir, string parent_dir, string write_directory);

	void CopyToMats(Mat& CameraMatrix, Mat& dis);

	void FindCornersArucoGeneral(string write_dir, bool write_intermediate);

	void CalibrateArucoSinglyAndUndistort(string write_dir, double homograph_scaling, bool write_intermediate);

};

#endif /* CAMERA_CALIBRATION_HPP_ */