/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014 Max-Planck-Institute for Intelligent Systems,
 *                     University of Southern California
 *    Jan Issac (jan.issac@gmail.com)
 *    Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 *
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * @date 2014
 * @author Jan Issac (jan.issac@gmail.com)
 * Max-Planck-Institute for Intelligent Systems,
 * University of Southern California
 */

#include <gtest/gtest.h>

#include <memory>

#include <Eigen/Dense>

#include <fl/exception/exception.hpp>
#include <fl/filter/gaussian/gaussian_filter.hpp>
#include <fl/filter/gaussian/unscented_transform.hpp>

#include "gaussian_filter_stubs.hpp"

template <typename FilteringAlgorithm>
class FilteringContext
{
public:
    typedef fl::FilterInterface<FilteringAlgorithm> Filter;

    FilteringContext(const typename Filter::Ptr& filter)
        : filter_(filter)
    {

    }

    void predict()
    {
        filter_->predict(0.0, typename Filter::Input(), state_distr_, state_distr_);
    }

    void update()
    {
        filter_->update(typename Filter::Observation(), state_distr_, state_distr_);
    }

    typename Filter::Ptr filter()
    {
        return filter_;
    }

    typename Filter::Ptr filter_;
    typename Filter::Observation y_;
    typename Filter::StateDistribution state_distr_;
};

TEST(GaussianFilter, some_test)
{
    /* ===  Step 1: define vectorial types === */
    typedef Eigen::Matrix<double, 11,  1> State;
    typedef Eigen::Matrix<double, 100, 1> Obsrv;
    typedef Eigen::Matrix<double, 3,   1> Input;
    typedef Eigen::Matrix<double, 6,   1> StateNoise;
    typedef Eigen::Matrix<double, 100, 1> ObsrvNoise;

    /* == Step 2: define models == */
    typedef ProcessModelStub<State, StateNoise, Input> ProcessModel;
    typedef ObservationModelStub<State, Obsrv, ObsrvNoise> ObservationModel;

    /* == Step 3: define the custom filter algorithm == */
    typedef fl::GaussianFilter<
                ProcessModel,
                ObservationModel,
                fl::UnscentedTransform
            > UnscentedKalmanFilter;

    typedef fl::FilterInterface<UnscentedKalmanFilter> Filter;

    std::shared_ptr<Filter> filter =
        std::make_shared<UnscentedKalmanFilter>(
            std::make_shared<ProcessModel>(),
            std::make_shared<ObservationModel>(),
            std::make_shared<fl::UnscentedTransform>());

    typename Filter::StateDistribution state_distr;

    filter->predict(0.0, Input(), state_distr, state_distr);
    filter->update(Obsrv(), state_distr, state_distr);
    filter->predict_and_update(0., Input(), Obsrv(), state_distr, state_distr);
}

//TEST(GaussianFilter, context_test)
//{
//    /* ===  Step 1: define vectorial types === */
//    typedef Eigen::Matrix<double, 7,  1> State;
//    typedef Eigen::Matrix<double, 4, 1> Obsrv;
//    typedef Eigen::Matrix<double, 3,   1> Input;
//    typedef Eigen::Matrix<double, 6,   1> StateNoise;
//    typedef Eigen::Matrix<double, 4, 1> ObsrvNoise;

//    /* == Step 2: define models == */
//    typedef ProcessModelStub<State, StateNoise, Input> ProcessModel;
//    typedef ObservationModelStub<State, Obsrv, ObsrvNoise> ObservationModel;

//    /* == Step 3: define filter algorithm == */
//    typedef fl::GaussianFilter<
//                ProcessModel,
//                ObservationModel,
//                fl::UnscentedTransform
//            > UnscentedKalmanFilter;

//    FilteringContext<UnscentedKalmanFilter> filter_context(
//        std::make_shared<UnscentedKalmanFilter>(
//            std::make_shared<ProcessModel>(),
//            std::make_shared<ObservationModel>(),
//            std::make_shared<fl::UnscentedTransform>()));

//    filter_context.predict();
//    filter_context.update();
//}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120323    K. Kumar          Created based off of unit test for RKF45 integrator; test data
 *                                  taken from (The Mathworks, 2012).
 *      120328    K. Kumar          Moved (Burden and Faires, 2011) test class to its own file.
 *      120404    K. Kumar          Updated MATLAB unit test by adding discrete-event data file.
 *      130116    K. Kumar          Rewrote unit test to make use of testing code for numerical
 *                                  integrators migrated to Tudat Core.
 *      130909    K. Kumar          Updated error tolerances for MuPAD-based tests.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *      The Mathworks, Inc. RKF87, Symbolic Math Toolbox, 2012.
 *
 *    Notes
 *      All the test for this integrator are based on the data generated using the Symbolic Math
 *      Toolbox (MathWorks, 2012). Ideally, another source of data should be used to complete the
 *      testing.
 *
 *      The single step and full integration error tolerances were picked to be as small as
 *      possible, without causing the tests to fail. These values are not deemed to indicate any
 *      bugs in the code; however, it is important to take these discrepancies into account when
 *      using this numerical integrator.
 *
 */

#define BOOST_TEST_MAIN

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

#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/reinitializableNumericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTests.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h"

#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"

#include <limits>
#include <string>

#include <Eigen/Core>

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_runge_kutta_fehlberg_78_integrator )

using linear_algebra::flipMatrixRows;

using numerical_integrators::NumericalIntegratorXdPointer;
using numerical_integrators::ReinitializableNumericalIntegratorXdPointer;
using numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd;
using numerical_integrators::RungeKuttaCoefficients;

using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative;
using numerical_integrator_test_functions::computeFehlbergLogirithmicTestODEStateDerivative ;
using numerical_integrator_test_functions::computeAnalyticalStateFehlbergODE ;

//! Test Runge-Kutta-Fehlberg 78 integrator using benchmark ODE of Fehlberg (1968)
BOOST_AUTO_TEST_CASE( test_RungeKuttaFehlberg78_Integrator_Fehlberg_Benchmark )
{
    using namespace numerical_integrators;
    RungeKuttaCoefficients coeff78 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 );

    // Integrator settings
    double minimumStepSize   = std::numeric_limits<double>::epsilon( );
    double maximumStepSize   = std::numeric_limits<double>::infinity( );
    double initialStepSize   = 1E-6; // Error: 0.0521 for initialStepSize = 1 ?
    double relativeTolerance = 1E-16;
    double absoluteTolerance = 1E-16;

    // Initial conditions
    double initialTime = 0.0;
    double finalTime   = 5.0;
    Eigen::Vector2d initialState( exp( 1.0 ), 1.0);

    // Setup integrator
    numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd integrator78(
                coeff78, computeFehlbergLogirithmicTestODEStateDerivative,
                initialTime, initialState, minimumStepSize,
                maximumStepSize, relativeTolerance, absoluteTolerance );

    // Obtain numerical solution
    Eigen::Vector2d numericalSolution = integrator78.integrateTo( finalTime, initialStepSize );

    // Analytical solution
    Eigen::Vector2d analyticalSolution = computeAnalyticalStateFehlbergODE( finalTime, initialState );

    Eigen::Vector2d computedError = numericalSolution - analyticalSolution;
    BOOST_CHECK_SMALL( std::fabs(computedError( 0 )), 1E-13 );
    BOOST_CHECK_SMALL( std::fabs(computedError( 1 )), 1E-13 );
}

//! Test Runge-Kutta-Fehlberg 78 integrator using benchmark data from (The MathWorks, 2012).
BOOST_AUTO_TEST_CASE( testRungeKuttaFehlberg78IntegratorUsingMatlabData )
{
    using namespace numerical_integrator_tests;

    // Read in benchmark data (generated using Symbolic Math Toolbox in Matlab
    // (The MathWorks, 2012)). This data is generated using the RKF87 numerical integrator.
    const std::string pathToForwardIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKuttaFehlberg78Forward.txt";
    const std::string pathToDiscreteEventIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKuttaFehlberg78DiscreteEvent.txt";

    // Store benchmark data in matrix.
    const Eigen::MatrixXd matlabForwardIntegrationData =
            input_output::readMatrixFromFile( pathToForwardIntegrationOutputFile, "," );
    Eigen::MatrixXd matlabBackwardIntegrationData = matlabForwardIntegrationData;
    flipMatrixRows( matlabBackwardIntegrationData );
    const Eigen::MatrixXd matlabDiscreteEventIntegrationData =
            input_output::readMatrixFromFile( pathToDiscreteEventIntegrationOutputFile, "," );

    // Set integrator parameters.

    // All of the following parameters are set such that the input data is fully accepted by the
    // integrator, to determine the steps to be taken.
    const double zeroMinimumStepSize = std::numeric_limits< double >::epsilon( );
    const double infiniteMaximumStepSize = std::numeric_limits< double >::infinity( );
    const double infiniteRelativeErrorTolerance = std::numeric_limits< double >::infinity( );
    const double infiniteAbsoluteErrorTolerance = std::numeric_limits< double >::infinity( );

    // The following parameters set how the error control mechanism should work.
    const double relativeErrorTolerance = 1.0e-15;
    const double absoluteErrorTolerance = 1.0e-15;

    // Case 1: Execute integrateTo() to integrate one step forward in time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        executeOneIntegrateToStep( matlabForwardIntegrationData, 1.0e-15, integrator );
    }

    // Case 2: Execute performIntegrationStep() to perform multiple integration steps until final
    //         time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabForwardIntegrationData,
                                               1.0e-15, 1.0e-15, integrator );
    }

    // Case 3: Execute performIntegrationStep() to perform multiple integration steps until initial
    //         time (backwards).
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabBackwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabBackwardIntegrationData( FIRST_ROW,
                                                        STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabBackwardIntegrationData,
                                               1.0e-15, 1.0e-14, integrator );
    }

    // Case 4: Execute integrateTo() to integrate to specified time in one step.
    {
        // Note that this test has a strange issue that the if the absolute error tolerance is set
        // to 1.0e-15, the last step that the integrateTo() function takes does not result in the
        // expected final time of 1.0. As a temporary solution, the absolute error tolerance has
        // been multiplied by 10.0, which seems to solve the problem. This error indicated a
        // possible problem with the implementation of the integrateTo() function, which needs to
        // be investigated in future.

        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    relativeErrorTolerance,
                    absoluteErrorTolerance * 10.0 );

        executeIntegrateToToSpecifiedTime( matlabForwardIntegrationData, 1.0e-13, integrator,
                                           matlabForwardIntegrationData(
                                               matlabForwardIntegrationData.rows( ) - 1,
                                               TIME_COLUMN_INDEX ) );
    }

    // Case 5: Execute performIntegrationstep() to integrate to specified time in multiple steps,
    //         including discrete events.
    {
        // Declare integrator with all necessary settings.
        ReinitializableNumericalIntegratorXdPointer integrator
                = boost::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTimeWithEvents( matlabDiscreteEventIntegrationData,
                                                         1.0e-15, 1.0e-13, integrator );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


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

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

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

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

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_intersection_linear_linear_areal
#endif

#ifdef BOOST_GEOMETRY_TEST_DEBUG
#define BOOST_GEOMETRY_DEBUG_TURNS
#define BOOST_GEOMETRY_DEBUG_SEGMENT_IDENTIFIER
#endif

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

#include <boost/range.hpp>

#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include "test_intersection_linear_linear.hpp"

typedef bg::model::point<double,2,bg::cs::cartesian>  point_type;
typedef bg::model::multi_linestring
    <
        bg::model::linestring<point_type>
    >  multi_linestring_type;

typedef bg::model::ring<point_type, true, false> open_ring_type;
typedef bg::model::polygon<point_type, true, false> open_polygon_type;
typedef bg::model::multi_polygon<open_polygon_type> open_multipolygon_type;

typedef bg::model::ring<point_type> closed_ring_type;
typedef bg::model::polygon<point_type> closed_polygon_type;
typedef bg::model::multi_polygon<closed_polygon_type> closed_multipolygon_type;

template
<
    typename OpenAreal1,
    typename OpenAreal2,
    typename ClosedAreal1,
    typename ClosedAreal2,
    typename MultiLinestring
>
struct test_intersection_aal
{
    static inline void apply(std::string const& case_id,
                             OpenAreal1 const& open_areal1,
                             OpenAreal2 const& open_areal2,
                             MultiLinestring const& expected1,
                             MultiLinestring const& expected2)
    {
        typedef test_intersection_of_geometries
            <
                OpenAreal1, OpenAreal2, MultiLinestring
            > tester;

        tester::apply(open_areal1, open_areal2, expected1, expected2, case_id);

        ClosedAreal1 closed_areal1;
        ClosedAreal2 closed_areal2;
        bg::convert(open_areal1, closed_areal1);
        bg::convert(open_areal2, closed_areal2);

        typedef test_intersection_of_geometries
            <
                ClosedAreal1, ClosedAreal2, MultiLinestring
            > tester_of_closed;

        std::string case_id_closed = case_id + "-closed";

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "testing closed areal geometries..." << std::endl;
#endif
        tester_of_closed::apply(closed_areal1, closed_areal2,
                                expected1, expected2, case_id_closed);
    }

    static inline void apply(std::string const& case_id,
                             OpenAreal1 const& open_areal1,
                             OpenAreal2 const& open_areal2,
                             MultiLinestring const& expected)
    {
        apply(case_id, open_areal1, open_areal2, expected, expected);
    }
};

BOOST_AUTO_TEST_CASE( test_intersection_ring_ring_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** RING / RING / LINEAR INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif
    typedef open_ring_type OG;
    typedef closed_ring_type CG;
    typedef multi_linestring_type ML;

    typedef test_intersection_aal<OG, OG, CG, CG, ML> tester;

    tester::apply
        ("r-r-01",
         from_wkt<OG>("POLYGON((0 0,0 2,2 2,2 0))"),
         from_wkt<OG>("POLYGON((2 1,2 4,4 4,4 0,1 0))"),
         from_wkt<ML>("MULTILINESTRING((2 1,2 2),(2 0,1 0),(2 1,2 1))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 1),(2 0,1 0),(2 1,2 1))")
         );

    tester::apply
        ("r-r-02",
         from_wkt<OG>("POLYGON(())"),
         from_wkt<OG>("POLYGON((2 1,2 4,4 4,4 0,1 0))"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("r-r-03",
         from_wkt<OG>("POLYGON((2 1,2 4,4 4,4 0,1 0))"),
         from_wkt<OG>("POLYGON(())"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("r-r-04",
         from_wkt<OG>("POLYGON(())"),
         from_wkt<OG>("POLYGON(())"),
         from_wkt<ML>("MULTILINESTRING()")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_ring_polygon_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** RING / POLYGON / LINEAR INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif
    typedef open_ring_type OG1;
    typedef open_polygon_type OG2;
    typedef closed_ring_type CG1;
    typedef closed_polygon_type CG2;
    typedef multi_linestring_type ML;

    typedef test_intersection_aal<OG1, OG2, CG1, CG2, ML> tester;

    tester::apply
        ("r-pg-01",
         from_wkt<OG1>("POLYGON((0 0,0 2,2 2,2 0))"),
         from_wkt<OG2>("POLYGON((2 1,2 4,4 4,4 0,1 0))"),
         from_wkt<ML>("MULTILINESTRING((2 1,2 2),(2 0,1 0),(2 1,2 1))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 1),(2 0,1 0),(2 1,2 1))")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_ring_multipolygon_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** RING / MULTIPOLYGON / LINEAR INTERSECTION ***"
              << std::endl;
    std::cout << std::endl;
#endif
    typedef open_ring_type OG1;
    typedef open_multipolygon_type OG2;
    typedef closed_ring_type CG1;
    typedef closed_multipolygon_type CG2;
    typedef multi_linestring_type ML;

    typedef test_intersection_aal<OG1, OG2, CG1, CG2, ML> tester;

    tester::apply
        ("r-mpg-01",
         from_wkt<OG1>("POLYGON((0 0,0 2,2 2,2 0))"),
         from_wkt<OG2>("MULTIPOLYGON(((2 1,2 4,4 4,4 0,1 0)))"),
         from_wkt<ML>("MULTILINESTRING((2 1,2 2),(2 0,1 0),(2 1,2 1))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 1),(2 0,1 0),(2 1,2 1))")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_polygon_polygon_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** POLYGON / POLYGON / LINEAR INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif
    typedef open_polygon_type OG;
    typedef closed_polygon_type CG;
    typedef multi_linestring_type ML;

    typedef test_intersection_aal<OG, OG, CG, CG, ML> tester;

    tester::apply
        ("pg-pg-01",
         from_wkt<OG>("POLYGON((0 0,0 2,2 2,2 0))"),
         from_wkt<OG>("POLYGON((2 1,2 4,4 4,4 0,1 0))"),
         from_wkt<ML>("MULTILINESTRING((2 1,2 2),(2 0,1 0),(2 1,2 1))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 1),(2 0,1 0),(2 1,2 1))")
         );

    tester::apply
        ("pg-pg-02",
         from_wkt<OG>("POLYGON((0 0,0 10,10 10,10 0),(2 2,7 2,7 7,2 7))"),
         from_wkt<OG>("POLYGON((2 2,2 7,7 7,7 2))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 2),(2 2,2 7,7 7,7 2,2 2),(2 2,2 2))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 2),(2 2,7 2,7 7,2 7,2 2),(2 2,2 2))")
         );

    tester::apply
        ("pg-pg-03",
         from_wkt<OG>("POLYGON((0 0,0 10,10 10,10 0),(2 2,7 2,7 7,2 7))"),
         from_wkt<OG>("POLYGON((2 3,2 6,6 6,6 3))"),
         from_wkt<ML>("MULTILINESTRING((2 3,2 6),(2 3,2 3))")
         );

    tester::apply
        ("pg-pg-04",
         from_wkt<OG>("POLYGON((0 0,0 10,10 10,10 0),(2 2,7 2,7 7,2 7))"),
         from_wkt<OG>("POLYGON((2 3,2 7,6 7,6 3))"),
         from_wkt<ML>("MULTILINESTRING((2 3,2 7,6 7),(2 3,2 3))")
         );

    tester::apply
        ("pg-pg-05",
         from_wkt<OG>("POLYGON((0 0,0 10,10 10,10 0),(2 2,7 2,7 7,2 7))"),
         from_wkt<OG>("POLYGON((2 3,2 7,7 7,7 3))"),
         from_wkt<ML>("MULTILINESTRING((2 3,2 7,7 7,7 3),(2 3,2 3))")
         );

    tester::apply
        ("pg-pg-06",
         from_wkt<OG>("POLYGON((0 0,0 10,10 10,10 0),(2 2,7 2,7 7,2 7))"),
         from_wkt<OG>("POLYGON((2 3,2 7,7 7,7 3))"),
         from_wkt<ML>("MULTILINESTRING((2 3,2 7,7 7,7 3),(2 3,2 3))")
         );

    tester::apply
        ("pg-pg-07",
         from_wkt<OG>("POLYGON((0 0,0 10,10 10,10 0),(2 2,7 2,7 7,2 7))"),
         from_wkt<OG>("POLYGON((2 5,5 7,7 5,5 2))"),
         from_wkt<ML>("MULTILINESTRING((2 5,2 5),(5 7,5 7),(7 5,7 5),(5 2,5 2))")
         );

    tester::apply
        ("pg-pg-08",
         from_wkt<OG>("POLYGON((0 0,0 10,10 10,10 0),(2 2,7 2,7 7,2 7))"),
         from_wkt<OG>("POLYGON((2 5,4 7,6 7,7 5,5 2))"),
         from_wkt<ML>("MULTILINESTRING((2 5,2 5),(4 7,6 7),(7 5,7 5),(5 2,5 2))")
         );

    tester::apply
        ("pg-pg-09",
         from_wkt<OG>("POLYGON(())"),
         from_wkt<OG>("POLYGON((2 1,2 4,4 4,4 0,1 0))"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("pg-pg-10",
         from_wkt<OG>("POLYGON((2 1,2 4,4 4,4 0,1 0))"),
         from_wkt<OG>("POLYGON(())"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("pg-pg-11",
         from_wkt<OG>("POLYGON(())"),
         from_wkt<OG>("POLYGON(())"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("pg-pg-12",
         from_wkt<OG>("POLYGON((),())"),
         from_wkt<OG>("POLYGON((),(),())"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("pg-pg-13",
         from_wkt<OG>("POLYGON((2 1,2 4,4 4,4 0,1 0),())"),
         from_wkt<OG>("POLYGON(())"),
         from_wkt<ML>("MULTILINESTRING()")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_polygon_multipolygon_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** POLYGON / MULTIPOLYGON / LINEAR INTERSECTION ***"
              << std::endl;
    std::cout << std::endl;
#endif
    typedef open_polygon_type OG1;
    typedef open_multipolygon_type OG2;
    typedef closed_polygon_type CG1;
    typedef closed_multipolygon_type CG2;
    typedef multi_linestring_type ML;

    typedef test_intersection_aal<OG1, OG2, CG1, CG2, ML> tester;

    tester::apply
        ("pg-mpg-01",
         from_wkt<OG1>("POLYGON((0 0,0 2,2 2,2 0))"),
         from_wkt<OG2>("MULTIPOLYGON(((2 1,2 4,4 4,4 0,1 0)))"),
         from_wkt<ML>("MULTILINESTRING((2 1,2 2),(2 0,1 0),(2 1,2 1))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 1),(2 0,1 0),(2 1,2 1))")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_multipolygon_multipolygon_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** MULTIPOLYGON / MULTIPOLYGON / LINEAR INTERSECTION ***"
              << std::endl;
    std::cout << std::endl;
#endif
    typedef open_multipolygon_type OG;
    typedef closed_multipolygon_type CG;
    typedef multi_linestring_type ML;

    typedef test_intersection_aal<OG, OG, CG, CG, ML> tester;

    tester::apply
        ("mpg-mpg-01",
         from_wkt<OG>("MULTIPOLYGON(((0 0,0 2,2 2,2 0)))"),
         from_wkt<OG>("MULTIPOLYGON(((2 1,2 4,4 4,4 0,1 0)))"),
         from_wkt<ML>("MULTILINESTRING((2 1,2 2),(2 0,1 0),(2 1,2 1))"),
         from_wkt<ML>("MULTILINESTRING((2 2,2 1),(2 0,1 0),(2 1,2 1))")
         );

    tester::apply
        ("mpg-mpg-02",
         from_wkt<OG>("MULTIPOLYGON(((0 0,0 10,10 10,10 0),(2 2,8 2,8 8,2 8)))"),
         from_wkt<OG>("MULTIPOLYGON(((2 4,2 6,8 6,8 4)))"),
         from_wkt<ML>("MULTILINESTRING((2 4,2 4),(2 4,2 6),(8 6,8 4))")
         );

    tester::apply
        ("mpg-mpg-03",
         from_wkt<OG>("MULTIPOLYGON()"),
         from_wkt<OG>("MULTIPOLYGON(((2 1,2 4,4 4,4 0,1 0)))"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("mpg-mpg-04",
         from_wkt<OG>("MULTIPOLYGON(((2 1,2 4,4 4,4 0,1 0)))"),
         from_wkt<OG>("MULTIPOLYGON()"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("mpg-mpg-05",
         from_wkt<OG>("MULTIPOLYGON()"),
         from_wkt<OG>("MULTIPOLYGON()"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("mpg-mpg-06",
         from_wkt<OG>("MULTIPOLYGON((()),((),()))"),
         from_wkt<OG>("MULTIPOLYGON()"),
         from_wkt<ML>("MULTILINESTRING()")
         );

    tester::apply
        ("mpg-mpg-07",
         from_wkt<OG>("MULTIPOLYGON(((2 1,2 4,4 4,4 0,1 0),(),()))"),
         from_wkt<OG>("MULTIPOLYGON()"),
         from_wkt<ML>("MULTILINESTRING()")
         );
}


#include <boost/test/unit_test.hpp>

#include "../math/math.h"

USING_NAMESPACE(sway)

BOOST_AUTO_TEST_SUITE(TColorTestSuite)

/*!
 * Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TColorTestCase_DefaultConstructor) {
	math::TColor<f32> color;

	BOOST_CHECK_EQUAL(color.getR(), 0.0f);
	BOOST_CHECK_EQUAL(color.getG(), 0.0f);
	BOOST_CHECK_EQUAL(color.getB(), 0.0f);
	BOOST_CHECK_EQUAL(color.getA(), 1.0f);
}

/*!
 * Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 * которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TColorTestCase_ComponentConstructor) {
	const f32 r = 0.1f, g = 0.2f, b = 0.3f, a = 1.0f;

	math::TColor<f32> color = math::TColor<f32>(r, g, b, a);

	BOOST_CHECK_EQUAL(color.getR(), r);
	BOOST_CHECK_EQUAL(color.getG(), g);
	BOOST_CHECK_EQUAL(color.getB(), b);
	BOOST_CHECK_EQUAL(color.getA(), a);
}

/*!
 * Убеждаемся, что преобразование в TVector4<type> проходит правильно.
 */
BOOST_AUTO_TEST_CASE(TColorTestCase_ConvertToVector4) {
	const f32 r = 0.1f, g = 0.2f, b = 0.3f, a = 1.0f;

	math::TColor<f32> color = math::TColor<f32>(r, g, b, a);
	math::TVector4<f32> vec4 = color.toVec4();

	BOOST_CHECK_EQUAL(vec4.getX(), r);
	BOOST_CHECK_EQUAL(vec4.getY(), g);
	BOOST_CHECK_EQUAL(vec4.getZ(), b);
	BOOST_CHECK_EQUAL(vec4.getW(), a);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>

namespace EigenHelpers{
    template <typename Derived>
        void dumpt(const Eigen::EigenBase<Derived>& mat) { std::cout<<mat<<std::endl; }

    inline void dump(const Eigen::MatrixXd& mat) { std::cout<<mat<<std::endl; }
    inline void dump(const Eigen::VectorXd& mat) { std::cout<<mat<<std::endl; }
    inline void dump(const Eigen::ArrayXd& mat)  { std::cout<<mat<<std::endl; }
    inline void dump(const Eigen::ArrayXXd& mat) { std::cout<<mat<<std::endl; }

    /* Won't work till C++17*/
    /* inline Eigen::IOFormat numpy_format(Eigen::StreamPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); */

    inline Eigen::IOFormat setNumpyFormat() {
        return Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
    }
}


/**
 * @file
 * @author lkoppel
 */

#ifndef WAVE_GEOMETRY_MATRIXMAP_HPP
#define WAVE_GEOMETRY_MATRIXMAP_HPP

#include <Eigen/Core>
#include <boost/container/flat_map.hpp>

namespace wave {

/**
 * Stores a collection of equal-height matrices in one contiguous matrix.
 *
 * Stores matrices of size n*m_1, n*m_2, ... n*m_p,  where n and
 * all m_i are known at construct time.
 *
 * Indexing is done through a corresponding set of keys k_1, k_2, ... k_p.
 *
 * MatrixMap is constructed with a map of keys k_i to widths m_i, and its size cannot not
 * change later. The matrix is initially resized but not initialized (holds garbage).
 *
 * @tparam Key the key type used for indexing
 * @tparam Scalar the scalar type of the matrix (e.g. double)
 */
template <typename Key, typename Scalar>
class MatrixMap {
    struct IndexPair {
        int col_index;
        int col_width;
    };

    using IndexMap = boost::container::flat_map<Key, IndexPair>;

 public:
    /** Constructs and resizes the matrix. (Doesn't initialize. It holds garbage).
     *
     * Variant for dynamic-height MatrixMap.
     *
     * @param begin, end a pair of iterators to a sequence of type <Key, int> holding
     * the {key, column width} for each matrix. For best performance it should be sorted;
     * duplicates allowed.
     * @param number of rows
     */
    template <typename MapIt>
    MatrixMap(const MapIt &begin, const MapIt &end, Eigen::Index rows) {
        int col = 0;
        index_map.reserve(std::distance(begin, end));
        for (auto it = begin; it != end; ++it) {
            const auto &key = it->first;
            const auto &width = it->second;
            index_map.emplace_hint(index_map.end(), key, IndexPair{col, width});
            col += width;
        }
        storage.resize(rows, col);
    }

    /** Returns a block representing the matrix for the given key */
    auto operator[](const Key &key) {
        const auto &v = this->index_map[key];
        return this->storage.block(0, v.col_index, storage.rows(), v.col_width);
    }

    /** Returns a const block representing the matrix for the given key */
    auto operator[](const Key &key) const {
        const auto &v = this->index_map[key];
        return this->storage.block(0, v.col_index, storage.rows(), v.col_width);
    }

    /** Returns a block representing the matrix for the given key
     * @throws out_of_range if key is not present
     */
    auto at(const Key &key) {
        const auto &v = this->index_map.at(key);
        return this->storage.block(0, v.col_index, storage.rows(), v.col_width);
    }

    /** Returns a const block representing the matrix for the given key
     * @throws out_of_range if key is not present
     */
    auto at(const Key &key) const {
        const auto &v = this->index_map.at(key);
        return this->storage.block(0, v.col_index, storage.rows(), v.col_width);
    }

    /** Returns 1 if the key is present, 0 otherwise */
    auto count(const Key &key) const {
        return this->index_map.count(key);
    }

    /** Set all blocks to zero */
    void setZero() {
        this->storage.setZero();
    }

 private:
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> storage;
    // Map of key -> (column index, column width) index
    IndexMap index_map;
};

}  // namespace wave

#endif  // WAVE_GEOMETRY_MATRIXMAP_HPP


////////////////////////////////////////////////////////////////
// Orkid Media Engine
// Copyright 1996-2020, Michael T. Mayers.
// Distributed under the Boost Software License - Version 1.0 - August 17, 2003
// see http://www.boost.org/LICENSE_1_0.txt
////////////////////////////////////////////////////////////////

#include <ork/lev2/config.h>
#if defined(ENABLE_IGL)

#include <ork/kernel/orklut.hpp>
#include <ork/math/plane.h>
#include <ork/lev2/gfx/meshutil/submesh.h>
#include <ork/lev2/gfx/meshutil/igl.h>
#include <iostream>

#include <Eigen/Core>

#include <igl/embree/ambient_occlusion.h>

namespace ork::meshutil {

//////////////////////////////////////////////////////////////////////////////
Eigen::VectorXd IglMesh::ambientOcclusion(int numsamples) const {
  Eigen::VectorXd AO;
  Eigen::MatrixXd N = computeVertexNormals();
  igl::embree::ambient_occlusion(_verts, _faces, _verts, N, numsamples, AO);
  AO = 1.0 - AO.array();
  return AO;
}

} // namespace ork::meshutil

#endif


// Std includes
#include <iostream> // cout, endl
#include <vector>
#include <memory> // shared_ptr
#include <map>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "fl0p/stationary.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;

template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMesh = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
using TypeMeshSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeFlow = fl0w::fl0p::Stationary<TypeVector, TypeMatrix, TypeRef, TypeMesh, TypeContainer, TypeMeshSub, TypeContainer>;

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") = \n" << flow.getVelocity(x, t).transpose() << std::endl;
    std::cout << "flow.getJacobian(" << x.transpose() << ", " << t << ") = \n" << flow.getJacobian(x, t) << std::endl;
    std::cout << "flow.getVorticity(" << x.transpose() << ", " << t << ") = \n" << flow.getVorticity(x, t).transpose() << std::endl;
    std::cout << "flow.getAcceleration(" << x.transpose() << ", " << t << ") = \n" << flow.getAcceleration(x, t).transpose() << std::endl;
    std::cout << std::endl;
}

double f(const double x, const double y) {
    return x;
}

int main () {
    // parameters
    std::size_t n = 8;
    // mesh
    std::vector<std::size_t> dimensions({n, n, n});
    std::vector<double> lengths({1.0, 1.0, 1.0});
    TypeVector origin({-0.5, -0.5, -0.5});
    // data
    std::vector<std::vector<float>> velocity(3, std::vector<float>(std::pow(n, DIM)));
    for(unsigned int i = 0; i < n; i++) {
        for(unsigned int j = 0; j < n; j++) {
            for(unsigned int k = 0; k < n; k++) {
                velocity[0][i + n*j + n*n*k] = (i + j + k)/3.0f;
                velocity[1][i + n*j + n*n*k] = (i + j + k)/3.0f;
                velocity[2][i + n*j + n*n*k] = (i + j + k)/3.0f;
            }
        }
    }
    // flow
    TypeFlow flow(std::make_shared<TypeMesh>(dimensions, lengths, origin, TypeContainer<bool>(DIM, true)), velocity, 4);
    // print
    print(flow, TypeVector({0.0, 0.0, 0.0}), 0.0);
}


#include "vr/util/ops_int.h"

#include "vr/data/NA.h"
#include "vr/util/logging.h"

#include "vr/test/utility.h"

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

//----------------------------------------------------------------------------
namespace vr
{
namespace util
{
//............................................................................
/*
 * note: this nlz() implementation has a defined behavior for zero input
 */
TEST (ops_int_test, vr_nlz)
{
#define vr_nlz_TEST(r, type, test) \
    { \
        const type x_input = BOOST_PP_TUPLE_ELEM (2, 0, test); \
        const int32_t n_expected = BOOST_PP_TUPLE_ELEM (2, 1, test); \
        auto x = x_input; \
        const int32_t n = vr_nlz_##type (x); \
        EXPECT_EQ (n_expected, n) << "x: " << x; \
        ASSERT_EQ (x_input, x); /* catch ABI bugs */ \
    } \
    /* */

    // 32 bits:

#define vr_nlz_TESTCASES (0, 32)(1, 31)(3, 30)(4, 29)(-1, 0)

    BOOST_PP_SEQ_FOR_EACH (vr_nlz_TEST, int32_t, VR_DOUBLE_PARENTHESIZE_2 (vr_nlz_TESTCASES))

#undef vr_nlz_TESTCASES
#define vr_nlz_TESTCASES (0, 64)(1, 63)(3, 62)(4, 61)(-1, 0)

    // 64 bits:

    BOOST_PP_SEQ_FOR_EACH (vr_nlz_TEST, int64_t, VR_DOUBLE_PARENTHESIZE_2 (vr_nlz_TESTCASES))

#undef vr_nlz_TEST
}
//............................................................................

using supported_int_types   = gt::Types<int32_t, int64_t>;

template<typename T> struct ops_int_test: public gt::Test { };
TYPED_TEST_CASE (ops_int_test, supported_int_types);

//............................................................................
//............................................................................
namespace
{

template<typename T, int32_t ZERO_RETURN>
void ilog10_floor_test ()
{
    using value_type    = T;

    using zero_policy   = arg_policy<zero_arg_policy::return_special, ZERO_RETURN>;

    using ops_fast      = ops_int<zero_policy, false>;
    using ops_checked   = ops_int<zero_policy, true>;

    // input validation:

    EXPECT_NO_THROW (ops_fast::ilog10_floor (-2));
    EXPECT_THROW (ops_checked::ilog10_floor (-2), invalid_input);

    // test zero input:
    {
        value_type x = 0; // note: special value (return controlled by policy)
        const int32_t l = ops_checked::ilog10_floor (x);
        const int32_t l_expected = zero_policy::zero_return;
        ASSERT_EQ (l, l_expected) << "failed for zero input ";
    }

    {
        value_type x = 9;
        const int32_t l = ops_checked::ilog10_floor (x);
        EXPECT_EQ (l, 0) << "x: " << x;
    }
    {
        value_type x = 10;
        const int32_t l = ops_checked::ilog10_floor (x);
        EXPECT_EQ (l, 1) << "x: " << x;
    }
    {
        value_type x = 11;
        const int32_t l = ops_checked::ilog10_floor (x);
        EXPECT_EQ (l, 1) << "x: " << x;
    }

    // test a large input:

    constexpr int32_t max_pow10     = std::numeric_limits<value_type>::digits10;
    const value_type x_big = boost::math::pow<max_pow10> (static_cast<value_type> (10));

    LOG_trace1 << "max pow10: " << max_pow10 << ", x_big: " << x_big;

    {
        value_type x = x_big - 1;
        const int32_t l = ops_checked::ilog10_floor (x);
        EXPECT_EQ (l, max_pow10 - 1) << "x: " << x;
    }
    {
        value_type x = x_big;
        const int32_t l = ops_checked::ilog10_floor (x);
        EXPECT_EQ (l, max_pow10) << "x: " << x;
    }
    {
        value_type x = x_big + 1;
        const int32_t l = ops_checked::ilog10_floor (x);
        EXPECT_EQ (l, max_pow10) << "x: " << x;
    }

    // test max input:

    const value_type x_max = std::numeric_limits<value_type>::max ();

    LOG_trace1 << "x_max: " << x_max;

    {
        value_type x = x_max;
        const int32_t l = ops_checked::ilog10_floor (x);
        EXPECT_EQ (l, max_pow10) << "x: " << x;
    }
}

} // end of anonymous
//............................................................................
//............................................................................
// TODO
// - log2 tests

TYPED_TEST (ops_int_test, ilog10_floor_zero_0)
{
    using value_type    = TypeParam; // test parameter

    ilog10_floor_test<value_type, 0> ();
}

TYPED_TEST (ops_int_test, ilog10_floor_zero_m1)
{
    using value_type    = TypeParam; // test parameter

    ilog10_floor_test<value_type, -1> ();
}
//............................................................................

TYPED_TEST (ops_int_test, popcount)
{
    using value_type    = TypeParam; // test parameter

    value_type const zero       = 0;
    EXPECT_EQ (popcount (zero), 0);

    value_type const all_ones   = -1;
    EXPECT_EQ (popcount (all_ones), signed_cast (8 * sizeof (value_type)));
}
//............................................................................

TYPED_TEST (ops_int_test, ls_zero_index)
{
    using value_type    = TypeParam; // test parameter

    for (int32_t i = 0; i < signed_cast (sizeof (value_type)); ++ i)
    {
        value_type const x      = -1 & ~(static_cast<value_type> (0xFF) << (i * 8));
        int32_t const lszix = ls_zero_index (x);

        EXPECT_EQ (lszix, i);
    }

    // some edge cases:
    {
        // no zeros:
        {
            value_type const x  = -1;
            int32_t const lszix = ls_zero_index (x);

            EXPECT_EQ (lszix, signed_cast (sizeof (value_type)));
        }

        // several zeros:
        {
            value_type x        = 0; // all zeros
            int32_t lszix = ls_zero_index (x);

            EXPECT_EQ (lszix, 0);

            x                   = 0xFF00FF; // all but 2 zeros
            lszix = ls_zero_index (x);
            EXPECT_EQ (lszix, 1);
        }
    }
}
//............................................................................

TYPED_TEST (ops_int_test, net_int)
{
    using value_type    = TypeParam; // test parameter

    const value_type inputs [] { std::numeric_limits<value_type>::min (), -1000, -1, 0, 1, 1000, std::numeric_limits<value_type>::max () };

    {
        using nint_type     = net_int<value_type, false>;

        nint_type ni;

        for (value_type i : inputs)
        {
            ni = i;
            value_type const ii = ni;

            ASSERT_EQ (ii, i) << "failed for input " << i;

            // copy construction:
            {
                nint_type const ni_cc { ni };
                value_type const ii_cc = ni_cc;

                ASSERT_EQ (ii_cc, ii);
            }
            // copy assignment:
            {
                nint_type ni_ca; ni_ca = ni;
                value_type const ii_ca = ni_ca;

                ASSERT_EQ (ii_ca, ii);
            }
        }
    }
    {
        using nint_type     = net_int<value_type, true>;

        nint_type ni;

        for (value_type i : inputs)
        {
            ni = i;
            value_type const ii = ni;

            ASSERT_EQ (ii, i) << "failed for input " << i;

            // copy construction:
            {
                nint_type const ni_cc { ni };
                value_type const ii_cc = ni_cc;

                ASSERT_EQ (ii_cc, ii);
            }
            // copy assignment:
            {
                nint_type ni_ca; ni_ca = ni;
                value_type const ii_ca = ni_ca;

                ASSERT_EQ (ii_ca, ii);
            }
        }
    }
}
//............................................................................

TEST (ops_int_test, mux)
{
    EXPECT_TRUE (0          == mux (0));
    EXPECT_TRUE (  0x0102FF == mux (0, 1, 2, 255));
    EXPECT_TRUE (0x01020304 == mux (1, 2, 3, 4));
}
//............................................................................

TEST (bit_cast, fp)
{
    {
        auto const na = data::NA<float> ();
        LOG_trace1 << "bit image of float NA: " << hex_string_cast (bit_cast (na));

        auto const na_image = bit_cast (na);
        EXPECT_EQ (signed_cast (na_image & 0xFFFF), VR_R_NA_MARKER);
    }
    {
        auto const na = data::NA<double> ();
        LOG_trace1 << "bit image of double NA: " << hex_string_cast (bit_cast (na));

        auto const na_image = bit_cast (na);
        EXPECT_EQ (signed_cast (na_image & 0xFFFF), VR_R_NA_MARKER);
    }
}

} // end of 'util'
} // end of namespace
//----------------------------------------------------------------------------


/// \file   test_parameter.cpp
/// \brief
/// \authors    maarten
/// \date       2021-02-08
/// \copyright  Copyright 2017-2021 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///             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.
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE model_parallel

#include <set>

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

#include <esl/simulation/parameter/sampler.hpp>
using namespace esl::simulation::parameter;
using namespace esl::simulation::parameter::sampler;

BOOST_AUTO_TEST_SUITE(ESL)

    ///
    /// \brief  This tests whether the grid sampler can generate points with
    ///         different densities in each dimension, whether it generates the
    ///         expected number of points and whether they are all unique.
    ///
    BOOST_AUTO_TEST_CASE(parameter_test_sampler_grid)
    {
        auto param0 = std::make_shared<interval<double>>(1.0, 5.0);
        auto param1 = std::make_shared<interval<double>>(-3, 3.0);
        auto param2= std::make_shared<interval<double>>(100, 200);

        std::map<std::shared_ptr<interval<double>>, unsigned int> p;
        p.emplace(param0, 2);
        p.emplace(param1, 5);
        p.emplace(param2, 3);
        auto samples_ = grid(p);

        BOOST_CHECK_EQUAL(samples_.size(), 2*5*3);

        std::set<std::tuple<double, double, double>> unique_;
        for(const auto &e: samples_){

            unique_.insert( std::make_tuple( e.find(param0)->second
                , e.find(param1)->second
                , e.find(param2)->second
            ) );
        }
        BOOST_CHECK_EQUAL(unique_.size(), 2*5*3);
    }

    BOOST_AUTO_TEST_CASE(parameter_test_sampler_hypercube)
    {
        auto param0 = std::make_shared<interval<double>>(1.0, 5.0);
        auto param1 = std::make_shared<interval<double>>(-3, 3.0);
        auto param2= std::make_shared<interval<double>>(100, 200);

        std::vector<std::shared_ptr<interval<double>>> p;
        p.emplace_back(param0);
        p.emplace_back(param1);
        p.emplace_back(param2);
        auto samples_ = latin_hypercube(p, 10);

        BOOST_CHECK_EQUAL(samples_.size(), 10 );

        std::set<std::tuple<double, double, double>> unique_;
        for(const auto &e: samples_){

            unique_.insert(std::make_tuple
                               ( e.find(param0)->second
                                   , e.find(param1)->second
                                   , e.find(param2)->second
                               ));
        }
        BOOST_CHECK_EQUAL(unique_.size(), 10);
    }

    BOOST_AUTO_TEST_CASE(parameter_test_sampler_orthogonal)
    {
        int dimensions = 8;
        int samples = 64;
        std::seed_seq seed = {1,2,3,4,5};
        int duplication  = 5;
        auto res = improved_hypercube_sampling (dimensions, samples, seed, duplication);
        for(int d = 0; d < dimensions; ++d) {
            std::set<unsigned int> ps;
            //std::cout << res[d][0];
            ps.insert(res[d][0]);
            for(int s = 1; s < samples; ++s) {
            //    std::cout << "," << res[d][s];
                ps.insert(res[d][s]);
            }
            BOOST_CHECK_EQUAL(ps.size(), samples);
            //std::cout << std::endl;
        }
    }

BOOST_AUTO_TEST_SUITE_END()  // ESL


#include <k4a/k4a.hpp>

#include <iostream>
#include <fstream>
#include <vector>
#include <array>

#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <aruco/aruco.h>

#include "Pixel.h"
#include "DepthPixelColorizer.h"
#include "StaticImageProperties.h"

using namespace std;
using namespace cv;
using namespace sen;
using namespace Eigen;

k4a::calibration calibration;
Mat camera_matrix_color;
Mat dist_coeffs_color;

aruco::Dictionary dic;
aruco::CameraParameters CamParam;
aruco::MarkerDetector MDetector;
std::map<int, aruco::MarkerPoseTracker> MTracker_master;
std::map<int, aruco::MarkerPoseTracker> MTracker_sub;
float MarkerSize = 0.1f;

const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n");

template <typename DataType>
void writeToCSVfile(std::string name, Eigen::Array<DataType, -1, -1> matrix)
{
	std::ofstream file(name.c_str());
	file << matrix.format(CSVFormat);
}

void eigenTransform2cvRvecTvec(const Transform<double, 3, Affine> frame, cv::Vec3d &rvec, cv::Vec3d &tvec)
{
	Translation<double, 3> t(frame.translation());
	Quaternion<double> q(frame.linear());
	tvec = cv::Vec3d(t.x(), t.y(), t.z());
	cv::Mat rotM;
	eigen2cv(q.toRotationMatrix(), rotM);
	cv::Rodrigues(rotM, rvec);
}

void drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
	InputArray _rvec, InputArray _tvec, float length)
{
	CV_Assert(_image.getMat().total() != 0 &&
		(_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
	CV_Assert(length > 0);

	// project axis points
	vector< Point3f > axisPoints;
	axisPoints.push_back(Point3f(0, 0, 0));
	axisPoints.push_back(Point3f(length, 0, 0));
	axisPoints.push_back(Point3f(0, length, 0));
	axisPoints.push_back(Point3f(0, 0, length));
	vector< Point2f > imagePoints;
	projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePoints);

	// draw axis lines
	line(_image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), 3);
	line(_image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), 3);
	line(_image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), 3);
}

int main(int argc, char **argv)
{
	const uint32_t deviceCount = k4a::device::get_installed_count();
	if (deviceCount == 0)
	{
		cout << "no azure kinect devices detected!" << endl;
	}

	k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
	config.camera_fps = K4A_FRAMES_PER_SECOND_30;
	config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
	config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
	config.color_resolution = K4A_COLOR_RESOLUTION_720P;
	config.synchronized_images_only = true;

	cout << "Started opening K4A device..." << endl;
	k4a::device dev_sub = k4a::device::open(1);
	dev_sub.start_cameras(&config);
	k4a::device dev_master = k4a::device::open(K4A_DEVICE_DEFAULT);
	dev_master.start_cameras(&config);
	cout << "Finished opening K4A device." << endl;

	std::pair<int, int> color_dimensions = GetColorDimensions(config.color_resolution);
	int texture_width = color_dimensions.first;
	int texture_height = color_dimensions.second;

	calibration = dev_master.get_calibration(config.depth_mode, config.color_resolution);
	k4a_calibration_intrinsic_parameters_t *intrinsics_color = &calibration.color_camera_calibration.intrinsics.parameters;
	vector<float> _camera_matrix = {
	   intrinsics_color->param.fx, 0.f, intrinsics_color->param.cx,
	   0.f, intrinsics_color->param.fy, intrinsics_color->param.cy,
	   0.f, 0.f, 1.f };
	camera_matrix_color = Mat(3, 3, CV_32F, &_camera_matrix[0]);
	vector<float> _dist_coeffs = { intrinsics_color->param.k1, intrinsics_color->param.k2, intrinsics_color->param.p1,
								   intrinsics_color->param.p2, intrinsics_color->param.k3, intrinsics_color->param.k4,
								   intrinsics_color->param.k5, intrinsics_color->param.k6 };
	dist_coeffs_color = Mat::zeros(5, 1, CV_32F);
	dic = aruco::Dictionary::load("ARUCO_MIP_36h12");
	CamParam.setParams(camera_matrix_color, dist_coeffs_color, cv::Size(texture_width, texture_height));
	MDetector.setDictionary("ARUCO_MIP_36h12", 0.05f);
	MDetector.setDetectionMode(aruco::DM_NORMAL);

	k4a::capture capture_master;
	k4a::capture capture_sub;

	k4a::image colorImage_master;
	uint8_t *colorTextureBuffer_master;
	cv::Mat colorFrame_master;

	k4a::image colorImage_sub;
	uint8_t *colorTextureBuffer_sub;
	cv::Mat colorFrame_sub;

	Eigen::Affine3d frame_master_marker;
	Eigen::Affine3d frame_sub_marker;
	Eigen::Affine3d frame_master_sub;

	bool poseEstimationOK_master = false;
	bool poseEstimationOK_sub = false;

	while (1)
	{
		if (dev_master.get_capture(&capture_master, std::chrono::milliseconds(0)) && dev_sub.get_capture(&capture_sub, std::chrono::milliseconds(0)))
		{
			//master
			{
				colorImage_master = capture_master.get_color_image();
				colorTextureBuffer_master = colorImage_master.get_buffer();
				colorFrame_master = cv::Mat(colorImage_master.get_height_pixels(), colorImage_master.get_width_pixels(), CV_8UC4, colorTextureBuffer_master);
				cvtColor(colorFrame_master, colorFrame_master, COLOR_BGRA2BGR);

				{
					vector<aruco::Marker> Markers = MDetector.detect(colorFrame_master);
					for (auto &marker : Markers)
					{
						MTracker_master[marker.id].estimatePose(marker, CamParam, MarkerSize);
					}
					if (CamParam.isValid() && MarkerSize != -1)
					{
						for (unsigned int i = 0; i < Markers.size(); ++i)
						{
							if (Markers[i].isPoseValid())
							{
								cv::Mat transformationMatrix_master = Markers[i].getTransformMatrix();
								cv2eigen(transformationMatrix_master, frame_master_marker.matrix());
								poseEstimationOK_master = true;
							}
							else
							{
								poseEstimationOK_master = false;
							}
						}
					}
				}
			}

			//sub
			{

				colorImage_sub = capture_sub.get_color_image();
				colorTextureBuffer_sub = colorImage_sub.get_buffer();
				colorFrame_sub = cv::Mat(colorImage_sub.get_height_pixels(), colorImage_sub.get_width_pixels(), CV_8UC4, colorTextureBuffer_sub);
				cvtColor(colorFrame_sub, colorFrame_sub, COLOR_BGRA2BGR);

				{
					vector<aruco::Marker> Markers = MDetector.detect(colorFrame_sub);
					for (auto &marker : Markers)
					{
						MTracker_sub[marker.id].estimatePose(marker, CamParam, MarkerSize);
					}
					if (CamParam.isValid() && MarkerSize != -1)
					{
						for (unsigned int i = 0; i < Markers.size(); ++i)
						{
							if (Markers[i].isPoseValid())
							{
								cv::Mat transformationMatrix_sub = Markers[i].getTransformMatrix();
								cv2eigen(transformationMatrix_sub, frame_sub_marker.matrix());
								poseEstimationOK_sub = true;
							}
							else
							{
								poseEstimationOK_sub = false;
							}
						}
					}
				}
			}

			if (poseEstimationOK_master && poseEstimationOK_sub)
			{
				Eigen::Affine3d frame_master_sub = frame_master_marker * frame_sub_marker.inverse();

				cv::Vec3d rvec, tvec;
				eigenTransform2cvRvecTvec(frame_sub_marker, rvec, tvec);
				drawAxis(colorFrame_sub, camera_matrix_color, dist_coeffs_color, rvec, tvec, 0.5f);

				Eigen::Affine3d frame_sub_master = frame_sub_marker * frame_master_marker.inverse();
				eigenTransform2cvRvecTvec(frame_sub_master, rvec, tvec);
				Eigen::Matrix4d frame_matrix = frame_sub_master.matrix();
				std::cout << "frame sub master" << std::endl;
				writeToCSVfile<double>("frame_sub_master.csv", frame_matrix);
				std::cout << "save matrix into csv file OK.\n";

				std::cout << "frame sub marker" << std::endl;
				frame_matrix = frame_sub_marker.matrix();
				writeToCSVfile<double>("frame_sub_marker.csv", frame_matrix);
				std::cout << "save matrix into csv file OK.\n";
				drawAxis(colorFrame_sub, camera_matrix_color, dist_coeffs_color, rvec, tvec, 0.5f);
			}
			else
			{
				std::cout << endl;
			}

			imshow("color sub", colorFrame_sub);
		}
		if (waitKey(30) == 27 || waitKey(30) == 'q')
		{
			dev_master.close();
			dev_sub.close();
			break;
		}
	}
	return 0;
}


/**
 * @copyright   Copyright (c) 2021, Swift Engineering Inc.
 * @license     Licensed under the MIT license. See LICENSE for details.
 */
#include "Motor_current_consumption_model.hpp"
#include <cmath>
#include <cstdio>
#include <limits>
#include <cstdint>
#include <boost/math/tools/rational.hpp>

Motor_current_consumption_model::Motor_current_consumption_model(const std::vector<double>
        &current_consumption_model_coeff) {
    m_current_consumption_model_coeff = current_consumption_model_coeff;
    m_model_type = RPM;
    m_c = 0;
    m_r = 1;
    m_last_current = 0;
}

Motor_current_consumption_model::Motor_current_consumption_model(const std::vector<double>
        &current_consumption_model_coeff, const double C, const double R) {
    if (R <= 0) {
        throw ("R must be greater than 0"); // NOLINT
    }

    if (C < 0) {
        throw ("C must be greater than or equal to 0");  // NOLINT
    }

    m_current_consumption_model_coeff = current_consumption_model_coeff;
    m_model_type = TORQUE;
    m_c = C;
    m_r = R;
    m_last_current = 0;
    // TODO(Mike Lyons) throw and exception if ?? is zero
}

double  Motor_current_consumption_model::get_current(const double value) {
    return boost::math::tools::evaluate_polynomial(m_current_consumption_model_coeff.data(), value,
            m_current_consumption_model_coeff.size());
}

double  Motor_current_consumption_model::get_current(const double value, const double timestep) {
    double current = get_current(value);
    current = current - (m_c / m_r) * (current - m_last_current) /
              (timestep);  // for RPM the dynamic response component equals 0
    m_last_current = current;
    return current;
}


#include <boost/test/unit_test.hpp>
#include <moreorg/OrganizationModel.hpp>
#include <moreorg/OrganizationModelAsk.hpp>
#include <moreorg/Algebra.hpp>
#include "test_utils.hpp"
#include <moreorg/vocabularies/OM.hpp>
#include <moreorg/algebra/Connectivity.hpp>
#include <graph_analysis/BaseGraph.hpp>
#include <graph_analysis/GraphIO.hpp>

using namespace moreorg;
using namespace moreorg::algebra;

BOOST_AUTO_TEST_SUITE(algebra)

BOOST_AUTO_TEST_CASE(max)
{
    ModelPool a;
    a["a"] = 1;
    a["b"] = 1;

    ModelPool b;
    b["a"] = 2;
    b["b"] = 1;
    b["c"] = 2;

    {
        ModelPool c = Algebra::max(a,b);
        BOOST_REQUIRE_MESSAGE( c["a"] == 2 && c["b"] == 1 && c["c"] == 2, "ModelPool max: expected, a:2,b:1,c:2 got " << c.toString());
    }

    ModelPool c;
    c["a"] = 4;
    c["b"] = 0;
    c["c"] = 1;
    c["d"] = 1;

    ModelPool::List poolList;
    poolList.push_back(a);
    poolList.push_back(b);
    poolList.push_back(c);

    ModelPool maxPool = Algebra::max(poolList);
    BOOST_REQUIRE_MESSAGE(maxPool["a"] == 4 && maxPool["b"] == 1 && maxPool["c"] == 2 && maxPool["d"] == 1,
            "ModelPool max of list: expected: a:4,b:1,c:2,d:1 got " << maxPool.toString());
}

BOOST_AUTO_TEST_CASE(min)
{
    ModelPool a;
    a["a"] = 1;
    a["b"] = 1;

    ModelPool b;
    b["a"] = 2;
    b["b"] = 0;
    b["c"] = 2;

    {
        ModelPool c = Algebra::min(a,b);
        BOOST_REQUIRE_MESSAGE( c["a"] == 1 && c["b"] == 0 && c["c"] == 2, "ModelPool min: expected, a:1,b:0,c:2 got " << c.toString());
    }

    ModelPool c;
    c["a"] = 4;
    c["b"] = 1;
    c["c"] = 1;
    c["d"] = 1;

    ModelPool::List poolList;
    poolList.push_back(a);
    poolList.push_back(b);
    poolList.push_back(c);

    ModelPool minPool = Algebra::min(poolList);
    BOOST_REQUIRE_MESSAGE(minPool["a"] == 1 && minPool["b"] == 0 && minPool["c"] == 1 && minPool["d"] == 1,
            "ModelPool min of list: expected: a:1,b:0,c:1,d:1 got " << minPool.toString());
}

BOOST_AUTO_TEST_CASE(resource_support)
{
    using namespace owlapi::vocabulary;
    using namespace owlapi::model;
    using namespace moreorg::vocabulary;

    OrganizationModel::Ptr om(new OrganizationModel(getRootDir() + "/test/data/om-schema-v0.7.owl"));
    OrganizationModelAsk ask(om);
    {

        IRI actor = OM::resolve("Actor");
        IRI sherpa = OM::resolve("Sherpa");
        IRI crex = OM::resolve("CREX");

        IRIList resources;
        resources.push_back(sherpa);
        resources.push_back(actor);
        resources.push_back(crex);

        base::VectorXd count = base::VectorXd::Zero(3);
        count(0) = 1;
        count(1) = 1;
        count(2) = 1;
        ResourceSupportVector supportVector(count, resources);

        ResourceSupportVector e_supportVector = supportVector.embedClassRelationship(ask);
        BOOST_TEST_MESSAGE("Test: " << e_supportVector.toString());
    }
    //{
    //    IRI system = OM::resolve("Sherpa");
    //    IRI service  = OM::resolve("StereoImageProvider");

    //    IRIList combination;
    //    combination.push_back(system);

    //    IRIList services;
    //    services.push_back(service);

    //    IRIList supportedServices = om.filterSupportedModels(combination, services);
    //    BOOST_REQUIRE_MESSAGE(supportedServices.size() == 1, "Sherpa support StereoImagerProvider");
    //}
}

BOOST_AUTO_TEST_CASE(resource_support_vector)
{
    {
        ResourceSupportVector nullSupport;
        BOOST_REQUIRE_MESSAGE(nullSupport.isNull(), "ResourceSupportVector is null");
    }

    {
        base::VectorXd aSizes(3);
        aSizes(0) = 1;
        aSizes(1) = 1;
        aSizes(2) = 1;

        ResourceSupportVector a(aSizes);

        base::VectorXd bSizes(3);
        bSizes(0) = 1;
        bSizes(1) = 1;
        bSizes(2) = 0;
        ResourceSupportVector b(bSizes);

        ResourceSupportVector intersection = ResourceSupportVector::intersection(a,b);

        BOOST_REQUIRE_MESSAGE(intersection(0) == 1, "Dim 0 --> 1");
        BOOST_REQUIRE_MESSAGE(intersection(1) == 1, "Dim 1 --> 1");
        BOOST_REQUIRE_MESSAGE(intersection(2) == 0, "Dim 1 --> 0");

        BOOST_REQUIRE_MESSAGE(a.contains(b), "ResourceSupportVector a contains b");
        BOOST_REQUIRE_MESSAGE(!b.contains(a), "ResourceSupportVector b does not contain a");

        {
            ResourceSupportVector missing = a.missingSupportFrom(b);
            BOOST_REQUIRE_MESSAGE(missing(2) == 1, "Missing support in dimension 2: " << missing(2));
        }
    }

    {
        base::VectorXd aSizes(3);
        aSizes(0) = 1;
        aSizes(1) = 1;
        aSizes(2) = 1;

        ResourceSupportVector a(aSizes);

        base::VectorXd bSizes(3);
        bSizes(0) = 0;
        bSizes(1) = 0;
        bSizes(2) = 3;
        ResourceSupportVector b(bSizes);

        ResourceSupportVector intersection = ResourceSupportVector::intersection(a,b);

        BOOST_REQUIRE_MESSAGE(intersection(0) == 0, "Dim 0 --> 0");
        BOOST_REQUIRE_MESSAGE(intersection(1) == 0, "Dim 1 --> 0");
        BOOST_REQUIRE_MESSAGE(intersection(2) == 1, "Dim 1 --> 1");
    }
}

BOOST_AUTO_TEST_CASE(composition)
{
    ModelPool empty;

    ModelPool a;
    a["http://model#a"] = 1;
    a["http://model#b"] = 4;
    a["http://model#c"] = 3;

    {
        ModelPool::Set aSet;
        aSet.insert(a);
        ModelPool::Set rSet = Algebra::maxCompositions(a, empty);
        BOOST_REQUIRE_MESSAGE(rSet == aSet, "Composition with model pool and empty set results in first argument");
    }
    {
        ModelPool::Set aSet;
        aSet.insert(a);
        ModelPool::Set rSet = Algebra::maxCompositions(empty,a);
        BOOST_REQUIRE_MESSAGE(rSet == aSet, "Composition with empty set and model pool results in second argument");
    }

    ModelPool b;
    b["http://model#a"] = 2;
    b["http://model#b"] = 4;
    b["http://model#c"] = 1;

    {
        ModelPool::Set aSet;
        aSet.insert(a);

        ModelPool::Set bSet;
        bSet.insert(b);

        // With min operator
        ModelPool::Set cSet = Algebra::maxCompositions(a,b);
        BOOST_REQUIRE_MESSAGE(cSet.size() == 1, "1 composition expected: was " << cSet.size());
        ModelPool c = *cSet.begin();
        BOOST_REQUIRE_MESSAGE(c["http://model#a"] == 2, "Composition entry a->2 expected, was a->" << c["http://model#a"]);
        BOOST_REQUIRE_MESSAGE(c["http://model#b"] == 4, "Composition entry b->4 expected, was b->" << c["http://model#b"]);
        BOOST_REQUIRE_MESSAGE(c["http://model#c"] == 3, "Composition entry c->3 expected, was c->" << c["http://model#c"]);
    }
    {
        ModelPool::Set aSet;
        aSet.insert(a);

        ModelPool::Set bSet;
        bSet.insert(a);
        bSet.insert(b);
        ModelPool::Set cSet = Algebra::maxCompositions(aSet,bSet);
        BOOST_REQUIRE_MESSAGE(cSet.size() == 2, "2 compositions expected: was 2 " << cSet.size() << ModelPool::toString(cSet));

        ModelPool e0 = a;
        BOOST_REQUIRE_MESSAGE(cSet.find(e0) != cSet.end(), "Compositions contains 'a[+]a'");

        ModelPool e1;
        e1["http://model#a"] = 2;
        e1["http://model#b"] = 4;
        e1["http://model#c"] = 3;
        BOOST_REQUIRE_MESSAGE(cSet.find(e1) != cSet.end(), "Compositions contains 'a[+]b'");
    }
    // Identical set
    {
        ModelPool::Set bSet;
        bSet.insert(a);
        bSet.insert(b);
        ModelPool::Set cSet = Algebra::maxCompositions(bSet,bSet);
        BOOST_REQUIRE_MESSAGE(cSet.size() == 3, "3 compositions expected: was 3 " << cSet.size() << ModelPool::toString(cSet));

        ModelPool e0 = a;
        BOOST_REQUIRE_MESSAGE(cSet.find(e0) != cSet.end(), "Compositions contains 'a[+]a'");

        ModelPool e1;
        e1["http://model#a"] = 2;
        e1["http://model#b"] = 4;
        e1["http://model#c"] = 3;
        BOOST_REQUIRE_MESSAGE(cSet.find(e1) != cSet.end(), "Compositions contains 'a[+]b'");

        ModelPool e2 = b;
        BOOST_REQUIRE_MESSAGE(cSet.find(e1) != cSet.end(), "Compositions contains 'b[+]b'");
    }
}

BOOST_AUTO_TEST_CASE(connectivity)
{
    OrganizationModel::Ptr om = make_shared<OrganizationModel>(getOMSchema());
    OrganizationModelAsk ask(om);

    owlapi::model::IRI ifModel0 = vocabulary::OM::resolve("EmiActive");
    owlapi::model::IRI ifModel1 = vocabulary::OM::resolve("EmiPassive");

    BOOST_REQUIRE_MESSAGE( ask.ontology().isRelatedTo(ifModel0,
                vocabulary::OM::compatibleWith(), ifModel1), "Interfaces are compatible");
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Payload")] = 2;
        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Payload")] = 10;

        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Payload")] = 20;

        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Payload")] = 40;

        graph_analysis::BaseGraph::Ptr baseGraph;
        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask,baseGraph,0), "ModelPool: " << modelPool.toString() );
        graph_analysis::io::GraphIO::write("/tmp/organization-model-connectivity-test-payload-10.dot", baseGraph);
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("CREX")] = 1;

        BOOST_REQUIRE_MESSAGE(Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );

    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("CREX")] = 2;

        BOOST_REQUIRE_MESSAGE( !Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("CREX")] = 10;

        BOOST_REQUIRE_MESSAGE( !Connectivity::isFeasible(modelPool, ask, 30000), "ModelPool: " << modelPool.toString() );

    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 1;
        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 2;
        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 20;
        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 1;
        modelPool[vocabulary::OM::resolve("CREX")] = 1;

        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 1;
        modelPool[vocabulary::OM::resolve("CREX")] = 10;

        BOOST_REQUIRE_MESSAGE(!Connectivity::isFeasible(modelPool, ask, 30000), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 4;
        modelPool[vocabulary::OM::resolve("CREX")] = 3;

        BOOST_REQUIRE_MESSAGE(Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 10;
        modelPool[vocabulary::OM::resolve("CREX")] = 10;

        graph_analysis::BaseGraph::Ptr baseGraph;
        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask, baseGraph,0), "ModelPool: " << modelPool.toString() );
        graph_analysis::io::GraphIO::write("/tmp/organization-model-connectivity-test-sherpa10-cex10.dot", baseGraph);
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 4;
        modelPool[vocabulary::OM::resolve("CREX")] = 10;

        BOOST_REQUIRE_MESSAGE(!Connectivity::isFeasible(modelPool, ask, 30000), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 6;
        modelPool[vocabulary::OM::resolve("CREX")] = 10;

        graph_analysis::BaseGraph::Ptr baseGraph;
        BOOST_REQUIRE_MESSAGE(!Connectivity::isFeasible(modelPool, ask, baseGraph, 30000), "ModelPool: " << modelPool.toString() );
        if(baseGraph)
        {
            graph_analysis::io::GraphIO::write("/tmp/organization-model-connectivity-test-sherpa6-cex10.dot", baseGraph);
        }
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 8;
        modelPool[vocabulary::OM::resolve("CREX")] = 10;

        BOOST_REQUIRE_MESSAGE(!Connectivity::isFeasible(modelPool, ask, 30000), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 9;
        modelPool[vocabulary::OM::resolve("CREX")] = 10;

        BOOST_REQUIRE_MESSAGE(Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }

    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("Sherpa")] = 5;
        modelPool[vocabulary::OM::resolve("CREX")] = 3;
        modelPool[vocabulary::OM::resolve("BaseCamp")] = 3;
        modelPool[vocabulary::OM::resolve("Payload")] = 10;

        // active: 2x5
        // passive: 3x3

        graph_analysis::BaseGraph::Ptr baseGraph;
        bool feasible = Connectivity::isFeasible(modelPool, ask, baseGraph,60000);
        if(baseGraph)
        {
            graph_analysis::io::GraphIO::write("/tmp/organization-model-connectivity-test-full-team_s5c3b5p25.dot", baseGraph);
        }
        BOOST_TEST_MESSAGE("Evaluation done: " << Connectivity::getStatistics().toString());
        BOOST_REQUIRE_MESSAGE(feasible, "ModelPool: " << modelPool.toString() );
    }
}

BOOST_AUTO_TEST_CASE(connectivity_multiple_interfaces)
{
    OrganizationModel::Ptr om(new OrganizationModel(getRootDir() + "/test/data/om-multiple-interfaces.owl") );
    OrganizationModelAsk ask(om);

    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("RobotA")] = 2;

        BOOST_REQUIRE_MESSAGE(!Connectivity::isFeasible(modelPool, ask), "RobotA 2 should not be a valid combination: ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("RobotB")] = 2;

        BOOST_REQUIRE_MESSAGE(!Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("RobotC")] = 2;

        BOOST_REQUIRE_MESSAGE( !Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );

    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("RobotA")] = 1;
        modelPool[vocabulary::OM::resolve("RobotB")] = 1;

        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("RobotA")] = 1;
        modelPool[vocabulary::OM::resolve("RobotC")] = 1;

        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("RobotB")] = 1;
        modelPool[vocabulary::OM::resolve("RobotC")] = 1;

        BOOST_REQUIRE_MESSAGE( !Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
    {
        ModelPool modelPool;
        modelPool[vocabulary::OM::resolve("RobotA")] = 1;
        modelPool[vocabulary::OM::resolve("RobotB")] = 1;
        modelPool[vocabulary::OM::resolve("RobotC")] = 1;

        BOOST_REQUIRE_MESSAGE( Connectivity::isFeasible(modelPool, ask), "ModelPool: " << modelPool.toString() );
    }
}

BOOST_AUTO_TEST_CASE(subset_superset)
{
    ModelPool modelPoolA;
    ModelPool modelPoolB;

    BOOST_REQUIRE_MESSAGE( Algebra::isSubset(modelPoolA, modelPoolB), "Empty models are subsets" );
    BOOST_REQUIRE_MESSAGE( Algebra::isSuperset(modelPoolA, modelPoolB), "Empty models are supersets" );

    modelPoolA[vocabulary::OM::resolve("RobotA")] = 1;
    modelPoolB[vocabulary::OM::resolve("RobotA")] = 1;

    BOOST_REQUIRE_MESSAGE( Algebra::isSubset(modelPoolA, modelPoolB), "Equal model pool are subsets" );
    BOOST_REQUIRE_MESSAGE( Algebra::isSubset(modelPoolB, modelPoolA), "Equal model pool are subsets" );
    BOOST_REQUIRE_MESSAGE( Algebra::isSuperset(modelPoolA, modelPoolB), "Equal model pool are supersets" );
    BOOST_REQUIRE_MESSAGE( Algebra::isSuperset(modelPoolB, modelPoolA), "Equal model pool are supersets" );

    modelPoolA[vocabulary::OM::resolve("RobotB")] = 1;
    BOOST_REQUIRE_MESSAGE( !Algebra::isSubset(modelPoolA, modelPoolB), "A is not subset of B");
    BOOST_REQUIRE_MESSAGE( Algebra::isSubset(modelPoolB, modelPoolA), "B is subset of A");
    BOOST_REQUIRE_MESSAGE( Algebra::isSuperset(modelPoolA, modelPoolB), "A is superset of B");
    BOOST_REQUIRE_MESSAGE( !Algebra::isSuperset(modelPoolB, modelPoolA), "B is not a superset of A");
}

BOOST_AUTO_TEST_SUITE_END()


#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>

#include<opencv2/core/core.hpp>

#include"../../ORB_SLAM2/include/System.h"
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Imu.h>

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

ros::Publisher  pose_pub;
ros::Publisher  imu_pub;

class EkfFusion
{
public:
    EkfFusion(){}

    void CamPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg)
    {
        Eigen::Vector3f cam_t(msg->pose.position.x,msg->pose.position.y,msg->pose.position.z);
        Eigen::Quaternionf cam_q(msg->pose.orientation.w,msg->pose.orientation.x,
                             msg->pose.orientation.y,msg->pose.orientation.z);

	cam_R_wc_ = cam_q.toRotationMatrix();

	Eigen::Matrix4f T_bc; //transformation matrix of camera frame wrt. body frame

	T_bc << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
         0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
        -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
         0.0, 0.0, 0.0, 1.0;

	Eigen::Matrix4f T_cb;
	T_cb = T_bc.inverse();

	Eigen::Matrix3f R_cb = T_cb.block<3,3>(0,0);
	//Eigen::Vector3f t_cb = T_cb.block<3,1>(0,3);

	Eigen::Vector3f acc_w = cam_R_wc_*(R_cb*acc_b_) + Eigen::Vector3f(0,9.81,0);

//         geometry_msgs::PoseStamped pose_msg;
//         pose_msg.pose.position.x = cam_t(0);
//         pose_msg.pose.position.y = cam_t(1);
//         pose_msg.pose.position.z = cam_t(2);
//         pose_msg.pose.orientation.x = cam_q.x();
//         pose_msg.pose.orientation.y = cam_q.y();
//         pose_msg.pose.orientation.z = cam_q.z();
//         pose_msg.pose.orientation.w = cam_q.w();
//         pose_msg.header = msg->header;
//         pose_pub.publish(pose_msg);
	sensor_msgs::Imu imu_msg;
        imu_msg.linear_acceleration.x = acc_w(0);
        imu_msg.linear_acceleration.y = acc_w(1);
        imu_msg.linear_acceleration.z = acc_w(2);
        imu_msg.header = msg->header;
        imu_pub.publish(imu_msg);
    }

    void ImuCallback(const sensor_msgs::ImuConstPtr& msg)
    {
        Eigen::Vector3f imu_acc(msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);
        Eigen::Vector3f imu_omega(msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);

	acc_b_ = 0.8*acc_b_ + 0.2*imu_acc;

//         sensor_msgs::Imu imu_msg;
//         imu_msg.angular_velocity.x = imu_omega(0);
//         imu_msg.angular_velocity.y = imu_omega(1);
//         imu_msg.angular_velocity.z = imu_omega(2);
//         imu_msg.linear_acceleration.x = imu_acc(0);
//         imu_msg.linear_acceleration.y = imu_acc(1);
//         imu_msg.linear_acceleration.z = imu_acc(2);
//         imu_msg.header = msg->header;
//         imu_pub.publish(imu_msg);
    }

    Eigen::Matrix3f cam_R_wc_;
    Eigen::Vector3f acc_b_;

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ekf_fusion");
    ros::start();

    EkfFusion ekf;

    ros::NodeHandle nodeHandler;

    pose_pub = nodeHandler.advertise<geometry_msgs::PoseStamped>("/ekf_pose",1);
    imu_pub  = nodeHandler.advertise<sensor_msgs::Imu>("/ekf_imu",1);
    ros::Subscriber cam_sub = nodeHandler.subscribe("/cam_pose", 1, &EkfFusion::CamPoseCallback, &ekf);
    ros::Subscriber imu_sub = nodeHandler.subscribe("/imu", 1, &EkfFusion::ImuCallback, &ekf);

    ros::spin();

    ros::shutdown();

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_REM_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_REM_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_arithmetic
 * \defgroup boost_simd_arithmetic_rem rem
 *
 * \par Description
 * This function computes the floating-point remainder of dividing a0 by a1.
 * The return value is a0-n*a1, where n is the quotient a0/a1 rounded
 * toward zero to an integer.\par
 * if one prefer: if a1 is zero returns a0, else return
 * a0-divfix(a0,a1)*a1
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/rem.hpp>
 * \endcode
 *
 * \par Alias
 * \arg fmod
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::rem_(A0,A0)>::type
 *     rem(const A0 & a0,const A0 & a1);
 * }
 * \endcode
 *
 * \param a0 the first parameter of rem
 * \param a1 the second parameter of rem
 *
 * \return a value of the common type of the parameters
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag rem_ of functor rem
     *        in namespace boost::simd::tag for toolbox boost.simd.arithmetic
    **/
    struct rem_ : ext::elementwise_<rem_> { typedef ext::elementwise_<rem_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::rem_, rem, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::rem_, fmod, 2)
} }

#endif

// modified by jt the 25/12/2010


#ifndef HOUSE_H
#define HOUSE_H

#include <Eigen/Dense>

#include <vector>

class HOUSE {

    public:

        // Dynamical model type
        typedef std::function <Eigen::VectorXd (double, double,
            const Eigen::VectorXd&, const Eigen::VectorXd&)> dyn_model;

        // Measurement model type
        typedef std::function <Eigen::VectorXd (double,
            const Eigen::VectorXd&, const Eigen::VectorXd&)> meas_model;

        // Distribution type
        class Dist {

            public:

                int n;

                Eigen::VectorXd mean, skew, kurt;
                Eigen::MatrixXd cov, covL;

                // Generate distribution from sigma points & weights
                Dist(const Eigen::MatrixXd& X, const Eigen::VectorXd& w);

                // Generate zero-mean Gaussian distribution
                Dist(const Eigen::MatrixXd& S);

        };

        // Augmented state sigma point type
        class Sigma {

            public:

                int n_state, n_noise, n_pts;

                Eigen::MatrixXd state, noise;
                Eigen::VectorXd wgt;

                Sigma(const Dist& distX, const Dist& distW, double delta);

        };

        // Constructor
        HOUSE(
            const dyn_model& f_,
            const meas_model& h_,
            int nz_,
            double t0,
            const Dist& distx0,
            const Dist& distw_,
            const Dist& distv_,
            double delta_
        );

        // Prediction step
        void predict(double tp);

        // Update step with one measurement
        void update(const Eigen::VectorXd& z);

        // Run filter for sequence of measurements
        void run(const Eigen::VectorXd& tz, const Eigen::MatrixXd& Z);

        // Dynamical model
        const dyn_model f;

        // Measurement model type
        const meas_model h;

        // Dimensions of state & measurement
        const int nx, nz;

        // Dimensions of process & measurement noise
        const int nw, nv;

        // Distribution of process noise
        const Dist distw;

        // Distribution of measurement noise
        const Dist distv;

        // Minimal weight at mean
        const double delta;

        // History of state estimate distributions
        std::vector<Dist> distx;

        // Times
        std::vector<double> t;

        // Reset filter
        void reset(double t0, const Dist& distx0);

        // Save results
        void save(const std::string& filename);

};

#endif


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <string>
#include <random>
#include <set>
#include <sstream>
#include <vector>
#include <algorithm>
#include <util/cityhash_tc.hpp>
#include <cmath>

#include <numerics/armadillo.hpp>

// ML-Data Utils
#include <unity/toolkits/ml_data_2/standardization-inl.hpp>

// Testing utils common to all of ml_data
#include <unity/toolkits/ml_data_2/testing_utils.hpp>

using namespace turi;

typedef arma::vec DenseVector;
typedef sparse_vector<double, size_t> SparseVector;

typedef arma::mat DenseMatrix;

struct standardization  {

  public:

  /*
   * Test the L2-scaler by generating random points and then projecting them
   * and inverse projecting back to get the same point back.
   */
  void _run_l2_scaling_test(size_t n, const std::string& run_string) {

    sframe X;
    v2::ml_data data;
    std::tie(X, data) = v2::make_random_sframe_and_ml_data(n, run_string);
    TS_ASSERT(n == X.size());
    TS_ASSERT(n == data.size());

    // Take a snapshot of the created metadata
    std::shared_ptr<v2::ml_metadata> metadata = data.metadata();
    size_t total_size;
    std::shared_ptr<l2_rescaling> scaler;
    DenseVector x, ans, sp1, sp2;
    SparseVector sp_x, sp_ans;
    DenseMatrix Xmat;

    scaler.reset(new l2_rescaling(metadata, true));

    // Test for the reference encoding section.
    // ---------------------------------------------------------------------
    total_size = scaler->get_total_size();

    x.resize(total_size);
    sp_x.resize(total_size);
    sp1.resize(total_size);
    sp2.resize(total_size);
    ans.resize(total_size);
    sp_ans.resize(total_size);
    Xmat.resize(n, total_size);

    for(auto it = data.get_iterator(0,1,false,true); !it.done(); ++it){
      // Densevector
      x.zeros();
      it.fill_row_expr(x);
      ans = x;
      scaler->transform(x);
      Xmat.row(it.row_index()) = x.t();
      sp1 = x;
      scaler->inverse_transform(x);
      TS_ASSERT(approx_equal(x, ans, "absdiff",  1e-5));

      // Sparse vector
      sp_x.zeros();
      it.fill_row_expr(sp_x);
      sp_ans = sp_x;
      scaler->transform(sp_x);
      sp2 = sp_x;
      TS_ASSERT(approx_equal(sp1, sp2, "absdiff",  1e-5));
      scaler->inverse_transform(sp_x);
      TS_ASSERT(approx_equal(sp_x, sp_ans, "absdiff", 1e-5));
    }

    // Check that each col has norm 1
    for(size_t i = 0; i < Xmat.n_cols-1; i++){
      TS_ASSERT(std::abs(arma::norm(Xmat.col(i), 2)/std::sqrt(n) - 1) < 3e-1);
    }

    // Test without reference encoding.
    // ---------------------------------------------------------------------
    scaler.reset(new l2_rescaling(metadata, false));
    total_size = scaler->get_total_size();
    x.resize(total_size);
    sp_x.resize(total_size);
    sp1.resize(total_size);
    sp2.resize(total_size);
    ans.resize(total_size);
    sp_ans.resize(total_size);
    Xmat.resize(n, total_size);

    for(auto it = data.get_iterator(); !it.done(); ++it){
      x.zeros();
      it.fill_row_expr(x);
      ans = x;
      scaler->transform(x);
      Xmat.row(it.row_index()) = x.t();
      sp1 = x;
      scaler->inverse_transform(x);
      TS_ASSERT(approx_equal(x, ans, "absdiff",  1e-5));

      // Sparse vector
      sp_x.zeros();
      it.fill_row_expr(sp_x);
      sp_ans = sp_x;
      scaler->transform(sp_x);
      sp2 = sp_x;
      TS_ASSERT(approx_equal(sp1, sp2, "absdiff",  1e-5));
      scaler->inverse_transform(sp_x);
      TS_ASSERT(approx_equal(sp_x, sp_ans, "absdiff",  1e-5));
    }

    // Check that each col has norm 1
    for(size_t i = 0; i < Xmat.n_cols-1; i++){
      TS_ASSERT(std::abs(arma::norm(Xmat.col(i), 2)/std::sqrt(n) - 1) < 3e-1);
    }

    // Save and Load
    dir_archive archive_write;
    archive_write.open_directory_for_write("standardization_tests");
    turi::oarchive oarc(archive_write);
    oarc << *scaler;
    archive_write.close();
    dir_archive archive_read;
    archive_read.open_directory_for_read("standardization_tests");
    turi::iarchive iarc(archive_read);
    iarc >> *scaler;

    // Test after save and load
    // ---------------------------------------------------------------------
    TS_ASSERT(total_size == scaler->get_total_size());
    total_size = scaler->get_total_size();
    x.resize(total_size);
    sp_x.resize(total_size);
    sp1.resize(total_size);
    sp2.resize(total_size);
    ans.resize(total_size);
    sp_ans.resize(total_size);
    Xmat.resize(n, total_size);

    for(auto it = data.get_iterator(); !it.done(); ++it){
      // Densevector
      x.zeros();
      it.fill_row_expr(x);
      ans = x;
      scaler->transform(x);
      Xmat.row(it.row_index()) = x.t();
      sp1 = x;
      scaler->inverse_transform(x);
      TS_ASSERT(approx_equal(x, ans, "absdiff",  1e-5));

      // Sparse vector
      sp_x.zeros();
      it.fill_row_expr(sp_x);
      sp_ans = sp_x;
      scaler->transform(sp_x);
      sp2 = sp_x;
      TS_ASSERT(approx_equal(sp1, sp2, "absdiff",  1e-5));
      scaler->inverse_transform(sp_x);
      TS_ASSERT(approx_equal(sp_x, sp_ans, "absdiff",  1e-5));
    }

    // Check that each col has norm 1
    for(size_t i = 0; i < Xmat.n_cols-1; i++){
      TS_ASSERT(std::abs(arma::norm(Xmat.col(i))/std::sqrt(n) - 1) < 3e-1);
    }
  }

  void test_standardization_n() {
    _run_l2_scaling_test(100, "n");
  }

  void test_standardization_v() {
    _run_l2_scaling_test(100, "v");
  }

};

BOOST_FIXTURE_TEST_SUITE(_standardization, standardization)
BOOST_AUTO_TEST_CASE(test_standardization_n) {
  standardization::test_standardization_n();
}
BOOST_AUTO_TEST_CASE(test_standardization_v) {
  standardization::test_standardization_v();
}
BOOST_AUTO_TEST_SUITE_END()


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Detail/CyclicPolygons.h"

#include "Molassembler/Temple/Random.h"
#include "Molassembler/Temple/Stringify.h"
#include "Molassembler/Temple/constexpr/Jsf.h"

#include <iomanip>
#include <iostream>
#include <fstream>

using namespace Scine::Molassembler;
using namespace std::string_literals;

extern Temple::Generator<> generator;

void writeAngleAnalysisFiles(
  const std::vector<double>& edgeLengths,
  const std::string& baseName
) {
  using namespace CyclicPolygons;

  const double longestEdge = Temple::max(edgeLengths);
  const double minR = longestEdge / 2 + 1e-10;
  const double lowerBound = minR;
  const double upperBound = std::max(
    Detail::regularCircumradius(edgeLengths.size(), longestEdge),
    minR
  );

  double rootGuess = Detail::regularCircumradius(
    edgeLengths.size(),
    std::max(Temple::average(edgeLengths), minR)
  );

  if(rootGuess < lowerBound) {
    rootGuess = lowerBound;
  } else if(rootGuess > upperBound) {
    rootGuess = upperBound;
  }

  double circumradius;
  bool circumcenterInside;

  std::tie(circumradius, circumcenterInside) = CyclicPolygons::Detail::convexCircumradius(edgeLengths);

  std::ofstream scanFile(baseName + ".csv"s);
  scanFile << std::fixed << std::setprecision(8);

  const unsigned nScanSteps = 1000;
  const double stepSize = (upperBound - lowerBound) / nScanSteps;
  for(unsigned i = 0; i <= nScanSteps; i++) {
    const double currentR = lowerBound + i * stepSize;
    scanFile << currentR << ", "
      << Detail::circumcenterInside::centralAnglesDeviation(currentR, edgeLengths)
      << ", "
      << Detail::circumcenterOutside::centralAnglesDeviation(currentR, edgeLengths, longestEdge)
      << std::endl;
  }

  scanFile.close();

  std::ofstream metaFile(baseName + "-meta.csv"s);
  metaFile << std::fixed << std::setprecision(8);
  for(unsigned i = 0; i < edgeLengths.size(); i++) {
    metaFile << edgeLengths[i];
    if(i != edgeLengths.size() - 1) {
      metaFile << ", ";
    }
  }
  metaFile << std::endl;
  metaFile << rootGuess << ", " << circumradius << std::endl;
  metaFile.close();
}

BOOST_AUTO_TEST_CASE(CentralAngleRootFinding, *boost::unit_test::label("Molassembler")) {
  unsigned failureIndex = 0;

  const double upperLimit = 5.6; // Fr-Fr single
  const double lowerLimit = 0.7; // H-H single

  const unsigned nTests = 1000;

  for(unsigned nSides = 3; nSides < 10; ++nSides) {
    for(unsigned testNumber = 0; testNumber < nTests; testNumber++) {
      std::vector<double> edgeLengths = Temple::Random::getN<double>(lowerLimit, upperLimit, nSides, generator.engine);
      while(!CyclicPolygons::exists(edgeLengths)) {
        edgeLengths = Temple::Random::getN<double>(lowerLimit, upperLimit, nSides, generator.engine);
      }

      double circumradius = std::nan("");
      bool circumcenterInside = false;

      auto assignCircumradius = [&]() -> void {
        std::tie(circumradius, circumcenterInside) = CyclicPolygons::Detail::convexCircumradius(
          edgeLengths
        );
      };

      BOOST_CHECK_NO_THROW(assignCircumradius());
      BOOST_CHECK(!std::isnan(circumradius));

      double deviation;
      if(circumcenterInside) {
        deviation = CyclicPolygons::Detail::circumcenterInside::centralAnglesDeviation(
          circumradius,
          edgeLengths
        );
      } else {
        deviation = CyclicPolygons::Detail::circumcenterOutside::centralAnglesDeviation(
          circumradius,
          edgeLengths,
          Temple::max(edgeLengths)
        );
      }

      bool pass = std::fabs(deviation) < 1e-4;

      BOOST_CHECK_MESSAGE(
        pass,
        "Central angle deviation norm is not smaller than 1e-5 for " << Temple::stringify(edgeLengths)
          << ", circumcenter is inside: " << std::boolalpha << circumcenterInside << ", deviation: " << deviation
      );

      const double internalAngleSumDeviation = Temple::sum(
        CyclicPolygons::Detail::generalizedInternalAngles(edgeLengths, circumradius, circumcenterInside)
      ) - (nSides - 2) * M_PI;

      pass = pass && (std::fabs(internalAngleSumDeviation) < 1e-5);

      BOOST_CHECK_MESSAGE(
        std::fabs(internalAngleSumDeviation) < 1e-5,
        "Internal angle sum deviation from " << (nSides - 2)
          <<  "π for edge lengths " << Temple::stringify(edgeLengths)
          << " is " << internalAngleSumDeviation
          << ", whose norm is not less than 1e-5"
      );

      if(!pass) {
        writeAngleAnalysisFiles(
          edgeLengths,
          "angle-failure-"s + std::to_string(failureIndex)
        );

        ++failureIndex;
      }
    }
  }
}