/*    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.
 *
 *    References
 *      Musegaas, P. (2012). Optimization of Space Trajectories Including Multiple Gravity Assists
 *          and Deep Space Maneuvers. MSc Thesis, Delft University of Technology, Delft,
 *          The Netherlands.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include <Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h>
#include <Tudat/Basics/testMacros.h>

#include "Tudat/Astrodynamics/TrajectoryDesign/departureLegMga.h"

namespace tudat
{
namespace unit_tests
{

//! Test implementation of departure leg within MGA trajectory model.
BOOST_AUTO_TEST_SUITE( test_departure_leg_mga )

//! Test delta-V computation.
BOOST_AUTO_TEST_CASE( testVelocities )
{
    // Set tolerance. Due to iterative nature in the process, (much) higher accuracy cannot be
    // achieved.
    const double tolerance = 1.0e-6;

    // Expected test result based on the first leg of the ideal Cassini 1 trajectory as modelled by
    // GTOP software distributed and downloadable from the ESA website, or within the PaGMO
    // Astrotoolbox.
    const double expectedDeltaV = 2754.63598732926;
    const Eigen::Vector3d expectedVelocity ( 34216.4827530912, -15170.1440677825,
                                             395.792122152361 );

    // Specify the required parameters.
    // Set the planetary positions and velocities.
    const Eigen::Vector3d planet1Position ( 113191651440.549, 95992973233.5064, 0. );
    const Eigen::Vector3d planet2Position ( -35554348960.8278, -102574987127.178,
                                            648696819.780156 );
    const Eigen::Vector3d planet1Velocity ( -19752.2624404406, 22607.9064746733, 0. );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    const double timeOfFlight = 158.302027105278 * physical_constants::JULIAN_DAY;

    // Set the gravitational parameters.
    const double sunGravitationalParameter = 1.32712428e20;
    const double earthGravitationalParameter = 3.9860119e14;

    // Set the departure parking orbit (at inifinity)
    const double semiMajorAxis = std::numeric_limits< double >::infinity( );
    const double eccentricity = 0.;

    // Set test case.
    using namespace tudat::transfer_trajectories;
    DepartureLegMga legTest ( planet1Position, planet2Position, timeOfFlight, planet1Velocity,
                              sunGravitationalParameter, earthGravitationalParameter,
                              semiMajorAxis, eccentricity );

    // Prepare the variables for the results.
    Eigen::Vector3d resultingVelocity;
    double resultingDeltaV;

    // Compute delta-V of the leg.
    legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance and if the computed velocity before target planet matches the expected velocity
    // within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, resultingDeltaV, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedVelocity, resultingVelocity, tolerance );
}

//! Test updating the variables.
BOOST_AUTO_TEST_CASE( testUpdatingVariables )
{
    // Set tolerance. Due to iterative nature in the process, (much) higher accuracy cannot be
    // achieved.
    const double tolerance = 1.0e-6;

    // Expected test result based on the first leg of the ideal Cassini 1 trajectory as modelled by
    // GTOP software distributed and downloadable from the ESA website, or within the PaGMO
    // Astrotoolbox.
    const double expectedDeltaV = 2754.63598732926;
    const Eigen::Vector3d expectedVelocity ( 34216.4827530912, -15170.1440677825,
                                             395.792122152361 );

    // Specify the required parameters.
    // Set the dummy positions and velocities.
    const Eigen::Vector3d dummyPosition1 ( TUDAT_NAN, TUDAT_NAN, TUDAT_NAN );
    const Eigen::Vector3d dummyPosition2 ( TUDAT_NAN, TUDAT_NAN, TUDAT_NAN );
    const Eigen::Vector3d dummyVelocity1 ( TUDAT_NAN, TUDAT_NAN, TUDAT_NAN );

    // Set the dummy time of flight.
    const double dummyTimeOfFlight = TUDAT_NAN;

    // Set the gravitational parameters.
    const double sunGravitationalParameter = 1.32712428e20;
    const double earthGravitationalParameter = 3.9860119e14;

    // Set the departure parking orbit (at inifinity)
    const double semiMajorAxis = std::numeric_limits< double >::infinity( );
    const double eccentricity = 0.;

    // Set test case.
    using namespace tudat::transfer_trajectories;
    DepartureLegMga legTest ( dummyPosition1, dummyPosition2, dummyTimeOfFlight, dummyVelocity1,
                              sunGravitationalParameter, earthGravitationalParameter,
                              semiMajorAxis, eccentricity );

    // Prepare the variables for the results.
    Eigen::Vector3d resultingVelocity;
    double resultingDeltaV;

    // Compute delta-V of the leg.
    //legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Specify the values for the parameters that are to be updated.
    // Set the planetary positions and velocities.
    const Eigen::Vector3d planet1Position ( 113191651440.549, 95992973233.5064, 0. );
    const Eigen::Vector3d planet2Position ( -35554348960.8278, -102574987127.178,
                                            648696819.780156 );
    const Eigen::Vector3d planet1Velocity ( -19752.2624404406, 22607.9064746733, 0. );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    Eigen::VectorXd variableVector ( 1 );
    variableVector[ 0 ] = 158.302027105278 * physical_constants::JULIAN_DAY;

    // Pass both the ephemeris and the time of flight to the leg.
    legTest.updateEphemeris( planet1Position, planet2Position, planet1Velocity );
    legTest.updateDefiningVariables( variableVector );

    // Compute delta-V of the leg.
    legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance and if the computed velocity before target planet matches the expected velocity
    // within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, resultingDeltaV, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedVelocity, resultingVelocity, tolerance );
}

//! Test intermediate points function.
BOOST_AUTO_TEST_CASE( testIntermediatePoints )
{
    // Set tolerance.
    const double tolerance = 1e-13;

    // Specify the required parameters, for the first leg of Cassini1 within GTOP.
    // Set the planetary positions and velocities.
    const Eigen::Vector3d planet1Position ( 113191651440.549, 95992973233.5064, 0. );
    const Eigen::Vector3d planet2Position ( -35554348960.8278, -102574987127.178,
                                            648696819.780156 );
    const Eigen::Vector3d planet1Velocity ( -19752.2624404406, 22607.9064746733, 0. );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    const double timeOfFlight = 158.302027105278 * 24 * 60 * 60;

    // Set the gravitational parameter.
    const double sunGravitationalParameter = 1.32712428e20;
    const double earthGravitationalParameter = 3.9860119e14;

    // Set the departure parking orbit (at inifinity)
    const double semiMajorAxis = 1e300;
    const double eccentricity = 0.;

    // Set test case.
    using namespace tudat::transfer_trajectories;
    DepartureLegMga legTest ( planet1Position, planet2Position, timeOfFlight, planet1Velocity,
                              sunGravitationalParameter, earthGravitationalParameter,
                              semiMajorAxis, eccentricity );

    // Initiate vectors for storing the results.
    std::vector < Eigen::Vector3d > positionVector1, positionVector2;
    std::vector < double > timeVector1, timeVector2;

    // Test the functionality in case the leg has not been calculated yet.
    legTest.intermediatePoints( 80. * physical_constants::JULIAN_DAY,
                                positionVector1, timeVector1 );

    // Prepare the variables for calculating the leg actively.
    Eigen::Vector3d resultingVelocity;
    double resultingDeltaV;

    // Calculate the leg actively.
    legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Test the functionality in case the leg has been calculated already.
    legTest.intermediatePoints( 40. * physical_constants::JULIAN_DAY,
                                positionVector2, timeVector2 );

    // Test if the halfway points of the intermediate points functions match.
    BOOST_CHECK_CLOSE_FRACTION( timeVector1[1] , timeVector2[2], tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( positionVector1[1], positionVector2[2], tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/* test_counter_detail.cpp
 *
 * Copyright (c) 2014 M.A. (Thijs) van den Berg
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */
#include <istream>
#include <ostream>
#include <iomanip>
#include <string>
#include <algorithm> // std::min

#include <boost/multiprecision/cpp_int.hpp>

#include <boost/random/threefry.hpp>
#include <boost/random/philox.hpp>
#include <boost/random/counter_based_engine.hpp>

#define BOOST_TEST_MODULE counter_engine_counter
#include <boost/test/included/unit_test.hpp>

#define BOOST_COUNTER_BASED_ENGINE_RESULT_TYPE uint64_t
#define BOOST_PSEUDO_RANDOM_FUNCTION boost::random::philox<4, uint64_t>
#define BOOST_COUNTER_BASED_ENGINE_CTRBITS 128u

typedef boost::random::counter_based_engine<BOOST_COUNTER_BASED_ENGINE_RESULT_TYPE, BOOST_PSEUDO_RANDOM_FUNCTION, BOOST_COUNTER_BASED_ENGINE_CTRBITS> engine_t;
#define BOOST_RANDOM_URNG engine_t

typedef engine_t::domain_type::value_type dvalue_type;
static const unsigned ctrbits = BOOST_RANDOM_URNG::counter_bits;

static const unsigned dvalbits = std::numeric_limits<dvalue_type>::digits;
static const unsigned ndomain = BOOST_RANDOM_URNG::domain_traits::Nbits / dvalbits;

static const unsigned nrange = engine_t::results_per_counter();

typedef engine_t::key_type::value_type kvalue_type;
static const unsigned kvalbits = std::numeric_limits<kvalue_type>::digits;
static const unsigned nkey = engine_t::key_traits::Nbits/kvalbits;

using namespace boost::multiprecision;

/*
 * Extract the "sample counter" value from a counter based engine.
 * "sample_counter" tell the numer of samples that are drawn from the engine.
 * It has a value of 1 after the first draw and increases with 1 after each draw.
 * If each encryption round generated S samples then this value is S * (2^counter_bits -1) + current_pos
 */
uint1024_t get_sample_counter(BOOST_RANDOM_URNG& urng)
{
    uint1024_t counter, digit;
    unsigned subcounter;

    std::stringstream ss;
    ss << urng;
    ss >> subcounter;

    for (unsigned int i=0; i<ndomain; ++i) {
        ss >> digit;
        counter <<= dvalbits;
        counter += digit;
    }

    counter &= (uint1024_t(1)<<ctrbits) - 1;
    counter = counter * nrange + subcounter;

    //std::cout << "counter=" <<  counter << " (" << os.str() << ")" << std::endl;
    return counter;
}

BOOST_AUTO_TEST_CASE(test_overflow)
{
    engine_t urng;
    engine_t urng0;

    // use 'private' knowledge of the external
    // stream representation to initialize a
    // nearly maximal counter.
    std::stringstream ss;
    ss << 0;
    for(size_t i=0; i<ndomain; ++i)
        ss << ' ' << ~dvalue_type(0);
    for(size_t i=0; i<nkey; ++i)
        ss << ' ' << 0;
    ss >> urng0;
    BOOST_CHECK(ss);

    // Run the urng right up to the edge..
    urng = urng0;
    for(unsigned i=0; i<nrange; ++i){
        urng();
        BOOST_CHECK_NE(urng, urng0);
    }
    // Verify that it throws when we push over the edge
    BOOST_CHECK_THROW(urng(), std::invalid_argument);

    // Try again with discard:
    urng = urng0;
    urng.discard(nrange);
    BOOST_CHECK_NE(urng, urng0);
    BOOST_CHECK_THROW(urng(), std::invalid_argument);

    // Make sure that discard itself throws if it pushes us over the
    // edge.
    urng = urng0;
    BOOST_CHECK_THROW(urng.discard(nrange+1), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(test_counter_detail)
{
    uint1024_t expected,actual,discards;
    BOOST_RANDOM_URNG urng;

    // initial check
    urng();                                 // generate a single random value to trigger initialisation
    expected = uint1024_t(1);               // we expect the sample counter to now be at 1
    actual = get_sample_counter(urng);
    BOOST_CHECK_EQUAL(expected, actual);

    // draw 300 samples
    for (unsigned i=0; i<300; ++i) {
        urng();
        ++expected;
        actual = get_sample_counter(urng);
        BOOST_CHECK_EQUAL(expected, actual);
    }

    // do 300 big discards
    for (unsigned i=0; i<300; ++i) {
        discards = uint1024_t(0xFFFFFFFFFFFFFFFF);
        expected += discards;
        urng.discard(0xFFFFFFFFFFFFFFFF);
        actual = get_sample_counter(urng);
        BOOST_CHECK_EQUAL(expected, actual);
    }
}


/*
 * rt_generic_rk4_lorenz.cpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * 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/array.hpp>

#include "rt_explicit_rk.hpp"

#include "rk_performance_test_case.hpp"

#include "phase_lattice.hpp"

const size_t N = 1024;

typedef boost::array< double , N > state_type;

typedef rt_explicit_rk< state_type > rk_stepper_type;

const size_t stage_count = 4;

class rt_generic_wrapper
{
public:

    rt_generic_wrapper( const double * const * a ,
                        const rk_stepper_type::coeff_b_type &b ,
                        const rk_stepper_type::coeff_c_type &c )
        : m_stepper( stage_count ,
                     (rk_stepper_type::coeff_a_type) a , b , c )
    { }

    void reset_init_cond()
    {
        for( size_t i = 0 ; i<N ; ++i )
            m_x[i] = 2.0*3.1415927*rand() / RAND_MAX;
        m_t = 0.0;
    }

    inline void do_step( const double dt )
    {
        m_stepper.do_step( phase_lattice<N>() , m_x , m_t , dt );
        //m_t += dt;
    }

    double state( const size_t i ) const
    { return m_x[i]; }

private:
    state_type m_x;
    double m_t;
    rk_stepper_type m_stepper;
};

int main()
{
    srand( 12312354 );

    const double a_tmp[3*4/2] = { 0.5 ,
                                  0.0 , 1.0 ,
                                  0.0 , 0.0 , 1.0 };
    const double* const a[3] = { a_tmp , a_tmp+1 , a_tmp+3 };

    rk_stepper_type::coeff_b_type b( stage_count );
    b[0] = 1.0/6; b[1] = 1.0/3; b[2] = 1.0/3; b[3] = 1.0/6;

    rk_stepper_type::coeff_c_type c( stage_count );
    c[0] = 0.0; c[1] = 0.5; c[2] = 0.5; c[3] = 1.0;

    rt_generic_wrapper stepper( a , b , c );

    run( stepper , 10000 , 1E-6 );
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/constant/one.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF(ceil_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      const A0 d0 = nearbyint(a0);
      return if_plus(is_less(d0,a0),d0,One<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ceil_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_< bd::integer_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };
} } }

#endif


#include <boost/graph/bc_clustering.hpp>


/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * 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 <iostream>
#include <memory>
#include <ctime>
#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

#include <boost/random.hpp>

#include "GaussianProcess.h"
#include "Kernel.h"
#include "Likelihood.h"
#include "LikelihoodUtils.h"
#include "Prior.h"
#include "MatrixIO.h"

// typedefinitions
typedef gpr::Kernel<double>                     KernelType;
typedef std::shared_ptr<KernelType>             KernelTypePointer;
typedef gpr::WhiteKernel<double>                 WhiteKernelType;
typedef std::shared_ptr<WhiteKernelType>        WhiteKernelTypePointer;
typedef gpr::GaussianKernel<double>             GaussianKernelType;
typedef std::shared_ptr<GaussianKernelType>     GaussianKernelTypePointer;
typedef gpr::PeriodicKernel<double>             PeriodicKernelType;
typedef std::shared_ptr<PeriodicKernelType>     PeriodicKernelTypePointer;
typedef gpr::SumKernel<double>                  SumKernelType;
typedef std::shared_ptr<SumKernelType>          SumKernelTypePointer;
typedef gpr::GaussianProcess<double>            GaussianProcessType;
typedef std::shared_ptr<GaussianProcessType>    GaussianProcessTypePointer;

void Test1(){
    /*
     * Test 1.1: perform maximum likelihood
     *      - try to infer a squared function
     *      - maximum is searched by brute force
     */
    std::cout << "Test 1: maximum log gaussian likelihood (brute force)... " << std::flush;

    typedef GaussianProcessType::VectorType VectorType;
    typedef GaussianProcessType::MatrixType MatrixType;

    // generating a signal
    // ground truth periodic variable
    //auto f = [](double x)->double { return 400*std::sin(1.5*x) + x*x; };
    auto f = [](double x)->double { return x*x; };

    double val = 0;
    unsigned n = 100;
    MatrixType signal = MatrixType::Zero(2,n);
    for(unsigned i=0; i<n; i++){
        signal(0,i) = val;
        signal(1,i) = f(val);
        val += 100.0/n;
    }

    // build Gaussian process and add the 1D samples
    WhiteKernelTypePointer pk(new WhiteKernelType(0)); // dummy kernel
    GaussianProcessTypePointer gp(new GaussianProcessType(pk));
    gp->SetSigma(std::sqrt(0.001)); // zero since with the white kernel (see later) this is considered

    for(unsigned i=0; i<10; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    for(unsigned i=80; i<90; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    // construct Gaussian log likelihood
    typedef gpr::GaussianLogLikelihood<double> GaussianLogLikelihoodType;
    typedef std::shared_ptr<GaussianLogLikelihoodType> GaussianLogLikelihoodTypePointer;
    GaussianLogLikelihoodTypePointer gl(new GaussianLogLikelihoodType());

    double sigma_max = 0;
    double scale_max = 0;
    double likelihood_max = std::numeric_limits<double>::lowest();

    for(double scale=1; scale<1000; scale+=100){
        for(double sigma=1; sigma<100; sigma+=2){
            // analytical
            try{
                GaussianKernelTypePointer gk(new GaussianKernelType(sigma, scale));
                gp->SetKernel(gk);

                double likelihood = (*gl)(gp)[0];
                if(likelihood > likelihood_max){
                    likelihood_max = likelihood;
                    sigma_max = sigma;
                    scale_max = scale;
                }
            }
            catch(std::string& s){
                std::cout << "[failed] " << s << std::endl;
                return;
            }
        }
    }

    std::vector<double> prediction_y;
    std::vector<double> prediction_x;
    // predict some stuff:
    for(unsigned i=0; i<100; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        prediction_y.push_back(gp->Predict(x)[0]);
        prediction_x.push_back(signal(0,i));
    }

    double err = 0;
    for(unsigned i=0; i<prediction_x.size(); i++ ){
        err += std::abs(prediction_y[i]-signal(1,i));
    }

    if(err/prediction_x.size() < 2){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<err/prediction_x.size(); throw ss.str();
    }
}

void Test2(){
    /*
     * Test 2: perform maximum likelihood
     *      - try to infer a squared function
     *      - maximum is searched by g
     */
    std::cout << "Test 2: maximum log gaussian likelihood (gradient descent)... " << std::flush;

    typedef GaussianProcessType::VectorType VectorType;
    typedef GaussianProcessType::MatrixType MatrixType;

    // generating a signal
    // ground truth periodic variable
    //auto f = [](double x)->double { return 400*std::sin(1.5*x) + x*x; };
    auto f = [](double x)->double { return x*x; };

    double val = 0;
    unsigned n = 101;
    MatrixType signal = MatrixType::Zero(2,n);
    for(unsigned i=0; i<n; i++){
        signal(0,i) = val;
        signal(1,i) = f(val);
        val += 100.0/n;
    }

    // build Gaussian process and add the 1D samples
    WhiteKernelTypePointer pk(new WhiteKernelType(0)); // dummy kernel
    GaussianProcessTypePointer gp(new GaussianProcessType(pk));
    gp->SetSigma(std::sqrt(0.1)); // zero since with the white kernel (see later) this is considered

    for(unsigned i=0; i<n; i+=10){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    // construct Gaussian log likelihood
    typedef gpr::GaussianLogLikelihood<double> GaussianLogLikelihoodType;
    typedef std::shared_ptr<GaussianLogLikelihoodType> GaussianLogLikelihoodTypePointer;
    GaussianLogLikelihoodTypePointer gl(new GaussianLogLikelihoodType());

    double sigma = 50;
    double scale = 1300;
    double lambda = 1;
    for(unsigned i=0; i<100; i++){
        // analytical
        try{
            GaussianKernelTypePointer gk(new GaussianKernelType(sigma, scale));
            gp->SetKernel(gk);

            VectorType likelihood_update = gl->GetParameterDerivatives(gp);

            //std::cout << (*gl)(gp) << ", sigma/scale: " << sigma << "/" << scale << std::endl;

            sigma += lambda * likelihood_update[0];
            scale += lambda * likelihood_update[1];
        }
        catch(std::string& s){
            std::cout << "[failed] " << s << std::endl;
            return;
        }
    }

    std::vector<double> prediction_y;
    std::vector<double> prediction_x;
    // predict some stuff:
    for(unsigned i=0; i<n; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        prediction_y.push_back(gp->Predict(x)[0]);
        prediction_x.push_back(signal(0,i));

        //std::cout << prediction_y.back() << std::endl;
    }

    double err = 0;
    for(unsigned i=0; i<prediction_x.size(); i++ ){
        err += std::abs(prediction_y[i]-signal(1,i));
    }

    if(err/prediction_x.size() < 5){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<err/prediction_x.size(); throw ss.str();
    }
}

void Test3(){
    /*
     * Test 3: perform maximum likelihood
     *      - try to infer a periodic function
     *      - maximum is searched by brute force
     */
    std::cout << "Test 3: maximum log gaussian likelihood (brute force)... " << std::flush;

    typedef GaussianProcessType::VectorType VectorType;
    typedef GaussianProcessType::MatrixType MatrixType;

    // generating a signal
    // ground truth periodic variable
    auto f = [](double x)->double { return 400*std::sin(1.5*x); };

    double val = 0;
    unsigned n = 200;
    MatrixType signal = MatrixType::Zero(2,n);
    for(unsigned i=0; i<n; i++){
        signal(0,i) = val;
        signal(1,i) = f(val);
        val += 50.0/n;
    }

    // build Gaussian process and add the 1D samples
    WhiteKernelTypePointer pk(new WhiteKernelType(0)); // dummy kernel
    GaussianProcessTypePointer gp(new GaussianProcessType(pk));
    gp->SetSigma(std::sqrt(0.001)); // zero since with the white kernel (see later) this is considered

    for(unsigned i=0; i<20; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    for(unsigned i=80; i<100; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    // construct Gaussian log likelihood
    typedef gpr::GaussianLogLikelihood<double> GaussianLogLikelihoodType;
    typedef std::shared_ptr<GaussianLogLikelihoodType> GaussianLogLikelihoodTypePointer;
    GaussianLogLikelihoodTypePointer gl(new GaussianLogLikelihoodType());

    //double period = M_PI/15;
    double sigma = 1;
    double scale = 400;
    double likelihood_max = std::numeric_limits<double>::lowest();
    double period_max = 0;
    for(double period=0.1; period<2*M_PI; period+=0.01){
        // analytical
        try{
            PeriodicKernelTypePointer gk(new PeriodicKernelType(scale, period, sigma));
            gp->SetKernel(gk);

            double likelihood= (*gl)(gp)[0];
            if(likelihood > likelihood_max){
                likelihood_max = likelihood;
                period_max = period;
            }
        }
        catch(std::string& s){
            throw std::string("something went wrong in likelihood calculation");
            return;
        }
    }

    PeriodicKernelTypePointer gk(new PeriodicKernelType(scale, period_max, sigma));
    gp->SetKernel(gk);

    std::vector<double> prediction_y;
    std::vector<double> prediction_x;
    // predict some stuff:
    for(unsigned i=0; i<signal.cols(); i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        prediction_y.push_back(gp->Predict(x)[0]);
        prediction_x.push_back(signal(0,i));
    }

    double err = 0;
    for(unsigned i=0; i<prediction_x.size(); i++ ){
        err += std::abs(prediction_y[i]-signal(1,i));
    }

    if(err/prediction_x.size() < 1e-4){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<err/prediction_x.size(); throw ss.str();
    }
}

int main (int argc, char *argv[]){
    std::cout << "Gaussian likelihood kernel test: " << std::endl;
    try{
        Test1();
        Test2();
        Test3();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/arch/common/detail/simd/f_invtrig.hpp>
#include <boost/simd/arch/common/detail/simd/d_invtrig.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_positive.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/negatenz.hpp>
#include <boost/simd/function/rec.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/logical_and.hpp>
#endif

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/logical_or.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( atan2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     inline A0 operator()(const pedantic_tag &,
                          const A0& a0,const A0& a1) const
      {
        A0 a00 = a0, a10 = a1;
  #ifndef BOOST_SIMD_NO_INFINITIES
        auto test1 =  bs::logical_and(bs::is_inf(a0),  bs::is_inf(a1));
        a00 =  bs::if_else(test1, bs::copysign(One<A0>(), a00), a00);
        a10 =  bs::if_else(test1, bs::copysign(One<A0>(), a10), a10);
  #endif
        A0 q = bs::abs(a00/a10);
        A0 z = detail::invtrig_base<A0,tag::radian_tag, tag::simd_type>::kernel_atan(q, rec(q));
        //A0 z = atan(abs(a0/a1));  // case a1 > 0,  a0 > 0
        A0 sgn = signnz(a0);
        z = bs::if_else(bs::is_positive(a10), z, bs::Pi<A0>()-z)*sgn;
        z = bs::if_else( bs::is_eqz(a00),
                         bs::if_else_zero( bs::is_negative(a10),  bs::Pi<A0>()*sgn),
                         z);
  #ifdef BOOST_SIMD_NO_NANS
        return z;
  #else
        return  bs::if_nan_else( bs::logical_or( bs::is_nan(a00),  bs::is_nan(a10)), z);
  #endif
      }
   };

  BOOST_DISPATCH_OVERLOAD_IF( atan2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     inline A0 operator()(const A0& a0,const A0& a1) const
      {
        A0 q = bs::abs(a0/a1);
        A0 z = detail::invtrig_base<A0,tag::radian_tag, tag::simd_type>::kernel_atan(q, rec(q));
        return bs::if_else(bs::is_positive(a1), z, bs::Pi<A0>()-z)* signnz(a0);
      }
   };
} } }

#endif


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( trunc_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( trunc_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return bitwise_or(floor(bs::abs(a0)), bitofsign(a0));
    }
  };
  BOOST_DISPATCH_OVERLOAD ( trunc_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( trunc_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_< bd::floating_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0) const BOOST_NOEXCEPT
    {
      return static_cast<A0>(static_cast<bd::as_integer_t<A0>>(a0));
    }
  };
  BOOST_DISPATCH_OVERLOAD ( trunc_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &
                                    ,  A0 a0) const BOOST_NOEXCEPT
    {
      return std::trunc(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( trunc_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &
                                    ,  A0 a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };

} } }

#endif


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE ExactTransientSolution

#include "main.cc"

#include <cap/energy_storage_device.h>
#include <cap/mp_values.h>
#include <deal.II/base/types.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <boost/test/unit_test.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <cmath>
#include <iostream>
#include <fstream>
#include <numeric>

namespace cap
{

void verification_problem(std::shared_ptr<cap::EnergyStorageDevice> dev)
{
  // gold vs computed
  double const charge_current = 5e-3;
  double const charge_time = 0.01;
  double const time_step = 1e-4;
  double const epsilon = time_step * 1.0e-4;

  unsigned int pos = 0;
  double computed_voltage;
  // check that error less than 1e-3 % AND less than 1 microvolt
  double const percent_tolerance = 1e-3;
  double const tolerance = 1e-6;
  std::vector<double> gold_solution(10);
  gold_solution[0] = 1.725914356067658e-01;
  gold_solution[1] = 1.802025636145941e-01;
  gold_solution[2] = 1.859326352495181e-01;
  gold_solution[3] = 1.905978440188036e-01;
  gold_solution[4] = 1.946022119085378e-01;
  gold_solution[5] = 1.981601232287249e-01;
  gold_solution[6] = 2.013936650249285e-01;
  gold_solution[7] = 2.043807296399895e-01;
  gold_solution[8] = 2.071701713934283e-01;
  gold_solution[9] = 2.097979282542038e-01;
  for (double time = 0.0; time <= charge_time + epsilon; time += time_step)
  {
    dev->evolve_one_time_step_constant_current(time_step, charge_current);
    dev->get_voltage(computed_voltage);
    if ((std::abs(time + time_step - 1e-3) < 1e-7) ||
        (std::abs(time + time_step - 2e-3) < 1e-7) ||
        (std::abs(time + time_step - 3e-3) < 1e-7) ||
        (std::abs(time + time_step - 4e-3) < 1e-7) ||
        (std::abs(time + time_step - 5e-3) < 1e-7) ||
        (std::abs(time + time_step - 6e-3) < 1e-7) ||
        (std::abs(time + time_step - 7e-3) < 1e-7) ||
        (std::abs(time + time_step - 8e-3) < 1e-7) ||
        (std::abs(time + time_step - 9e-3) < 1e-7) ||
        (std::abs(time + time_step - 10e-3) < 1e-7))
    {
      BOOST_CHECK_CLOSE(computed_voltage, gold_solution[pos],
                        percent_tolerance);
      BOOST_CHECK_SMALL(computed_voltage - gold_solution[pos], tolerance);
      ++pos;
    }
  }
}

} // end namespace cap

BOOST_AUTO_TEST_CASE(test_exact_transient_solution)
{
  // parse input file
  boost::property_tree::ptree device_database;
  boost::property_tree::info_parser::read_info("super_capacitor.info",
                                               device_database);
  boost::property_tree::ptree geometry_database;
  boost::property_tree::info_parser::read_info("read_mesh.info",
                                               geometry_database);
  device_database.put_child("geometry", geometry_database);

  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(device_database,
                                      boost::mpi::communicator());

  cap::verification_problem(device);
}


/*
@copyright Louis Dionne 2015
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/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/iterable.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/monad_plus.hpp>
#include <boost/hana/sequence.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

struct LazyList;

template <typename X, typename Xs>
struct lazy_cons_type {
    X x;
    Xs xs;
    struct hana { using datatype = LazyList; };
};

auto lazy_cons = [](auto x, auto xs) {
    return lazy_cons_type<decltype(x), decltype(xs)>{x, xs};
};

struct lazy_nil_type { struct hana { using datatype = LazyList; }; };

constexpr lazy_nil_type lazy_nil{};

auto repeat_ = fix([](auto repeat, auto x) {
    return lazy_cons(x, lazy(repeat)(x));
});

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Iterable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct head_impl<LazyList> {
        template <typename Xs>
        static constexpr auto apply(Xs lcons)
        { return lcons.x; }
    };

    template <>
    struct tail_impl<LazyList> {
        template <typename Xs>
        static constexpr auto apply(Xs lcons)
        { return eval(lcons.xs); }
    };

    template <>
    struct is_empty_impl<LazyList> {
        template <typename Xs>
        static constexpr auto apply(Xs lcons)
        { return false_; }

        static constexpr auto apply(lazy_nil_type)
        { return true_; }
    };

    //////////////////////////////////////////////////////////////////////////
    // MonadPlus
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct prepend_impl<LazyList> {
        template <typename X, typename Xs>
        static constexpr auto apply(X x, Xs xs)
        { return lazy_cons(x, lazy(xs)); }
    };

    template <>
    struct empty_impl<LazyList> {
        static constexpr auto apply()
        { return lazy_nil; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Sequence
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct models_impl<Sequence, LazyList>
        : decltype(true_)
    { };
}}

int main() {
    BOOST_HANA_CONSTANT_CHECK(!is_empty(repeat_(1)));
    BOOST_HANA_CONSTEXPR_CHECK(head(repeat_(1)) == 1);
    BOOST_HANA_CONSTEXPR_CHECK(at(size_t<10>, repeat_(1)) == 1);

    BOOST_HANA_CONSTEXPR_CHECK(take(size_t<2>, repeat_('x')) == make<Tuple>('x', 'x'));
}


//  Copyright (c) 2018-2019 Cem Bassoy
//
//  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/test/unit_test.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>
#include <vector>

BOOST_AUTO_TEST_SUITE ( test_extents )

//*boost::unit_test::label("extents")
//*boost::unit_test::label("constructor")

BOOST_AUTO_TEST_CASE(test_extents_ctor)
{
	using namespace boost::numeric;
	using extents = ublas::basic_extents<unsigned>;

	auto e0 = extents{};
	BOOST_CHECK( e0.empty());
	BOOST_CHECK_EQUAL ( e0.size(),0);

	auto e1 = extents{1,1};
	BOOST_CHECK(!e1.empty());
	BOOST_CHECK_EQUAL ( e1.size(),2);

	auto e2 = extents{1,2};
	BOOST_CHECK(!e2.empty());
	BOOST_CHECK_EQUAL ( e2.size(),2);

	auto e3 = extents{2,1};
	BOOST_CHECK (!e3.empty());
	BOOST_CHECK_EQUAL  ( e3.size(),2);

	auto e4 = extents{2,3};
	BOOST_CHECK(!e4.empty());
	BOOST_CHECK_EQUAL ( e4.size(),2);

	auto e5 = extents{2,3,1};
	BOOST_CHECK (!e5.empty());
	BOOST_CHECK_EQUAL  ( e5.size(),3);

	auto e6 = extents{1,2,3}; // 6
	BOOST_CHECK(!e6.empty());
	BOOST_CHECK_EQUAL ( e6.size(),3);

	auto e7 = extents{4,2,3};  // 7
	BOOST_CHECK(!e7.empty());
	BOOST_CHECK_EQUAL ( e7.size(),3);

	BOOST_CHECK_THROW( extents({1,0}), std::length_error );
	BOOST_CHECK_THROW( extents({0}  ), std::length_error );
	BOOST_CHECK_THROW( extents({3}  ), std::length_error );
	BOOST_CHECK_THROW( extents({0,1}), std::length_error );
}

struct fixture {
	using extents_type = boost::numeric::ublas::basic_extents<unsigned>;
	fixture() : extents{
	              extents_type{},            // 0

	              extents_type{1,1},         // 1
	              extents_type{1,2},         // 2
	              extents_type{2,1},         // 3

	              extents_type{2,3},         // 4
	              extents_type{2,3,1},       // 5
	              extents_type{1,2,3},       // 6
	              extents_type{1,1,2,3},     // 7
	              extents_type{1,2,3,1,1},   // 8

	              extents_type{4,2,3},       // 9
	              extents_type{4,2,1,3},     // 10
	              extents_type{4,2,1,3,1},   // 11
	              extents_type{1,4,2,1,3,1}, // 12
}  // 13
	{}
	std::vector<extents_type> extents;
};

BOOST_FIXTURE_TEST_CASE(test_extents_access, fixture, *boost::unit_test::label("extents") *boost::unit_test::label("access"))
{
	using namespace boost::numeric;

	BOOST_REQUIRE_EQUAL(extents.size(),13);

	BOOST_CHECK_EQUAL  (extents[ 0].size(), 0);
	BOOST_CHECK (extents[ 0].empty()  );

	BOOST_REQUIRE_EQUAL(extents[ 1].size(), 2);
	BOOST_REQUIRE_EQUAL(extents[ 2].size(), 2);
	BOOST_REQUIRE_EQUAL(extents[ 3].size(), 2);
	BOOST_REQUIRE_EQUAL(extents[ 4].size(), 2);
	BOOST_REQUIRE_EQUAL(extents[ 5].size(), 3);
	BOOST_REQUIRE_EQUAL(extents[ 6].size(), 3);
	BOOST_REQUIRE_EQUAL(extents[ 7].size(), 4);
	BOOST_REQUIRE_EQUAL(extents[ 8].size(), 5);
	BOOST_REQUIRE_EQUAL(extents[ 9].size(), 3);
	BOOST_REQUIRE_EQUAL(extents[10].size(), 4);
	BOOST_REQUIRE_EQUAL(extents[11].size(), 5);
	BOOST_REQUIRE_EQUAL(extents[12].size(), 6);

	BOOST_CHECK_EQUAL(extents[1][0],1);
	BOOST_CHECK_EQUAL(extents[1][1],1);

	BOOST_CHECK_EQUAL(extents[2][0],1);
	BOOST_CHECK_EQUAL(extents[2][1],2);

	BOOST_CHECK_EQUAL(extents[3][0],2);
	BOOST_CHECK_EQUAL(extents[3][1],1);

	BOOST_CHECK_EQUAL(extents[4][0],2);
	BOOST_CHECK_EQUAL(extents[4][1],3);

	BOOST_CHECK_EQUAL(extents[5][0],2);
	BOOST_CHECK_EQUAL(extents[5][1],3);
	BOOST_CHECK_EQUAL(extents[5][2],1);

	BOOST_CHECK_EQUAL(extents[6][0],1);
	BOOST_CHECK_EQUAL(extents[6][1],2);
	BOOST_CHECK_EQUAL(extents[6][2],3);

	BOOST_CHECK_EQUAL(extents[7][0],1);
	BOOST_CHECK_EQUAL(extents[7][1],1);
	BOOST_CHECK_EQUAL(extents[7][2],2);
	BOOST_CHECK_EQUAL(extents[7][3],3);

	BOOST_CHECK_EQUAL(extents[8][0],1);
	BOOST_CHECK_EQUAL(extents[8][1],2);
	BOOST_CHECK_EQUAL(extents[8][2],3);
	BOOST_CHECK_EQUAL(extents[8][3],1);
	BOOST_CHECK_EQUAL(extents[8][4],1);

	BOOST_CHECK_EQUAL(extents[9][0],4);
	BOOST_CHECK_EQUAL(extents[9][1],2);
	BOOST_CHECK_EQUAL(extents[9][2],3);

	BOOST_CHECK_EQUAL(extents[10][0],4);
	BOOST_CHECK_EQUAL(extents[10][1],2);
	BOOST_CHECK_EQUAL(extents[10][2],1);
	BOOST_CHECK_EQUAL(extents[10][3],3);

	BOOST_CHECK_EQUAL(extents[11][0],4);
	BOOST_CHECK_EQUAL(extents[11][1],2);
	BOOST_CHECK_EQUAL(extents[11][2],1);
	BOOST_CHECK_EQUAL(extents[11][3],3);
	BOOST_CHECK_EQUAL(extents[11][4],1);

	BOOST_CHECK_EQUAL(extents[12][0],1);
	BOOST_CHECK_EQUAL(extents[12][1],4);
	BOOST_CHECK_EQUAL(extents[12][2],2);
	BOOST_CHECK_EQUAL(extents[12][3],1);
	BOOST_CHECK_EQUAL(extents[12][4],3);
	BOOST_CHECK_EQUAL(extents[12][5],1);
}

BOOST_FIXTURE_TEST_CASE(test_extents_copy_ctor, fixture, *boost::unit_test::label("extents") *boost::unit_test::label("copy_ctor"))
{
	BOOST_REQUIRE_EQUAL(extents.size(),13);

	auto e0  = extents[ 0]; // {}
	auto e1  = extents[ 1]; // {1,1}
	auto e2  = extents[ 2]; // {1,2}
	auto e3  = extents[ 3]; // {2,1}
	auto e4  = extents[ 4]; // {2,3}
	auto e5  = extents[ 5]; // {2,3,1}
	auto e6  = extents[ 6]; // {1,2,3}
	auto e7  = extents[ 7]; // {1,1,2,3}
	auto e8  = extents[ 8]; // {1,2,3,1,1}
	auto e9  = extents[ 9]; // {4,2,3}
	auto e10 = extents[10]; // {4,2,1,3}
	auto e11 = extents[11]; // {4,2,1,3,1}
	auto e12 = extents[12]; // {1,4,2,1,3,1}

	BOOST_CHECK_EQUAL  (e0.size(), 0);
	BOOST_CHECK (e0.empty()  );

	BOOST_REQUIRE_EQUAL(e1 .size(), 2);
	BOOST_REQUIRE_EQUAL(e2 .size(), 2);
	BOOST_REQUIRE_EQUAL(e3 .size(), 2);
	BOOST_REQUIRE_EQUAL(e4 .size(), 2);
	BOOST_REQUIRE_EQUAL(e5 .size(), 3);
	BOOST_REQUIRE_EQUAL(e6 .size(), 3);
	BOOST_REQUIRE_EQUAL(e7 .size(), 4);
	BOOST_REQUIRE_EQUAL(e8 .size(), 5);
	BOOST_REQUIRE_EQUAL(e9 .size(), 3);
	BOOST_REQUIRE_EQUAL(e10.size(), 4);
	BOOST_REQUIRE_EQUAL(e11.size(), 5);
	BOOST_REQUIRE_EQUAL(e12.size(), 6);

	BOOST_CHECK_EQUAL(e1[0],1);
	BOOST_CHECK_EQUAL(e1[1],1);

	BOOST_CHECK_EQUAL(e2[0],1);
	BOOST_CHECK_EQUAL(e2[1],2);

	BOOST_CHECK_EQUAL(e3[0],2);
	BOOST_CHECK_EQUAL(e3[1],1);

	BOOST_CHECK_EQUAL(e4[0],2);
	BOOST_CHECK_EQUAL(e4[1],3);

	BOOST_CHECK_EQUAL(e5[0],2);
	BOOST_CHECK_EQUAL(e5[1],3);
	BOOST_CHECK_EQUAL(e5[2],1);

	BOOST_CHECK_EQUAL(e6[0],1);
	BOOST_CHECK_EQUAL(e6[1],2);
	BOOST_CHECK_EQUAL(e6[2],3);

	BOOST_CHECK_EQUAL(e7[0],1);
	BOOST_CHECK_EQUAL(e7[1],1);
	BOOST_CHECK_EQUAL(e7[2],2);
	BOOST_CHECK_EQUAL(e7[3],3);

	BOOST_CHECK_EQUAL(e8[0],1);
	BOOST_CHECK_EQUAL(e8[1],2);
	BOOST_CHECK_EQUAL(e8[2],3);
	BOOST_CHECK_EQUAL(e8[3],1);
	BOOST_CHECK_EQUAL(e8[4],1);

	BOOST_CHECK_EQUAL(e9[0],4);
	BOOST_CHECK_EQUAL(e9[1],2);
	BOOST_CHECK_EQUAL(e9[2],3);

	BOOST_CHECK_EQUAL(e10[0],4);
	BOOST_CHECK_EQUAL(e10[1],2);
	BOOST_CHECK_EQUAL(e10[2],1);
	BOOST_CHECK_EQUAL(e10[3],3);

	BOOST_CHECK_EQUAL(e11[0],4);
	BOOST_CHECK_EQUAL(e11[1],2);
	BOOST_CHECK_EQUAL(e11[2],1);
	BOOST_CHECK_EQUAL(e11[3],3);
	BOOST_CHECK_EQUAL(e11[4],1);

	BOOST_CHECK_EQUAL(e12[0],1);
	BOOST_CHECK_EQUAL(e12[1],4);
	BOOST_CHECK_EQUAL(e12[2],2);
	BOOST_CHECK_EQUAL(e12[3],1);
	BOOST_CHECK_EQUAL(e12[4],3);
	BOOST_CHECK_EQUAL(e12[5],1);

}

BOOST_FIXTURE_TEST_CASE(test_extents_is, fixture, *boost::unit_test::label("extents") *boost::unit_test::label("query"))
{
	BOOST_REQUIRE_EQUAL(extents.size(),13);

	auto e0  = extents[ 0]; // {}
	auto e1  = extents[ 1]; // {1,1}
	auto e2  = extents[ 2]; // {1,2}
	auto e3  = extents[ 3]; // {2,1}
	auto e4  = extents[ 4]; // {2,3}
	auto e5  = extents[ 5]; // {2,3,1}
	auto e6  = extents[ 6]; // {1,2,3}
	auto e7  = extents[ 7]; // {1,1,2,3}
	auto e8  = extents[ 8]; // {1,2,3,1,1}
	auto e9  = extents[ 9]; // {4,2,3}
	auto e10 = extents[10]; // {4,2,1,3}
	auto e11 = extents[11]; // {4,2,1,3,1}
	auto e12 = extents[12]; // {1,4,2,1,3,1}

	BOOST_CHECK(   e0.empty    ());
	BOOST_CHECK( ! e0.is_scalar());
	BOOST_CHECK( ! e0.is_vector());
	BOOST_CHECK( ! e0.is_matrix());
	BOOST_CHECK( ! e0.is_tensor());

	BOOST_CHECK( ! e1.empty    () );
	BOOST_CHECK(   e1.is_scalar() );
	BOOST_CHECK( ! e1.is_vector() );
	BOOST_CHECK( ! e1.is_matrix() );
	BOOST_CHECK( ! e1.is_tensor() );

	BOOST_CHECK( ! e2.empty    () );
	BOOST_CHECK( ! e2.is_scalar() );
	BOOST_CHECK(   e2.is_vector() );
	BOOST_CHECK( ! e2.is_matrix() );
	BOOST_CHECK( ! e2.is_tensor() );

	BOOST_CHECK( ! e3.empty    () );
	BOOST_CHECK( ! e3.is_scalar() );
	BOOST_CHECK(   e3.is_vector() );
	BOOST_CHECK( ! e3.is_matrix() );
	BOOST_CHECK( ! e3.is_tensor() );

	BOOST_CHECK( ! e4.empty    () );
	BOOST_CHECK( ! e4.is_scalar() );
	BOOST_CHECK( ! e4.is_vector() );
	BOOST_CHECK(   e4.is_matrix() );
	BOOST_CHECK( ! e4.is_tensor() );

	BOOST_CHECK( ! e5.empty    () );
	BOOST_CHECK( ! e5.is_scalar() );
	BOOST_CHECK( ! e5.is_vector() );
	BOOST_CHECK(   e5.is_matrix() );
	BOOST_CHECK( ! e5.is_tensor() );

	BOOST_CHECK( ! e6.empty    () );
	BOOST_CHECK( ! e6.is_scalar() );
	BOOST_CHECK( ! e6.is_vector() );
	BOOST_CHECK( ! e6.is_matrix() );
	BOOST_CHECK(   e6.is_tensor() );

	BOOST_CHECK( ! e7.empty    () );
	BOOST_CHECK( ! e7.is_scalar() );
	BOOST_CHECK( ! e7.is_vector() );
	BOOST_CHECK( ! e7.is_matrix() );
	BOOST_CHECK(   e7.is_tensor() );

	BOOST_CHECK( ! e8.empty    () );
	BOOST_CHECK( ! e8.is_scalar() );
	BOOST_CHECK( ! e8.is_vector() );
	BOOST_CHECK( ! e8.is_matrix() );
	BOOST_CHECK(   e8.is_tensor() );

	BOOST_CHECK( ! e9.empty    () );
	BOOST_CHECK( ! e9.is_scalar() );
	BOOST_CHECK( ! e9.is_vector() );
	BOOST_CHECK( ! e9.is_matrix() );
	BOOST_CHECK(   e9.is_tensor() );

	BOOST_CHECK( ! e10.empty    () );
	BOOST_CHECK( ! e10.is_scalar() );
	BOOST_CHECK( ! e10.is_vector() );
	BOOST_CHECK( ! e10.is_matrix() );
	BOOST_CHECK(   e10.is_tensor() );

	BOOST_CHECK( ! e11.empty    () );
	BOOST_CHECK( ! e11.is_scalar() );
	BOOST_CHECK( ! e11.is_vector() );
	BOOST_CHECK( ! e11.is_matrix() );
	BOOST_CHECK(   e11.is_tensor() );

	BOOST_CHECK( ! e12.empty    () );
	BOOST_CHECK( ! e12.is_scalar() );
	BOOST_CHECK( ! e12.is_vector() );
	BOOST_CHECK( ! e12.is_matrix() );
	BOOST_CHECK(   e12.is_tensor() );
}

BOOST_FIXTURE_TEST_CASE(test_extents_squeeze, fixture, *boost::unit_test::label("extents") *boost::unit_test::label("squeeze"))
{
	BOOST_REQUIRE_EQUAL(extents.size(),13);

	auto e0  = extents[ 0].squeeze(); // {}
	auto e1  = extents[ 1].squeeze(); // {1,1}
	auto e2  = extents[ 2].squeeze(); // {1,2}
	auto e3  = extents[ 3].squeeze(); // {2,1}

	auto e4  = extents[ 4].squeeze(); // {2,3}
	auto e5  = extents[ 5].squeeze(); // {2,3}
	auto e6  = extents[ 6].squeeze(); // {2,3}
	auto e7  = extents[ 7].squeeze(); // {2,3}
	auto e8  = extents[ 8].squeeze(); // {2,3}

	auto e9  = extents[ 9].squeeze(); // {4,2,3}
	auto e10 = extents[10].squeeze(); // {4,2,3}
	auto e11 = extents[11].squeeze(); // {4,2,3}
	auto e12 = extents[12].squeeze(); // {4,2,3}

	BOOST_CHECK( (e0  == extents_type{}   ) );
	BOOST_CHECK( (e1  == extents_type{1,1}) );
	BOOST_CHECK( (e2  == extents_type{1,2}) );
	BOOST_CHECK( (e3  == extents_type{2,1}) );

	BOOST_CHECK( (e4  == extents_type{2,3}) );
	BOOST_CHECK( (e5  == extents_type{2,3}) );
	BOOST_CHECK( (e6  == extents_type{2,3}) );
	BOOST_CHECK( (e7  == extents_type{2,3}) );
	BOOST_CHECK( (e8  == extents_type{2,3}) );

	BOOST_CHECK( (e9  == extents_type{4,2,3}) );
	BOOST_CHECK( (e10 == extents_type{4,2,3}) );
	BOOST_CHECK( (e11 == extents_type{4,2,3}) );
	BOOST_CHECK( (e12 == extents_type{4,2,3}) );

}

BOOST_FIXTURE_TEST_CASE(test_extents_valid, fixture, *boost::unit_test::label("extents") *boost::unit_test::label("valid"))
{

	using namespace boost::numeric;

	BOOST_REQUIRE_EQUAL(extents.size(),13);

	for(auto const& e : extents){
		if(e.empty())
			BOOST_CHECK_EQUAL(e.valid(),false);
		else
			BOOST_CHECK_EQUAL(e.valid(), true );
	}

	BOOST_CHECK_EQUAL( extents_type{}.valid() , false  );

	BOOST_CHECK_THROW( ublas::basic_extents<unsigned>({0,1}), std::length_error  );
	BOOST_CHECK_THROW( ublas::basic_extents<unsigned>({1,0,1}), std::length_error  );

}

BOOST_FIXTURE_TEST_CASE(test_extents_product, fixture, *boost::unit_test::label("extents") *boost::unit_test::label("product"))
{

	auto e0  = extents[ 0].product(); // {}
	auto e1  = extents[ 1].product(); // {1,1}
	auto e2  = extents[ 2].product(); // {1,2}
	auto e3  = extents[ 3].product(); // {2,1}
	auto e4  = extents[ 4].product(); // {2,3}
	auto e5  = extents[ 5].product(); // {2,3,1}
	auto e6  = extents[ 6].product(); // {1,2,3}
	auto e7  = extents[ 7].product(); // {1,1,2,3}
	auto e8  = extents[ 8].product(); // {1,2,3,1,1}
	auto e9  = extents[ 9].product(); // {4,2,3}
	auto e10 = extents[10].product(); // {4,2,1,3}
	auto e11 = extents[11].product(); // {4,2,1,3,1}
	auto e12 = extents[12].product(); // {1,4,2,1,3,1}

	BOOST_CHECK_EQUAL( e0 ,  0 );
	BOOST_CHECK_EQUAL( e1 ,  1 );
	BOOST_CHECK_EQUAL( e2 ,  2 );
	BOOST_CHECK_EQUAL( e3 ,  2 );
	BOOST_CHECK_EQUAL( e4 ,  6 );
	BOOST_CHECK_EQUAL( e5 ,  6 );
	BOOST_CHECK_EQUAL( e6 ,  6 );
	BOOST_CHECK_EQUAL( e7 ,  6 );
	BOOST_CHECK_EQUAL( e8 ,  6 );
	BOOST_CHECK_EQUAL( e9 , 24 );
	BOOST_CHECK_EQUAL( e10, 24 );
	BOOST_CHECK_EQUAL( e11, 24 );
	BOOST_CHECK_EQUAL( e12, 24 );

}

BOOST_AUTO_TEST_SUITE_END()


// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License as
// published by the Free Software Foundation, either version 3 of the
// License, or (at your option) any later version.
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Affero General Public License for more details.
// You should have received a copy of the GNU Affero General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

// Steffen Urban

#include "theia/sfm/filter_view_pairs_from_orientation.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <unordered_map>
#include <unordered_set>

#include "theia/math/rotation.h"
#include "theia/math/util.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"

namespace theia {

namespace {

bool AngularDifferenceIsAcceptable(
    const Eigen::Vector3d& orientation1,
    const Eigen::Vector3d& orientation2,
    const Eigen::Vector3d& relative_orientation,
    const double sq_max_relative_rotation_difference_radians) {
  const Eigen::Vector3d composed_relative_rotation =
      MultiplyRotations(orientation2, -orientation1);
  const Eigen::Vector3d loop_rotation =
      MultiplyRotations(-relative_orientation, composed_relative_rotation);
  const double sq_rotation_angular_difference_radians =
      loop_rotation.squaredNorm();
  return sq_rotation_angular_difference_radians <=
         sq_max_relative_rotation_difference_radians;
}

}  // namespace

void FilterViewPairsFromOrientation(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const double max_relative_rotation_difference_degrees,
    ViewGraph* view_graph) {
  CHECK_NOTNULL(view_graph);
  CHECK_GE(max_relative_rotation_difference_degrees, 0.0);

  // Precompute the squared threshold in radians.
  const double max_relative_rotation_difference_radians =
      DegToRad(max_relative_rotation_difference_degrees);
  const double sq_max_relative_rotation_difference_radians =
      max_relative_rotation_difference_radians *
      max_relative_rotation_difference_radians;

  std::unordered_set<ViewIdPair> view_pairs_to_remove;
  const auto& view_pairs = view_graph->GetAllEdges();
  for (const auto& view_pair : view_pairs) {
    const Eigen::Vector3d* orientation1 =
        FindOrNull(orientations, view_pair.first.first);
    const Eigen::Vector3d* orientation2 =
        FindOrNull(orientations, view_pair.first.second);

    // If the view pair contains a view that does not have an orientation then
    // remove it.
    if (orientation1 == nullptr || orientation2 == nullptr) {
      LOG(WARNING)
          << "View pair (" << view_pair.first.first << ", "
          << view_pair.first.second
          << ") contains a view that does not exist! Removing the view pair.";
      view_pairs_to_remove.insert(view_pair.first);
      continue;
    }

    // Remove the view pair if the relative rotation estimate is not within the
    // tolerance.
    if (!AngularDifferenceIsAcceptable(
            *orientation1,
            *orientation2,
            view_pair.second.rotation_2,
            sq_max_relative_rotation_difference_radians)) {
      view_pairs_to_remove.insert(view_pair.first);
    }
  }

  // Remove all the "bad" relative poses.
  for (const ViewIdPair view_id_pair : view_pairs_to_remove) {
    view_graph->RemoveEdge(view_id_pair.first, view_id_pair.second);
  }
  VLOG(1) << "Removed " << view_pairs_to_remove.size()
          << " view pairs by rotation filtering.";
}

}  // namespace theia


/*  ______________________________________________________________________

    PECOS: Parallel Environment for Creation Of Stochastics
    Copyright (c) 2011, Sandia National Laboratories.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Pecos directory.
    _______________________________________________________________________ */

//- Class:       BoostRNG_Monostate
//- Description: Wrapper for various implementations of Random Number Generators
//- Owner:       Laura Swiler, Dave Gay, and Bill Bohnhoff
//- Checked by:
//- Version: $Id$

#ifndef BOOST_RNG_MONOSTATE_HPP
#define BOOST_RNG_MONOSTATE_HPP

#include "pecos_data_types.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

// BoostRNG_Monostate helps replace the default LHS rnumlhs1/rnumlhs2
// RNGs. In standalone LHS, these functions are provided by LHS (see
// Lhs_drv.f90), but BoostRNG_Monostate provides them when LHS is compiled as a
// library for use by Dakota. This permits user-selectable RNG, either LHS
// default or mt19937 provided by Boost.
//
// Summary of random number generator replacement call chain:
//
// * LHS calls to RNG call RNUMLHS1, RNUMLHS2, which return double
//
// * BoostRNG_Monostate defines global symbols RNUMLHS1, RNUMLHS2 which
//   forward calls to BoostRNG_Monostate::randomNum(), randomNum2().
//   These replace those provided by LHS, so symbol resolution at link
//   time remains important.
//
// * randomNum/randomNum2 default to point to
//   BoostRNG_Monostate::mt19937 (Boost mt19937), but can be overridden by
//   LHSDriver to point to defaultrnum1, defaultrnum2, provided by LHS
//

namespace Pecos {

/// RNG function pointer type taking no arguments and returning Real
typedef Real (*Rfunc)();

/** The monostate BoostRNG class provides static pointers randomNum
    and randomNum2 that at runtime will either point to
    BoostRNG_Monostate::mt19937, or be overridden to point to
    defaultnum1 / defaultrnum2.  At construct time they point to the
    BoostRNG_Monostate generators. */
class BoostRNG_Monostate
{
private:

  //
  //- Heading: Data
  //

  static unsigned int rngSeed;
  static boost::mt19937 rnumGenerator;
  static boost::uniform_real<> uniDist;
  static boost::variate_generator<boost::mt19937&,boost::uniform_real<> > uniMT;

public:

  //
  //- Heading: Member functions
  //

  /// set randomSeed
  static void seed(unsigned int rng_seed);
  /// return randomSeed
  static unsigned int seed();

  static Real mt19937();

  //
  //- Heading: Data
  //

  /// Cached function pointers
  static Rfunc randomNum;
  static Rfunc randomNum2;
};

inline void BoostRNG_Monostate::seed(unsigned int rng_seed)
{ rngSeed = rng_seed; rnumGenerator.seed(rng_seed); }

inline unsigned int BoostRNG_Monostate::seed()
{ return rngSeed; }

inline Real BoostRNG_Monostate::mt19937()
{ return uniMT(); }

unsigned int BoostRNG_Monostate::rngSeed(41u); // 41 used in the Boost examples

boost::mt19937 BoostRNG_Monostate::rnumGenerator( BoostRNG_Monostate::seed() );

boost::uniform_real<> BoostRNG_Monostate::uniDist(0, 1);

boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
  BoostRNG_Monostate::uniMT(BoostRNG_Monostate::rnumGenerator,
                            BoostRNG_Monostate::uniDist);

Real (*BoostRNG_Monostate::randomNum)()  = BoostRNG_Monostate::mt19937;
Real (*BoostRNG_Monostate::randomNum2)() = BoostRNG_Monostate::mt19937;
} // namespace Pecos

// This section defines Fortran 90 functions rnumlhs1 and rnumlhs2,
// which forward to the BoostRNG_Monostate randomNum and randomNum2
// functions (which may further delegate to Boost or back to LHS
// defaultrnum1/defaultrnum2).  LHS will call Fortran-mangled versions of
// rnumlhs1 and rnumlhs2 throughout its code.

#ifdef HAVE_LHS
#include "LHS.h"
#define rnum1 LHS_GLOBAL(rnumlhs1,RNUMLHS1)
#define rnum2 LHS_GLOBAL(rnumlhs2,RNUMLHS2)
#endif  // HAVE_LHS

extern "C" Pecos::Real rnum1(void), rnum2(void);

// inline leads to DIFFs in the Regression suite
/* inline */ Pecos::Real rnum1(void)
{
  //PCout << "running Boost MT" << "\n";
  return Pecos::BoostRNG_Monostate::randomNum();
}

/* inline */ Pecos::Real rnum2(void)
{
  // clone of rnum1
  //PCout << "running Boost MT" << "\n";
  return Pecos::BoostRNG_Monostate::randomNum2();
}

#endif  // BOOST_RNG_MONOSTATE_HPP


#include <skelly_sim.hpp>

#include "cnpy.hpp"
#include <Eigen/Core>
#include <iostream>
#include <mpi.h>

#ifdef NDEBUG
#undef NDEBUG
#include <cassert>
#define NDEBUG
#else
#include <cassert>
#endif

#include <fiber.hpp>

using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;

MatrixXd load_mat(cnpy::npz_t &npz, const char *var) {
    return Map<Eigen::ArrayXXd>(npz[var].data<double>(), npz[var].shape[1], npz[var].shape[0]).matrix();
}

VectorXd load_vec(cnpy::npz_t &npz, const char *var) {
    return Map<Eigen::VectorXd>(npz[var].data<double>(), npz[var].shape[0]);
}

template <typename DerivedA, typename DerivedB>
bool allclose(
    const Eigen::DenseBase<DerivedA> &a, const Eigen::DenseBase<DerivedB> &b,
    const typename DerivedA::RealScalar &rtol = Eigen::NumTraits<typename DerivedA::RealScalar>::dummy_precision(),
    const typename DerivedA::RealScalar &atol = Eigen::NumTraits<typename DerivedA::RealScalar>::epsilon()) {
    return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all();
}

int main(int argc, char *argv[]) {
    cnpy::npz_t np_fib = cnpy::npz_load("np_fib.npz");

    MatrixXd x = load_mat(np_fib, "x");
    MatrixXd xs = load_mat(np_fib, "xs");
    MatrixXd xss = load_mat(np_fib, "xss");
    MatrixXd xsss = load_mat(np_fib, "xsss");
    MatrixXd xssss = load_mat(np_fib, "xssss");

    MatrixXd D_1_0 = load_mat(np_fib, "D_1_0");
    MatrixXd D_2_0 = load_mat(np_fib, "D_2_0");
    MatrixXd D_3_0 = load_mat(np_fib, "D_3_0");
    MatrixXd D_4_0 = load_mat(np_fib, "D_4_0");

    MatrixXd P_X = load_mat(np_fib, "P_X").transpose();
    MatrixXd P_T = load_mat(np_fib, "P_T").transpose();
    MatrixXd P_downsample_bc = load_mat(np_fib, "P_downsample_bc").transpose();

    MatrixXd force_external = load_mat(np_fib, "force_external");
    MatrixXd flow_on = load_mat(np_fib, "flow_on");

    MatrixXd A = load_mat(np_fib, "A").transpose();
    MatrixXd ABC = load_mat(np_fib, "ABC").transpose();
    VectorXd RHS = load_vec(np_fib, "RHS");
    VectorXd RHSBC = load_vec(np_fib, "RHSBC");
    auto &mat = Fiber::matrices_.at(x.cols());

    MatrixXd force_operator = load_mat(np_fib, "force_operator").transpose();

    assert(allclose(mat.D_1_0, D_1_0, 0, 1E-14));
    assert(allclose(mat.D_2_0, D_2_0, 0, 1E-14));
    assert(allclose(mat.D_3_0, D_3_0, 0, 1E-14));
    assert(allclose(mat.D_4_0, D_4_0, 0, 1E-14));
    assert(allclose(mat.P_X, P_X, 0, 1E-14));
    assert(allclose(mat.P_T, P_T, 0, 1E-14));
    assert(allclose(mat.P_downsample_bc, P_downsample_bc, 0, 1E-12));

    double bending_rigidity = *np_fib["bending_rigidity"].data<double>();
    double length = *np_fib["length"].data<double>();
    double eta = *np_fib["eta"].data<double>();
    double dt = *np_fib["dt"].data<double>();

    Fiber fib(x.cols(), bending_rigidity, eta);
    fib.length_ = length;
    fib.length_prev_ = length;
    fib.x_ = x;
    fib.update_derivatives();
    fib.update_stokeslet(eta);
    fib.update_linear_operator(dt, eta);
    fib.update_force_operator();

    assert(allclose(fib.x_, x, 0, 1E-12));
    assert(allclose(fib.xs_, xs, 0, 1E-12));
    assert(allclose(fib.xss_, xss, 0, 1E-12));
    assert(allclose(fib.xsss_, xsss, 0, 1E-12));
    assert(allclose(fib.xssss_, xssss, 0, 1E-12));

    MatrixXd stokeslet = load_mat(np_fib, "stokeslet");
    assert(allclose(fib.stokeslet_, stokeslet));

    int np = fib.n_nodes_;

    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            Eigen::MatrixXd Apy = A.block(i * np, j * np, np, np);
            Eigen::MatrixXd Acpp = fib.A_.block(i * np, j * np, np, np);
            assert(allclose(Apy, Acpp, 0, 1E-9));
        }
    }

    // Test right-hand-side computation
    fib.update_RHS(dt, flow_on, force_external);
    assert(allclose(RHS, fib.RHS_));

    // Test BC calculation
    fib.bc_minus_ = {Fiber::BC::Velocity, Fiber::BC::AngularVelocity};
    fib.bc_plus_ = {Fiber::BC::Force, Fiber::BC::Torque};
    fib.apply_bc_rectangular(dt, flow_on, force_external);
    // FIXME: Test fails. input data doesn't have properly set Force BC on the RHS
    // assert(allclose(RHSBC, fib.RHS_));

    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            Eigen::MatrixXd Apy = ABC.block(i * np, j * np, np, np);
            Eigen::MatrixXd Acpp = fib.A_.block(i * np, j * np, np, np);
            assert(allclose(Acpp, Apy, 0, 1E-9));
        }
    }

    fib.update_force_operator();
    assert(allclose(force_operator, fib.force_operator_, 0, 1E-7));

    std::cout << "Test passed\n";
    return 0;
}


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#include "poplar/Target.hpp"
#include "poplibs_test/Check.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/GatherStatistics.hpp"
#include "popops/codelets.hpp"
#include <boost/program_options.hpp>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poputil/TileMapping.hpp>
#include <stdexcept>
#include <string.h>
#include <utility>

using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace poplibs_test::util;
using namespace poputil;

using namespace poplibs_support;
namespace po = boost::program_options;
// Host model of the histogram.  Intentionally coded in a straightforward way
template <typename HistType>
std::vector<HistType>
histogram(const std::vector<float> &data, const std::vector<float> &limits,
          const std::vector<HistType> &initialHistogram, bool isAbsolute) {

  std::vector<HistType> histogram = initialHistogram;
  for (unsigned i = 0; i < data.size(); i++) {
    auto dataElem = data[i];
    if (isAbsolute) {
      dataElem = std::fabs(dataElem);
    }
    for (unsigned j = 0; j < limits.size(); j++) {
      if (j == 0 && dataElem < limits[j]) {
        // Below the lower bound
        histogram[j]++;
      } else {
        if (j == limits.size() - 1 && dataElem >= limits[j]) {
          // Above the upper bound
          histogram[j + 1]++;
        }
        if (dataElem < limits[j] && dataElem >= limits[j - 1]) {
          // Between a bound and the one below it
          histogram[j]++;
        }
      }
    }
  }
  return histogram;
}

template <typename HistType>
bool doTest(TestDevice &device, DeviceType deviceType, bool profile,
            const std::vector<double> &data, const std::vector<float> &limits,
            const std::vector<HistType> &initialHistogram,
            bool useFloatArithmetic, bool withOutput, bool update,
            const poplar::Type &dataType, bool isAbsolute,
            bool twoDInputTensor) {
  auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  const auto rawSize = target.getTypeSize(dataType);

  std::vector<char> rawData(data.size() * rawSize);
  std::vector<char> rawLimits(limits.size() * rawSize);

  copy(target, data.data(), data.size(), dataType, rawData.data());
  copy(target, limits.data(), limits.size(), dataType, rawLimits.data());

  const auto dataSize = data.size();

  auto ipuData = twoDInputTensor
                     ? graph.addVariable(dataType, {2, dataSize / 2})
                     : graph.addVariable(dataType, {dataSize});
  auto ipuLimits = graph.addVariable(dataType, {limits.size()});
  // Map transpose to get non contiguous regions on a tile
  mapTensorLinearly(graph, twoDInputTensor ? ipuData.transpose() : ipuData);
  graph.setTileMapping(ipuLimits, 0);

  auto prog = Sequence();
  poplar::OptionFlags options = {
      {"useFloatArithmetic", useFloatArithmetic ? "true" : "false"}};
  Tensor ipuHistogram;
  if (withOutput) {
    ipuHistogram = graph.addVariable(useFloatArithmetic ? FLOAT : UNSIGNED_INT,
                                     {limits.size() + 1});
    mapTensorLinearly(graph, ipuHistogram);
    graph.createHostWrite("histogram", ipuHistogram);

    popops::histogram(graph, ipuData, ipuHistogram, update, ipuLimits,
                      isAbsolute, prog, "Test Histogram");
  } else {
    ipuHistogram = popops::histogram(graph, ipuData, ipuLimits, isAbsolute,
                                     prog, "Test Histogram", options);
  }
  graph.createHostRead("histogram", ipuHistogram);
  graph.createHostWrite("data", ipuData);
  graph.createHostWrite("limits", ipuLimits);

  OptionFlags engineOptions;
  if (profile) {
    engineOptions.set("debug.instrumentCompute", "true");
  }
  Engine e(graph, prog, engineOptions);
  std::vector<HistType> resultHistogram(limits.size() + 1);

  device.bind([&](const Device &d) {
    e.load(d);

    e.writeTensor("data", rawData.data(), rawData.data() + rawData.size());
    e.writeTensor("limits", rawLimits.data(),
                  rawLimits.data() + rawLimits.size());
    if (withOutput) {
      e.writeTensor("histogram", initialHistogram.data(),
                    initialHistogram.data() + initialHistogram.size());
    }
    e.run();
    e.readTensor("histogram", resultHistogram.data(),
                 resultHistogram.data() + resultHistogram.size());
  });

  // Cast half data back to get float data that represents half accuracy, or
  // just copy if float.  This matters where limits are equal to the histogram
  // entries in the numeric format of the codelet.
  std::vector<float> dataConvert(data.size());
  std::vector<float> limitsConvert(limits.size());
  copy(target, dataType, rawData.data(), dataConvert.data(),
       dataConvert.size());
  copy(target, dataType, rawLimits.data(), limitsConvert.data(),
       limitsConvert.size());

  bool success = true;
  // Generate host result, compare
  auto hostResult = histogram<HistType>(dataConvert, limitsConvert,
                                        initialHistogram, isAbsolute);

  std::cout << "Limits:" << limitsConvert << "\n";

  std::cout << "    Hist:" << resultHistogram << "\nExpected:" << hostResult;
  std::cout << "\n";

  for (unsigned i = 0; i < hostResult.size(); ++i) {
    if (hostResult[i] != resultHistogram[i]) {
      success = false;
      break;
    }
  }

  if (profile && deviceType != DeviceType::Cpu) {
    e.printProfileSummary(std::cout,
                          OptionFlags{{"showExecutionSteps", "true"}});
  }
  return success;
}

int main(int argc, char **argv) {
  DeviceType deviceType = DeviceType::IpuModel2;
  // Default input parameters.
  Type dataType = FLOAT;
  bool isAbsolute = false;
  bool useFloatArithmetic = false;
  bool withOutput = false;
  bool update = false;
  bool twoD = false;
  unsigned innermostDimSize;
  unsigned limitSize;
  unsigned tiles = 4;
  double dataMin = -65504;
  double dataRange = 65504 * 2;
  float limitsMin = -60000;
  float limitsRange = 60000 * 2;
  float limitsStep = 0.0;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Produce help message")
    ("device-type",
       po::value<DeviceType>(&deviceType)->default_value(deviceType),
       deviceTypeHelp)
    ("tiles-per-ipu", po::value(&tiles)->default_value(tiles),
      "Number of tiles per IPU")
    ("profile", "Output profiling report")
    ("type", po::value(&dataType)->default_value(dataType),
      "Data type of data and limits")
    ("two-d", po::value(&twoD)->default_value(twoD),
     "Use 2D input tensor (data size must be even in such case")
    ("inner-dim-size", po::value(&innermostDimSize)->required(),
      "Number of data elements in the innermost dimension (number of elements "
      "are twice this if 2d is enabled")
    ("data-min", po::value(&dataMin)->default_value(dataMin),
      "Minimum data value")
    ("data-range", po::value(&dataRange)->default_value(dataRange),
      "Data range")
    ("limits-size", po::value(&limitSize)->required(),
      "Number of limits, histogram results = limits + 1")
    ("limits-min", po::value(&limitsMin)->default_value(limitsMin),
      "Minimum limit")
    ("limits-range", po::value(&limitsRange)->default_value(limitsRange),
      "Limits range")
    ("limits-step", po::value(&limitsStep)->default_value(limitsStep),
      "Limits step, found from range unless this parameter is used")
    ("absolute", po::value(&isAbsolute)->default_value(isAbsolute),
      "Use absolute values of the data")
    ("use-float-arithmetic", po::value(&useFloatArithmetic)->default_value(useFloatArithmetic),
      "Use float arithmetic, producing a float result for speed")
    ("with-output", po::value(&withOutput)->default_value(withOutput),
      "Provide an output external to the histogram function")
    ("update", po::value(&update)->default_value(update),
      "Update (continue to gather) histogram results, implies with-output=true")
    ;
  // clang-format on
  po::variables_map vm;
  bool profile = false;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    if (vm.count("profile")) {
      profile = true;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error parsing command line: " << e.what() << "\n";
    return 1;
  }

  if (update) {
    withOutput = true;
  }

  const auto dataSize = innermostDimSize * (1 + twoD);

  // Random data within the range specified
  std::mt19937 randomEngine;
  std::vector<double> data(dataSize);
  auto device = createTestDevice(deviceType, 1, tiles);
  writeRandomValues(device.getTarget(), dataType, data.data(),
                    data.data() + data.size(), dataMin, dataMin + dataRange,
                    randomEngine);

  // Evenly spaced limits
  std::vector<float> limits(limitSize);
  if (limitsStep == 0.0) {
    limitsStep = limits.size() == 1
                     ? 1
                     : limitsRange / static_cast<float>(limits.size() - 1);
  }
  for (unsigned i = 0; i < limits.size(); i++) {
    limits[i] = limitsMin + static_cast<float>(i) * limitsStep;
  }
  bool success;
  if (useFloatArithmetic) {
    std::vector<float> initialHistogram(limitSize + 1);
    for (unsigned i = 0; i < limitSize + 1; i++) {
      initialHistogram[i] = update ? i + 1 : 0;
    }
    success = doTest<float>(device, deviceType, profile, data, limits,
                            initialHistogram, useFloatArithmetic, withOutput,
                            update, dataType, isAbsolute, twoD);
  } else {
    std::vector<unsigned> initialHistogram(limitSize + 1);
    for (unsigned i = 0; i < limitSize + 1; i++) {
      initialHistogram[i] = update ? i + 1 : 0;
    }
    success = doTest<unsigned>(device, deviceType, profile, data, limits,
                               initialHistogram, useFloatArithmetic, withOutput,
                               update, dataType, isAbsolute, twoD);
  }
  if (!success) {
    std::cerr << "Failure\n";
  }
  return !success;
}


// Boost.Geometry

// Copyright (c) 2015-2020 Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_UTIL_HAS_NAN_COORDINATE_HPP
#define BOOST_GEOMETRY_UTIL_HAS_NAN_COORDINATE_HPP

#include <cstddef>
#include <type_traits>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

struct isnan
{
    template <typename T>
    static inline bool apply(T const& t)
    {
        return boost::math::isnan(t);
    }
};

template
<
    typename Point,
    typename Predicate,
    bool Enable,
    std::size_t I = 0,
    std::size_t N = geometry::dimension<Point>::value
>
struct has_coordinate_with_property
{
    static bool apply(Point const& point)
    {
        return Predicate::apply(geometry::get<I>(point))
            || has_coordinate_with_property
                <
                    Point, Predicate, Enable, I+1, N
                >::apply(point);
    }
};

template <typename Point, typename Predicate, std::size_t I, std::size_t N>
struct has_coordinate_with_property<Point, Predicate, false, I, N>
{
    static inline bool apply(Point const&)
    {
        return false;
    }
};

template <typename Point, typename Predicate, std::size_t N>
struct has_coordinate_with_property<Point, Predicate, true, N, N>
{
    static bool apply(Point const& )
    {
        return false;
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

template <typename Point>
bool has_nan_coordinate(Point const& point)
{
    return detail::has_coordinate_with_property
        <
            Point,
            detail::isnan,
            std::is_floating_point
                <
                    typename coordinate_type<Point>::type
                >::value
        >::apply(point);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_HAS_NAN_COORDINATE_HPP


///////////////////////////////////////////////////////////////////////////////
/// \file piecewise_surface_sample.hpp
/// Calculate a piecewise constant surface by sampling series at
/// steps defined by two range representing the sample points
/// for x and y coordinate of the surface.
//
//  Copyright 2006 Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TIME_SERIES_NUMERIC_PIECEWISE_SURFACE_SAMPLE_060615_HPP
#define BOOST_TIME_SERIES_NUMERIC_PIECEWISE_SURFACE_SAMPLE_060615_HPP

#include <boost/implicit_cast.hpp>
#include <boost/concept/requires.hpp>
#include <boost/detail/range_concepts.hpp>
#include <boost/range_run_storage/algorithm/transform.hpp>
#include <boost/time_series/concepts.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/numeric/detail/tagged_ops.hpp>
#include <boost/time_series/numeric/piecewise_sample.hpp>

namespace boost { namespace time_series
{

    namespace detail
    {
        namespace rrs = range_run_storage;

        template<typename Value, typename Offset, typename Out, typename Periods>
        struct piecewise_surface_sample_inserter
        {
            template<typename Series>
            piecewise_surface_sample_inserter(Series const &series, Periods const &periods, Out const &out)
              : out_(out)
              , value_(rrs::get_at(series, -inf))
              , zero_(implicit_cast<Value const &>(rrs::zero(series)))
              , offset_(-inf)
              , periods_(periods)
            {}

            template<typename Run, typename Arg>
            void set_at(Run const &run, left<Arg> const &value)
            {}

            template<typename Run, typename Arg>
            void set_at(Run const &run, right<Arg> const &value)
            {
                this->out_(
                    time_series::piecewise_sample(this->value_, this->periods_)
                  , this->offset_
                  , rrs::offset(run)
                );
                this->offset_ = rrs::offset(run);
                this->value_ = this->zero_;
            }

            template<typename Run, typename Left, typename Right>
            void set_at(Run const &run, both<Left, Right> const &value)
            {
                this->out_(
                    time_series::piecewise_sample(this->value_, this->periods_)
                  , this->offset_
                  , rrs::offset(run)
                );
                this->offset_ = rrs::offset(run);
                this->value_ = value.left;
            }

            Out const &finalize()
            {
                this->out_(
                    time_series::piecewise_sample(this->value_, this->periods_)
                  , this->offset_
                  , inf
                );
                return this->out_;
            }
        private:
            Out out_;
            Value value_;
            Value zero_;
            Offset offset_;
            Periods const &periods_;
        };

    } // namespace detail

    /// \brief Calculate a piecewise constant surface by sampling series at
    ///     steps defined by two ranges representing the sample points
    ///     for x and y coordinate of the surface.
    ///
    /// Calculate a piecewise constant surface by sampling series at
    ///     steps defined by two ranges representing the sample points
    ///     for x and y coordinate of the surface. The \c x periods are specified
    ///     as follows:
    /// \li <tt> \verbatim [-inf,*boost::begin(x_periods)) \endverbatim </tt>
    /// \li <tt> \verbatim [*boost::begin(x_periods),*(boost::begin(x_periods)+1)) \endverbatim </tt>
    /// \li <tt> \verbatim [*(boost::begin(x_periods)+1),*(boost::begin(x_periods)+2)) \endverbatim </tt>
    /// \li <tt> \verbatim ... \endverbatim </tt>
    /// \li <tt> \verbatim [*(boost::end(x_periods)-1),inf) \endverbatim </tt>
    ///
    /// ... and likewise for \c y periods.
    ///
    /// \param series The series of series to sample
    /// \param x_periods The x coordinates of the surface
    /// \param y_periods The y coordinates of the surface
    /// \param out The inserter into which the new series is written
    /// \pre \c Series is a model of the \c TimeSeries concept.
    /// \pre The \c value_type of \c Series is a model of the \c TimeSeries concept.
    /// \pre \c XPeriods is a model of the \c RandomAccessRange concept.
    /// \pre \c YPeriods is a model of the \c RandomAccessRange concept.
    /// \pre \c Out is a model of the \c OrderedInserter concept.
    template<typename Series, typename XPeriods, typename YPeriods, typename Out>
    BOOST_CONCEPT_REQUIRES(
        ((concepts::TimeSeries<Series const>))
        ((concepts::TimeSeries<typename concepts::TimeSeries<Series const>::value_type>))
        ((concepts::RandomAccessRange<XPeriods /*const*/>))
        ((concepts::RandomAccessRange<YPeriods /*const*/>))
        ,
    (ordered_inserter<Out>))
    piecewise_surface_sample(Series const &series, XPeriods const &x_periods, YPeriods const &y_periods, ordered_inserter<Out> out)
    {
        typedef typename concepts::TimeSeries<Series const>::value_type value_type;
        typedef typename concepts::RandomAccessRange<XPeriods /*const*/>::value_type offset_type;

        detail::periods_storage_adaptor<XPeriods const> x_periods_adaptr(x_periods);
        detail::piecewise_surface_sample_inserter<value_type, offset_type, ordered_inserter<Out>, YPeriods>
            o(series, y_periods, out);

        return range_run_storage::transform(
            series
          , x_periods_adaptr
          , detail::both_op
          , detail::left_op
          , detail::right_op
          , o
        ).finalize();
    }

    /// \overload
    ///
    template<typename Series, typename XPeriods, typename YPeriods>
    BOOST_CONCEPT_REQUIRES(
        ((concepts::TimeSeries<Series const>))
        ((concepts::TimeSeries<typename concepts::TimeSeries<Series const>::value_type>))
        ((concepts::RandomAccessRange<XPeriods /*const*/>))
        ((concepts::RandomAccessRange<YPeriods /*const*/>))
        ,
    (piecewise_constant_series<
        typename concepts::TimeSeries<Series const>::value_type
      , typename concepts::TimeSeries<Series const>::discretization_type
      , typename concepts::RandomAccessRange<XPeriods /*const*/>::value_type
    >))
    piecewise_surface_sample(Series const &series, XPeriods const &x_periods, YPeriods const &y_periods)
    {
        typedef typename concepts::TimeSeries<Series const>::value_type value_type;
        typedef typename concepts::TimeSeries<Series const>::discretization_type discretization_type;
        typedef typename concepts::RandomAccessRange<XPeriods /*const*/>::value_type offset_type;

        piecewise_constant_series<value_type, discretization_type, offset_type> result(
            time_series::discretization = series.discretization()
          , time_series::zero = range_run_storage::zero(series)
        );

        time_series::piecewise_surface_sample(
            series
          , x_periods
          , y_periods
          , time_series::make_ordered_inserter(result)
        ).commit();

        return result;
    }

}} // namespace boost::time_series

#endif // BOOST_TIME_SERIES_NUMERIC_PIECEWISE_SURFACE_SAMPLE_060615_HPP


typedef void(*function_t)(int);

function_t higher_order_function1();
void higher_order_function2(function_t f);
function_t higher_order_function3(function_t f);

template <class Param1, class Param2, class Func1, class Func2>
struct coalesce;

#include <boost/mpl/apply.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>

template <class Param1, class Param2, class Func1, class Func2>
struct coalesce {
    typedef typename boost::mpl::apply<Func1, Param1>::type type1;
    typedef typename boost::mpl::apply<Func2, Param2>::type type2;

    typedef typename boost::mpl::if_<
        boost::is_same< boost::mpl::false_, type1>,
        type2,
        type1
    >::type type;
};

#include <boost/static_assert.hpp>
#include <boost/mpl/not.hpp>

using boost::mpl::_1;
using boost::mpl::_2;

typedef coalesce<
    boost::mpl::true_,
    boost::mpl::true_,
    boost::mpl::not_<_1>,
    boost::mpl::not_<_1>
>::type res1_t;
BOOST_STATIC_ASSERT((!res1_t::value));

typedef coalesce<
    boost::mpl::true_,
    boost::mpl::false_,
    boost::mpl::not_<_1>,
    boost::mpl::not_<_1>
>::type res2_t;
BOOST_STATIC_ASSERT((res2_t::value));

typedef coalesce<
    boost::mpl::false_,
    boost::mpl::false_,
    boost::mpl::not_<_1>,
    boost::mpl::not_<boost::mpl::not_<_1> >
>::type res3_t;
BOOST_STATIC_ASSERT((res3_t::value));

typedef coalesce<
    boost::mpl::false_,
    boost::mpl::true_,
    boost::mpl::not_<_1>,
    boost::mpl::not_<boost::mpl::not_<_1> >
>::type res4_t;
BOOST_STATIC_ASSERT((res4_t::value));

#include <iostream>
#include <typeinfo>
int main() {
    // std::cerr << typeid(res2_t).name() << '\n';
}


#include "catch.hpp"
#include <boost/icl/interval.hpp>
#include <boost/icl/interval_set.hpp>
#include <boost/icl/map.hpp> // required only for boost::icl::partial_enricher.
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/interval_map.hpp>
#include <boost/optional.hpp>
#include <iostream>

TEST_CASE("interval tests", "[boost]")
{
    typedef boost::icl::interval<int> Interval;

    SECTION("closed intervals")
    {
        auto ival = Interval::closed(1, 3);
        REQUIRE(ival.lower() == 1);
        REQUIRE(ival.upper() == 3);
        REQUIRE(contains(ival, 1) == true);
        REQUIRE(contains(ival, 3) == true);
    }

    SECTION("open intervals")
    {
        auto ival = Interval::open(1, 3);
        REQUIRE(ival.lower() == 1);
        REQUIRE(ival.upper() == 3);
        REQUIRE(contains(ival, 1) == false);
        REQUIRE(contains(ival, 3) == false);
    }

    SECTION("right open intervals")
    {
        auto ival = Interval::right_open(1, 3);
        REQUIRE(ival.lower() == 1);
        REQUIRE(ival.upper() == 3);
        REQUIRE(contains(ival, 1) == true);
        REQUIRE(contains(ival, 3) == false);
    }

    SECTION("left open intervals")
    {
        auto ival = Interval::left_open(1, 3);
        REQUIRE(ival.lower() == 1);
        REQUIRE(ival.upper() == 3);
        REQUIRE(contains(ival, 1) == false);
        REQUIRE(contains(ival, 3) == true);
    }
}

TEST_CASE("interval set tests", "[boost]")
{
    typedef boost::icl::interval<int> Interval;

    SECTION("interval sets")
    {
        // intervals joined on overlap or touch.
        typedef boost::icl::interval_set<int> IntervalSet;
        auto iset = IntervalSet{};

        iset += Interval::closed(1, 3);
        REQUIRE((iset == IntervalSet(Interval::closed(1, 3))));

        iset += Interval::open(2, 5);
        REQUIRE((iset == IntervalSet(Interval::right_open(1, 5))));

        iset += Interval::closed(5, 7);
        REQUIRE((iset == IntervalSet(Interval::closed(1, 7))));

        iset += Interval::closed(10, 12);
        REQUIRE(contains(iset, Interval::closed(2, 6)));
        REQUIRE(contains(iset, Interval::closed(10, 12)));
        REQUIRE_FALSE(contains(iset, Interval::closed(8, 9)));
        REQUIRE_FALSE(contains(iset, Interval::closed(8, 11)));
    }

    SECTION("separate interval sets")
    {
        // intervals joined on overlap, not touch.
        typedef boost::icl::separate_interval_set<int> SeparateIntervalSet;
        auto se_iset = SeparateIntervalSet{};

        se_iset += Interval::right_open(1, 3);
        se_iset += Interval::closed(3, 5);

        // As se_iset is {[1, 3), [3, 5]}, it cannot equal [1,5].
        REQUIRE_FALSE((se_iset == SeparateIntervalSet(Interval::closed(1, 5))));

        se_iset += Interval::closed(2, 4);

        // Now, the disjoint intervals are connected and make se_iset [1,5].
        REQUIRE((se_iset == SeparateIntervalSet(Interval::closed(1, 5))));
    }

    SECTION("split interval sets")
    {
        // intervals split on overlap, borders preserved.
        typedef boost::icl::split_interval_set<int> SplitIntervalSet;
        auto sp_iset = SplitIntervalSet{};

        sp_iset += Interval::right_open(1, 3);
        sp_iset += Interval::right_open(2, 5);

        auto res = SplitIntervalSet{};
        res += Interval::right_open(1, 2);
        res += Interval::right_open(2, 3);
        res += Interval::right_open(3, 5);
        REQUIRE((sp_iset == res));
    }
}

TEST_CASE("interval map tests", "[boost]")
{
    // interval maps have the "identity absorber" property. This prevents
    // storing identities as values. For example, the following values cannot
    // be stored:
    // - int values cannot store 0.
    // - std::string values cannot store "".
    // - set<T> values cannot store {}.
    typedef boost::icl::interval<int> Interval;
    typedef boost::icl::discrete_interval<int> DiscreteInterval;

    SECTION("interval maps")
    {
        typedef boost::icl::interval_map<int, int> IntervalMap;
        auto imap = IntervalMap{};

        imap += std::make_pair(Interval::closed(6, 8), 1);
        imap += std::make_pair(Interval::closed(7, 9), 2);

        std::vector<int> key_lowers, key_uppers, vals;
        std::vector<DiscreteInterval> keys;
        for (auto it = imap.begin(); it != imap.end(); ++it)
        {
            key_lowers.push_back(it->first.lower());
            key_uppers.push_back(it->first.upper());
            vals.push_back(it->second);
            keys.push_back(it->first);
        }

        REQUIRE((key_lowers == std::vector<int>({6, 7, 8})));
        REQUIRE((key_uppers == std::vector<int>({7, 8, 9})));
        REQUIRE((vals == std::vector<int>({1, 3, 2})));
        REQUIRE((keys == std::vector<DiscreteInterval>(
                             {DiscreteInterval::right_open(6, 7),
                              DiscreteInterval::closed(7, 8),
                              DiscreteInterval::left_open(8, 9)})));
    }

    SECTION("enriched interval maps")
    {
        typedef boost::icl::interval_map<int, int, boost::icl::partial_enricher> EIMap;

        // The third template above is a trait of the interval map.
        // There are 4 possible traits:
        // - partial_absorber
        // - partial_enricher
        // - total absorber
        // - total enricher
        // "partial" means that the map is defined only for keys that have been
        // inserted.
        // "total" means that the map is considered to have a "neutral" value
        // for all keys not stored in the map.
        // "absorber" means that the identity element with respect to "+"
        // cannot be stored as a value. For example, 0 for int types and "" for
        // std::string types.
        // "enricher" allows storing the identity element.

        auto eimap = EIMap{};

        eimap += std::make_pair(Interval::closed(1, 10), 0);
        eimap += std::make_pair(Interval::closed(3, 5), 1);
        eimap += std::make_pair(Interval::closed(8, 10), 3);

        std::vector<DiscreteInterval> keys;
        std::vector<int> vals;
        for (auto it = eimap.begin(); it != eimap.end(); ++it)
        {
            keys.push_back(it->first);
            vals.push_back(it->second);
        }

        REQUIRE((keys == std::vector<DiscreteInterval>(
                             {DiscreteInterval::right_open(1, 3),
                              DiscreteInterval::closed(3, 5),
                              DiscreteInterval::open(5, 8),
                              DiscreteInterval::closed(8, 10)})));
        REQUIRE((vals == std::vector<int>({0, 1, 0, 3})));
    }
}

TEST_CASE("optional test", "[boost]")
{
    SECTION("optional construction")
    {
        auto optInt = boost::optional<int>{};
        REQUIRE_FALSE(optInt.is_initialized());
        optInt = 1;
        REQUIRE(optInt.is_initialized());
        REQUIRE(optInt.get() == 1);
    }

    SECTION("optional returning function")
    {
        auto positivePart = [](int num) -> boost::optional<int> {
            return num > 0 ? num : boost::optional<int>{};
        };

        auto optVal = positivePart(5);
        REQUIRE(optVal.is_initialized());
        REQUIRE(optVal.get() == 5);
        REQUIRE_FALSE(positivePart(-5).is_initialized());
    }
}