// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "../type_template.hpp"
#include "../check_arrays_are_close.hpp"

#include "assembly/blocked_boundary_operator.hpp"
#include "assembly/blocked_operator_structure.hpp"
#include "assembly/context.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/identity_operator.hpp"
#include "assembly/laplace_3d_single_layer_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"
#include "grid/grid_factory.hpp"
#include "grid/grid.hpp"
#include "space/piecewise_constant_scalar_space.hpp"
#include "space/piecewise_linear_continuous_scalar_space.hpp"

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

using namespace Bempp;

// Tests

BOOST_AUTO_TEST_SUITE(BlockedBoundaryOperator)

BOOST_AUTO_TEST_CASE_TEMPLATE(blocked_boundary_operator_produces_correct_weak_form_for_1x1_operator,
                              ValueType, result_types)
{
    // space | PL
    // ------+---
    // PC    |  V

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pwiseLinears, pwiseLinears, pwiseConstants);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 0, op00);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> nonblockedWeakForm = op00.weakForm()->asMatrix();
    arma::Mat<RT> blockedWeakForm = blockedOp.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, blockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blocked_boundary_operator_produces_correct_weak_form_for_2x1_operator,
                              ValueType, result_types)
{
    // space  | PL0
    // -------+---
    // PC0    |  V
    // PL1    |  V

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid0 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented.msh",
                false /* verbose */);
    shared_ptr<Grid> grid1 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-2.msh",
                false /* verbose */);

    shared_ptr<Space<BFT> > pc0(new PiecewiseConstantScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pl0(new PiecewiseLinearContinuousScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pc1(new PiecewiseConstantScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pl1(new PiecewiseLinearContinuousScalarSpace<BFT>(grid1));

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pl0, pc0);
    BoundaryOperator<BFT, RT> op10 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pc1, pl1);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 0, op00);
    structure.setBlock(1, 0, op10);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> mat00 = op00.weakForm()->asMatrix();
    arma::Mat<RT> mat10 = op10.weakForm()->asMatrix();
    arma::Mat<RT> nonblockedWeakForm = arma::join_cols(mat00, mat10);
    arma::Mat<RT> blockedWeakForm = blockedOp.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, blockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blocked_boundary_operator_produces_correct_weak_form_for_2x3_operator,
                              ValueType, result_types)
{
    // space  | PL0 | PC1 | PL2
    // -------+-----+-----+----
    // PC0    |  V  |  V  |  V
    // PL2    |  V  |  V  |  V

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid0 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented.msh",
                false /* verbose */);
    shared_ptr<Grid> grid1 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-2.msh",
                false /* verbose */);
    shared_ptr<Grid> grid2 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-4.msh",
                false /* verbose */);

    shared_ptr<Space<BFT> > pc0(new PiecewiseConstantScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pl0(new PiecewiseLinearContinuousScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pc1(new PiecewiseConstantScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pl1(new PiecewiseLinearContinuousScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pc2(new PiecewiseConstantScalarSpace<BFT>(grid2));
    shared_ptr<Space<BFT> > pl2(new PiecewiseLinearContinuousScalarSpace<BFT>(grid2));

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pl0, pc0);
    BoundaryOperator<BFT, RT> op01 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pl0, pc0);
    BoundaryOperator<BFT, RT> op02 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pl0, pc0);
    BoundaryOperator<BFT, RT> op10 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pc2, pl2);
    BoundaryOperator<BFT, RT> op11 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pc2, pl2);
    BoundaryOperator<BFT, RT> op12 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pc2, pl2);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 0, op00);
    structure.setBlock(0, 1, op01);
    structure.setBlock(0, 2, op02);
    structure.setBlock(1, 0, op10);
    structure.setBlock(1, 1, op11);
    structure.setBlock(1, 2, op12);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> mat00 = op00.weakForm()->asMatrix();
    arma::Mat<RT> mat01 = op01.weakForm()->asMatrix();
    arma::Mat<RT> mat02 = op02.weakForm()->asMatrix();
    arma::Mat<RT> mat10 = op10.weakForm()->asMatrix();
    arma::Mat<RT> mat11 = op11.weakForm()->asMatrix();
    arma::Mat<RT> mat12 = op12.weakForm()->asMatrix();
    arma::Mat<RT> nonblockedWeakForm =
        arma::join_rows(arma::join_rows(arma::join_cols(mat00, mat10),
                                        arma::join_cols(mat01, mat11)),
                        arma::join_cols(mat02, mat12));
    arma::Mat<RT> blockedWeakForm = blockedOp.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, blockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blocked_boundary_operator_produces_correct_weak_form_for_2x3_operator_with_empty_blocks,
                              ValueType, result_types)
{
    // space  | PL0 | PC1 | PL2
    // -------+-----+-----+----
    // PC0    |  0  |  0  |  V
    // PL2    |  V  |  V  |  0

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid0 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented.msh",
                false /* verbose */);
    shared_ptr<Grid> grid1 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-2.msh",
                false /* verbose */);
    shared_ptr<Grid> grid2 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-4.msh",
                false /* verbose */);

    shared_ptr<Space<BFT> > pc0(new PiecewiseConstantScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pl0(new PiecewiseLinearContinuousScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pc1(new PiecewiseConstantScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pl1(new PiecewiseLinearContinuousScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pc2(new PiecewiseConstantScalarSpace<BFT>(grid2));
    shared_ptr<Space<BFT> > pl2(new PiecewiseLinearContinuousScalarSpace<BFT>(grid2));

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pl0, pc0);
    BoundaryOperator<BFT, RT> op01 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pl0, pc0);
    BoundaryOperator<BFT, RT> op02 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pl0, pc0);
    BoundaryOperator<BFT, RT> op10 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pc2, pl2);
    BoundaryOperator<BFT, RT> op11 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pc2, pl2);
    BoundaryOperator<BFT, RT> op12 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pc2, pl2);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 2, op02);
    structure.setBlock(1, 0, op10);
    structure.setBlock(1, 1, op11);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> mat00 = 0. * op00.weakForm()->asMatrix();
    arma::Mat<RT> mat01 = 0. * op01.weakForm()->asMatrix();
    arma::Mat<RT> mat02 = op02.weakForm()->asMatrix();
    arma::Mat<RT> mat10 = op10.weakForm()->asMatrix();
    arma::Mat<RT> mat11 = op11.weakForm()->asMatrix();
    arma::Mat<RT> mat12 = 0. * op12.weakForm()->asMatrix();
    arma::Mat<RT> nonblockedWeakForm =
        arma::join_rows(arma::join_rows(arma::join_cols(mat00, mat10),
                                        arma::join_cols(mat01, mat11)),
                        arma::join_cols(mat02, mat12));
    arma::Mat<RT> blockedWeakForm = blockedOp.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, blockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(asDiscreteAcaBoundaryOperator_produces_correct_weak_form_for_1x1_operator,
                              ValueType, result_types)
{
    // space | PL
    // ------+---
    // PC    |  V

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AssemblyOptions assemblyOptions;
    AcaOptions acaOptions;
    acaOptions.minimumBlockSize = 2;
    assemblyOptions.switchToAcaMode(acaOptions);
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pwiseLinears, pwiseLinears, pwiseConstants);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 0, op00);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> nonblockedWeakForm = op00.weakForm()->asMatrix();
    arma::Mat<RT> acaBlockedWeakForm =
            blockedOp.weakForm()->asDiscreteAcaBoundaryOperator()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, acaBlockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(asDiscreteAcaBoundaryOperator_produces_correct_weak_form_for_2x1_operator,
                              ValueType, result_types)
{
    // space  | PL0
    // -------+---
    // PC0    |  V
    // PL1    |  V

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid0 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented.msh",
                false /* verbose */);
    shared_ptr<Grid> grid1 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-2.msh",
                false /* verbose */);

    shared_ptr<Space<BFT> > pc0(new PiecewiseConstantScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pl0(new PiecewiseLinearContinuousScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pc1(new PiecewiseConstantScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pl1(new PiecewiseLinearContinuousScalarSpace<BFT>(grid1));

    AssemblyOptions assemblyOptions;
    AcaOptions acaOptions;
    acaOptions.minimumBlockSize = 2;
    assemblyOptions.switchToAcaMode(acaOptions);
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pl0, pc0);
    BoundaryOperator<BFT, RT> op10 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pc1, pl1);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 0, op00);
    structure.setBlock(1, 0, op10);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> mat00 = op00.weakForm()->asMatrix();
    arma::Mat<RT> mat10 = op10.weakForm()->asMatrix();
    arma::Mat<RT> nonblockedWeakForm = arma::join_cols(mat00, mat10);
    arma::Mat<RT> acaBlockedWeakForm =
            blockedOp.weakForm()->asDiscreteAcaBoundaryOperator()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, acaBlockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(asDiscreteAcaBoundaryOperator_produces_correct_weak_form_for_2x3_operator,
                              ValueType, result_types)
{
    // space  | PL0 | PC1 | PL2
    // -------+-----+-----+----
    // PC0    |  V  |  V  |  V
    // PL2    |  V  |  V  |  V

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid0 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented.msh",
                false /* verbose */);
    shared_ptr<Grid> grid1 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-2.msh",
                false /* verbose */);
    shared_ptr<Grid> grid2 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-4.msh",
                false /* verbose */);

    shared_ptr<Space<BFT> > pc0(new PiecewiseConstantScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pl0(new PiecewiseLinearContinuousScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pc1(new PiecewiseConstantScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pl1(new PiecewiseLinearContinuousScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pc2(new PiecewiseConstantScalarSpace<BFT>(grid2));
    shared_ptr<Space<BFT> > pl2(new PiecewiseLinearContinuousScalarSpace<BFT>(grid2));

    AssemblyOptions assemblyOptions;
    AcaOptions acaOptions;
    acaOptions.minimumBlockSize = 2;
    assemblyOptions.switchToAcaMode(acaOptions);
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pl0, pc0);
    BoundaryOperator<BFT, RT> op01 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pl0, pc0);
    BoundaryOperator<BFT, RT> op02 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pl0, pc0);
    BoundaryOperator<BFT, RT> op10 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pc2, pl2);
    BoundaryOperator<BFT, RT> op11 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pc2, pl2);
    BoundaryOperator<BFT, RT> op12 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pc2, pl2);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 0, op00);
    structure.setBlock(0, 1, op01);
    structure.setBlock(0, 2, op02);
    structure.setBlock(1, 0, op10);
    structure.setBlock(1, 1, op11);
    structure.setBlock(1, 2, op12);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> mat00 = op00.weakForm()->asMatrix();
    arma::Mat<RT> mat01 = op01.weakForm()->asMatrix();
    arma::Mat<RT> mat02 = op02.weakForm()->asMatrix();
    arma::Mat<RT> mat10 = op10.weakForm()->asMatrix();
    arma::Mat<RT> mat11 = op11.weakForm()->asMatrix();
    arma::Mat<RT> mat12 = op12.weakForm()->asMatrix();
    arma::Mat<RT> nonblockedWeakForm =
        arma::join_rows(arma::join_rows(arma::join_cols(mat00, mat10),
                                        arma::join_cols(mat01, mat11)),
                        arma::join_cols(mat02, mat12));
    arma::Mat<RT> acaBlockedWeakForm =
            blockedOp.weakForm()->asDiscreteAcaBoundaryOperator()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, acaBlockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(asDiscreteAcaBoundaryOperator_produces_correct_weak_form_for_2x3_operator_with_empty_blocks,
                              ValueType, result_types)
{
    // space  | PL0 | PC1 | PL2
    // -------+-----+-----+----
    // PC0    |  0  |  0  |  V
    // PL2    |  V  |  V  |  0

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid0 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented.msh",
                false /* verbose */);
    shared_ptr<Grid> grid1 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-2.msh",
                false /* verbose */);
    shared_ptr<Grid> grid2 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented-shifted-on-x-by-4.msh",
                false /* verbose */);

    shared_ptr<Space<BFT> > pc0(new PiecewiseConstantScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pl0(new PiecewiseLinearContinuousScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pc1(new PiecewiseConstantScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pl1(new PiecewiseLinearContinuousScalarSpace<BFT>(grid1));
    shared_ptr<Space<BFT> > pc2(new PiecewiseConstantScalarSpace<BFT>(grid2));
    shared_ptr<Space<BFT> > pl2(new PiecewiseLinearContinuousScalarSpace<BFT>(grid2));

    AssemblyOptions assemblyOptions;
    AcaOptions acaOptions;
    acaOptions.minimumBlockSize = 2;
    assemblyOptions.switchToAcaMode(acaOptions);
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pl0, pc0);
    BoundaryOperator<BFT, RT> op01 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pl0, pc0);
    BoundaryOperator<BFT, RT> op02 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pl0, pc0);
    BoundaryOperator<BFT, RT> op10 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pc2, pl2);
    BoundaryOperator<BFT, RT> op11 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pc1, pc2, pl2);
    BoundaryOperator<BFT, RT> op12 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl2, pc2, pl2);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 2, op02);
    structure.setBlock(1, 0, op10);
    structure.setBlock(1, 1, op11);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> mat00 = 0. * op00.weakForm()->asMatrix();
    arma::Mat<RT> mat01 = 0. * op01.weakForm()->asMatrix();
    arma::Mat<RT> mat02 = op02.weakForm()->asMatrix();
    arma::Mat<RT> mat10 = op10.weakForm()->asMatrix();
    arma::Mat<RT> mat11 = op11.weakForm()->asMatrix();
    arma::Mat<RT> mat12 = 0. * op12.weakForm()->asMatrix();
    arma::Mat<RT> nonblockedWeakForm =
        arma::join_rows(arma::join_rows(arma::join_cols(mat00, mat10),
                                        arma::join_cols(mat01, mat11)),
                        arma::join_cols(mat02, mat12));
    arma::Mat<RT> acaBlockedWeakForm =
            blockedOp.weakForm()->asDiscreteAcaBoundaryOperator()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, acaBlockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(asDiscreteAcaBoundaryOperator_produces_correct_weak_form_for_2x1_operator_containing_sparse_block,
                              ValueType, result_types)
{
    // space  | PL0
    // -------+---
    // PC0    |  V
    // PL1    |  I

    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid0 = GridFactory::importGmshGrid(
                params, "meshes/cube-12-reoriented.msh",
                false /* verbose */);

    shared_ptr<Space<BFT> > pc0(new PiecewiseConstantScalarSpace<BFT>(grid0));
    shared_ptr<Space<BFT> > pl0(new PiecewiseLinearContinuousScalarSpace<BFT>(grid0));

    AssemblyOptions assemblyOptions;
    AcaOptions acaOptions;
    acaOptions.minimumBlockSize = 2;
    assemblyOptions.switchToAcaMode(acaOptions);
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op00 = laplace3dSingleLayerBoundaryOperator<BFT, RT>(
        context, pl0, pl0, pc0);
    BoundaryOperator<BFT, RT> op10 = identityOperator<BFT, RT>(
        context, pl0, pc0, pl0);

    BlockedOperatorStructure<BFT, RT> structure;
    structure.setBlock(0, 0, op00);
    structure.setBlock(1, 0, op10);
    Bempp::BlockedBoundaryOperator<BFT, RT> blockedOp(structure);

    arma::Mat<RT> mat00 = op00.weakForm()->asMatrix();
    arma::Mat<RT> mat10 = op10.weakForm()->asMatrix();
    arma::Mat<RT> nonblockedWeakForm = arma::join_cols(mat00, mat10);
    arma::Mat<RT> acaBlockedWeakForm =
            blockedOp.weakForm()->asDiscreteAcaBoundaryOperator()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    nonblockedWeakForm, acaBlockedWeakForm,
                    10. * std::numeric_limits<RealType>::epsilon()));
}
BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing saturate_at capabilities

    Returns the saturated value of the first input relative to the symmetric
    interval defined by the template parameter which has to be the tag of
    a constant.

    @par Semantic:

    @code
    T r = saturate <Tag>(x)
    @endcode

    is similar to:

    @code
    if (a0 > Tag<T>()) r =  Tag<T>();
    else if  (a0 < -Tag<T>()) r = -Tag<T>() ;
    else r = a0;
    @endcode

  **/
  const boost::dispatch::functor<tag::saturate_at_> saturate_at = {};
} }
#endif

#include <boost/simd/function/scalar/saturate_at.hpp>

#endif


#pragma once
#include <autoppl/util/var_expr_traits.hpp>
#include <autoppl/variable.hpp>
#include <autoppl/math/math.hpp>
#include <iostream>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

namespace ppl {
namespace expr {

#if __cplusplus <= 201703L
template <class UnaryOp, class LHSVarExprType>
#else
template <class UnaryOp, util::var_expr LHSVarExprType>
#endif
struct UnaryOpNode :
    util::VarExpr<UnaryOpNode<UnaryOp, LHSVarExprType> >
{
#if __cplusplus <= 201703L
	static_assert(util::assert_is_var_expr_v<LHSVarExprType>);

#endif

	using value_t = std::common_type_t<
		typename util::var_expr_traits<LHSVarExprType>::value_t>;

	UnaryOpNode(const LHSVarExprType& lhs)
		: lhs_{lhs}
	{ assert(true); }

    value_t get_value(size_t i = 0) const {
        auto lhs_value = lhs_.get_value(i);
        return UnaryOp::evaluate(lhs_value);
    }

    size_t size() const { return lhs_.size(); }

    std::set<boost::uuids::uuid> getDeps(){
	std::set<boost::uuids::uuid> used_vars;
	std::set<boost::uuids::uuid> lhs_used_vars = lhs_.getDeps(); //runtime polymorphism: whatever class mean_ is should have it's own getDeps() function
	return lhs_used_vars;
    }

private:
	LHSVarExprType lhs_;

};

struct StepOp {

	template <class LHSValueType>
	static auto evaluate(LHSValueType x)
	{
		LHSValueType zero_(0.);
		LHSValueType one_(1.);
		return x > zero_ ? one_ : zero_ ;
	}

};

} // namespace expr
} // namespace ppl


//
// Created by Mario Marchand on 17-11-28.
//

#include <iostream>
#include <boost/python.hpp>

#include "Heap.h"
#include "ActiveSequence.h"

namespace bp = boost::python;

bp::list removeOverlaps(const std::vector<double> &tentativeAlignmentPoint, double window_size)
{
    bp::list alignmentPoint;

    size_t n = tentativeAlignmentPoint.size();
    std::vector<bool> isOverlapping(n, false);

    if (n == 0) return alignmentPoint;

    for (size_t i = 0; i < (n - 1); ++i)
    {
        if ((1.0 + window_size) * tentativeAlignmentPoint.at(i) >=
            (1.0 - window_size) * tentativeAlignmentPoint.at(i + 1))
        {
            isOverlapping[i] = true;
            isOverlapping[i + 1] = true;
        }
    }
    for (size_t i = 0; i < n; ++i)
    {
        if (!isOverlapping.at(i))
        {
            alignmentPoint.append(tentativeAlignmentPoint[i]);
        }

    }
    return alignmentPoint;
}

bp::list alignmentPointDetection(const bp::list &peak, double window_size, bool for_vlm)
{
    std::vector<double> tentativeAlignmentPoint; //aligment points that may overlap; initially empty

    auto nbOfSpectra = static_cast<size_t>(len(peak));

    Heap heap(peak); //each spectra has its first peak in the min heap h
    ActiveSequence as(nbOfSpectra, window_size, for_vlm); //initially empty (contains zero peaks)

    bool found = false;

    while (!heap.empty())
    {
        if (as.isValid(heap)) found = true;

        if (!as.insert(heap, peak)) //was not able to insert next peak in as
        {
            if (found)
            {
                tentativeAlignmentPoint.push_back(as.getAverageMz());
                found = false;
            }
            as.advanceLowerBound();
        } else //the insertion in as has succeeded
        {
            if (heap.empty()) //if there are no more peak to insert you are done
            {
                while (!as.empty())
                {
                    if (as.isValid(heap))
                    {
                        tentativeAlignmentPoint.push_back(as.getAverageMz());
                        break;
                    }
                    as.advanceLowerBound();
                }
            }
        }
    }

    return removeOverlaps(tentativeAlignmentPoint, window_size);

}

bp::list find_vlm(const bp::list  & pylistOfpylists, const bp::object & pyws)
{
    double window_size = bp::extract<double>(pyws);
    return alignmentPointDetection(pylistOfpylists, window_size, true);
}

bp::list find_alpt(const bp::list  & pylistOfpylists, const bp::object & pyws)
{
    double window_size = bp::extract<double>(pyws);
    return alignmentPointDetection(pylistOfpylists, window_size, false);

//    bp::list rl;
//    std::vector<double>::iterator it;
//    for (it = alignmentPoint.begin(); it != alignmentPoint.end(); ++it)
//    {
//        rl.append(*it);
//    }
//    return rl;
}

BOOST_PYTHON_MODULE(msAlign)
{
    bp::def("find_vlm", &find_vlm);
    bp::def("find_alpt", &find_alpt);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_EQUAL_RANGE_HPP
#define BOOST_COMPUTE_ALGORITHM_EQUAL_RANGE_HPP

#include <utility>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/lower_bound.hpp>
#include <boost/compute/algorithm/upper_bound.hpp>

namespace boost {
namespace compute {

/// Returns a pair of iterators containing the range of values equal
/// to \p value in the sorted range [\p first, \p last).
///
/// Space complexity: \Omega(1)
template<class InputIterator, class T>
inline std::pair<InputIterator, InputIterator>
equal_range(InputIterator first,
            InputIterator last,
            const T &value,
            command_queue &queue = system::default_queue())
{
    return std::make_pair(
               ::boost::compute::lower_bound(first, last, value, queue),
               ::boost::compute::upper_bound(first, last, value, queue)
           );
}

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_EQUAL_RANGE_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_PIO2_2T_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_PIO2_2T_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pio2_2t generic tag

     Represents the Pio2_2t constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    // 2.02226624879595063154e-21
    BOOST_SIMD_CONSTANT_REGISTER( Pio2_2t, double
                                , 0, 0x2e85a308
                                , 0x3BA3198A2E037073ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pio2_2t, Site> dispatching_Pio2_2t(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Pio2_2t, Site>();
   }
   template<class... Args>
   struct impl_Pio2_2t;
  }
  /*!
    Constant used in modular computation involving \f$\pi\f$

    @par Semantic:

    For type T0:

    @code
    T0 r = Pio2_2t<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio2_2t, Pio2_2t);
}

#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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
//==============================================================================
#ifndef BOOST_DISPATCH_META_DOWNGRADE_HPP_INCLUDED
#define BOOST_DISPATCH_META_DOWNGRADE_HPP_INCLUDED

/*!
 * \file
 * \brief Defines and implements the \ref boost::dispatch::meta::downgrade \metafunction
 */

#include <boost/dispatch/meta/sign_of.hpp>
#include <boost/dispatch/meta/factory_of.hpp>
#include <boost/dispatch/meta/primitive_of.hpp>
#include <boost/dispatch/meta/make_integer.hpp>
#include <boost/dispatch/meta/make_floating.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/type_traits/is_void.hpp>

namespace boost { namespace dispatch { namespace meta
{
  //============================================================================
  /*!
   * Returns the input type rebound with a type that is twice as small as
   * its primitive, with optional sign change.\
   *
   * \tparam Type Hierarchizable type to downgrade.
   * \tparam Sign Optional sign to use while downgrading \c Type. If unspecified,
   * \c Sign is equals to the sign of \c Type.
   *
   * \par Models:
   *
   * \metafunction
   *
   * \semantic
   *
   * \code
   * typedef boost::dispatch::meta::downgrade<T, Sign>::type r;
   * \endcode
   *
   * is equivalent to:
   *
   * \code
   * typedef make_floating< max< sizeof(primitive_of<T>::type)/2
   *                       , sizeof(float)
   *                       >::value
   *                  , factory_of<T>::type
   *                  >::type                                  r;
   * \endcode
   *
   * if \c primitive_of<T> is of hierarchy \ref boost::dispatch::tag::floating_
   * and to:
   *
   * \code
   * typedef make_integer< max<sizeof(primitive_of<T>::type)/2, 1>::value
   *                     , Sign
   *                     , factory_of<T>::type
   *                     >::type                                r;
   * \endcode
   *
   * otherwise.
   *
   * \usage
   *
   * \include downgrade.cpp
   */
  //============================================================================
  template< class T, class Sign = void> struct  downgrade;
} } }

namespace boost { namespace dispatch { namespace ext
{
  template<typename Type, typename Sign, typename Enable = void>
  struct downgrade
  {
    typedef typename meta::factory_of<Type>::type    lambda;
    typedef typename meta::primitive_of<Type>::type  base;
    typedef typename meta::downgrade<base,Sign>::type  up;
    typedef typename mpl::apply1<lambda, up>::type   type;
  };

  template<typename T, typename Sign>
  struct downgrade< T, Sign
                  , typename enable_if<typename is_integral<T>::type>::type
                  >
  {
    typedef typename meta::factory_of<T>::type    lambda;
    typedef typename meta::primitive_of<T>::type  base;

    BOOST_STATIC_CONSTANT ( std::size_t
                          , size = (sizeof(base) > 1) ? sizeof(base)/2 : 1
                          );

    typedef typename mpl::eval_if < is_void<Sign>
                                  , meta::sign_of<T>
                                  , mpl::identity<Sign>
                                  >::type         sign;

    typedef typename meta::make_integer<size,sign,lambda>::type type;
  };

  template<typename T, typename Sign>
  struct downgrade< T, Sign
                  , typename enable_if<typename is_floating_point<T>::type>::type
                  >
  {
    typedef typename meta::factory_of<T>::type    lambda;
    typedef typename meta::primitive_of<T>::type  base;

    BOOST_STATIC_CONSTANT ( std::size_t
                          , size = (sizeof(base) > 1) ? sizeof(base)/2 : 1
                          );

    typedef typename meta::make_floating<size,lambda>::type  type;
  };
} } }

namespace boost { namespace dispatch { namespace meta
{
  template<class T,class Sign>
  struct  downgrade               : ext::downgrade<T, Sign> {};

  template<class T, class Sign>
  struct downgrade<T&,Sign>       : downgrade<T,Sign> {};

  template<class T, class Sign>
  struct downgrade<T const,Sign>  : downgrade<T,Sign> {};
} } }

#endif


#include "SkipList.hpp"
#include <benchmark/benchmark.h>
#include <boost/pool/pool_alloc.hpp>

// template <typename T>
// using skiplist = typename wijagels::skiplist<T, std::less<T>,
// boost::fast_pool_allocator<T>>;
using wijagels::skiplist;

static void BM_Default_Constructor(benchmark::State &state) {
  for (auto _ : state) {
    skiplist<int> dest;
    benchmark::DoNotOptimize(dest);
  }
}
BENCHMARK(BM_Default_Constructor);

/**
 * Demonstrate the efficiency of the copy constructor due to hinting
 */
static void BM_Copy_Constructor(benchmark::State &state) {
  std::mt19937 gen{};
  skiplist<int> src;
  std::uniform_int_distribution<> dis{};
  for (int i = 0; i < state.range(0); i++) {
    src.insert(dis(gen));
  }
  for (auto _ : state) {
    skiplist<int> dest{src};
  }
  state.SetComplexityN(src.size());
}
BENCHMARK(BM_Copy_Constructor)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();

static void BM_Copy_Manually(benchmark::State &state) {
  std::mt19937 gen{};
  skiplist<int> src;
  std::uniform_int_distribution<> dis{};
  for (int i = 0; i < state.range(0); i++) {
    src.insert(dis(gen));
  }
  for (auto _ : state) {
    skiplist<int> dest;
    for (const auto &e : src) {
      dest.insert(e);
    }
  }
  state.SetComplexityN(src.size());
}
BENCHMARK(BM_Copy_Manually)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();

template <typename T>
void BM_Insert(benchmark::State &state) {
  std::mt19937 gen{};
  std::vector<int> src;
  src.reserve(static_cast<decltype(src)::size_type>(state.range(0)));
  std::uniform_int_distribution<> dis{};
  for (int i = 0; i < state.range(0); i++) {
    src.push_back(dis(gen));
  }
  for (auto _ : state) {
    T dest;
    for (const auto &e : src) {
      dest.insert(e);
    }
  }
  state.SetComplexityN(state.range(0));
}
BENCHMARK_TEMPLATE(BM_Insert, skiplist<int>)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();
BENCHMARK_TEMPLATE(BM_Insert, std::set<int>)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();

template <typename T>
void BM_Find(benchmark::State &state) {
  std::mt19937 gen{};
  std::vector<int> src;
  src.reserve(static_cast<decltype(src)::size_type>(state.range(0)));
  std::uniform_int_distribution<> dis{};
  for (int i = 0; i < state.range(0); i++) {
    src.push_back(dis(gen));
  }
  T list{};
  list.insert(src.begin(), src.end());
  for (auto _ : state) {
    for (const auto &e : src) {
      benchmark::DoNotOptimize(list.find(e));
    }
  }
  state.SetComplexityN(list.size());
}
BENCHMARK_TEMPLATE(BM_Find, skiplist<int>)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();
BENCHMARK_TEMPLATE(BM_Find, std::set<int>)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();

template <typename T>
void BM_Erase(benchmark::State &state) {
  std::mt19937 gen{};
  std::vector<int> src;
  src.reserve(static_cast<decltype(src)::size_type>(state.range(0)));
  std::uniform_int_distribution<> dis{};
  for (int i = 0; i < state.range(0); i++) {
    src.push_back(dis(gen));
  }
  T list{};
  list.insert(src.begin(), src.end());
  for (auto _ : state) {
    state.PauseTiming();
    T copy{list};
    state.ResumeTiming();
    for (const auto &e : src) {
      copy.erase(e);
    }
  }
  state.SetComplexityN(list.size());
}
BENCHMARK_TEMPLATE(BM_Erase, skiplist<int>)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();
BENCHMARK_TEMPLATE(BM_Erase, std::set<int>)
    ->RangeMultiplier(2)
    ->Range(1 << 7, 1 << 14)
    ->Complexity();

BENCHMARK_MAIN();


// type_promotion.hpp
//
// Copyright (c) 2016 Piotr K. Semenov (piotr.k.semenov at gmail dot com)
// Distributed under the New BSD License. (See accompanying file LICENSE)

/*!
 \file type_promotion.hpp

 Provides the run-time overflow/underflow safety checks for basic arithmetics operations.
*/

#ifndef INC_LIBQ_TYPE_PROMOTION_HPP_
#define INC_LIBQ_TYPE_PROMOTION_HPP_

#include <boost/integer.hpp>
#include <boost/mpl/eval_if.hpp>

#include <limits>
#include <type_traits>

namespace libq {
namespace details {

/*!
 \brief
 \tparam T fixed-point type
 \tparam delta_n
 \tparam delta_f
*/
template<typename T, std::size_t delta_n, std::size_t delta_f, int delte_e>
class type_promotion_base {
 public:
    using promoted_type = T;
};

template<typename T, std::size_t _n, std::size_t _f, int _e, class op, class up, std::size_t delta_n, std::size_t delta_f, int delta_e>  // NOLINT
class type_promotion_base<libq::fixed_point<T, _n, _f, _e, op, up>, delta_n, delta_f, delta_e> {  // NOLINT
    using Q = libq::fixed_point<T, _n, _f, _e, op, up>;
    using this_class = type_promotion_base<Q, delta_n, delta_f, delta_e>;

    using max_type = typename std::conditional<Q::is_signed,
                                               std::intmax_t,
                                               std::uintmax_t>::type;
    enum: std::size_t {
        sign_bit = static_cast<std::size_t>(Q::is_signed),

        n = Q::bits_for_integral,
        f = Q::bits_for_fractional
    };
    enum: int {
        e = Q::scaling_factor_exponent
    };

    // simple "type" wrapper for lazy instantiation of its "internal" type
    struct storage_type_promotion_traits {
        // Note, boost::int_t takes a sign bit into account
        using type = typename std::conditional<Q::is_signed,
                                               typename boost::int_t<(n + delta_n) + (f + delta_f) + this_class::sign_bit>::least,  // NOLINT
                                               typename boost::uint_t<(n + delta_n) + (f + delta_f)>::least>::type;  // NOLINT
    };
    struct storage_type_default_traits {
        using type = typename Q::storage_type;
    };

 public:
    enum: bool {
        is_expandable = ((n + delta_n) + (f + delta_f) <=
                            std::numeric_limits<max_type>::digits)
    };

    /*!
     \brief
    */
    using promoted_storage_type =
        typename boost::mpl::eval_if_c<this_class::is_expandable,
                                       typename this_class::storage_type_promotion_traits,  // NOLINT
                                       typename this_class::storage_type_default_traits>::type;  // NOLINT

    using promoted_type = typename std::conditional<this_class::is_expandable,
        libq::fixed_point<promoted_storage_type, n + delta_n, f + delta_f, e + delta_e, typename Q::overflow_policy, typename Q::underflow_policy>,  // NOLINT
        libq::fixed_point<promoted_storage_type, n, f, e, typename Q::overflow_policy, typename Q::underflow_policy> >::type;  // NOLINT
};

}  // namespace details
}  // namespace libq
#endif  // INC_LIBQ_TYPE_PROMOTION_HPP_


//
//=======================================================================
// Copyright 2012
// Author: Alex Hagen-Zanker
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
// A helper to get the type of and make the bgl direct_d_ary_heap for use
// in the shortest path algorithm.
//
//=======================================================================
//
#ifndef BLINK_GRAPH_DIJKSTRA_QUEUE_HPP
#define BLINK_GRAPH_DIJKSTRA_QUEUE_HPP

#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp> // color_traits
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/shared_array_property_map.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp> //tie

#include <vector>

namespace blink {

template <typename T>
struct has_clear
{
private:
  typedef char yes_type[1];
  typedef char no_type[2];

  template <typename S, void( S::*)(void)>
  struct dummy {};

  template <typename S>
  static yes_type& check(dummy<S, &S::clear>*);

  template <typename S>
  static no_type& check (...);

public:
  static bool const value = sizeof( check<T>(0) ) == sizeof( yes_type );
  typedef boost::integral_constant<bool, value> type;
};

template<typename Queue>
void clear(Queue& queue, boost::false_type)
{
  while(!queue.empty()) {
    queue.pop();
  }
}

template<typename Queue>
void clear(Queue& queue, boost::true_type)
{
  queue.clear();
}

template<typename Queue>
void clear(Queue& queue)
{
  typedef typename has_clear<Queue>::type has;
  return clear(queue, has());
}

template <typename Graph, typename DistanceMap, typename IndexMap,
  typename Compare>
struct dijkstra_queue_bgl
{
  typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename boost::shared_array_property_map<std::size_t, IndexMap>
    index_in_heap_map;

  typedef boost::d_ary_heap_indirect<vertex_descriptor, 4, index_in_heap_map,
    DistanceMap, Compare, std::vector<vertex_descriptor> > type;

  // typedef boost::d_ary_heap_indirect<vertex_descriptor, 4, index_in_heap_map,
  //   DistanceMap, Compare, std::vector<vertex_descriptor>& > ref_type;

  static boost::shared_ptr<type> make_smart(const Graph& graph, DistanceMap distance, Compare compare,
    IndexMap indexmap)
  {
    return boost::shared_ptr<type>(new type(distance, index_in_heap_map(num_vertices(graph), indexmap)
      , compare) );
  }

  static type make(const Graph& graph, DistanceMap distance, Compare compare,
    IndexMap indexmap)
  {
    return type(distance, index_in_heap_map(num_vertices(graph), indexmap)
      , compare);
  }
};

template<typename Graph, typename ColorMap, typename DijkstraQueue>
void reset_dijkstra_queue_by_colormap(
      ColorMap colormap,
      const Graph& graph,
    DijkstraQueue& queue)
{
  typedef typename boost::graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename boost::property_traits<ColorMap>::value_type color_type;
  typedef boost::color_traits<color_type> color_traits;

  clear(queue);

  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = boost::vertices(graph); vi != vi_end; ++vi) {
    if(get(colormap, *vi) == color_traits::gray()) {
      queue.push(*vi);
    }
  }
}
}; // namespace blink

#endif // BLINK_GRAPH_DIJKSTRA_QUEUE_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_MZERO_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MZERO_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Mzero generic tag

     Represents the Mzero constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Mzero, double, 0
                                , 0x80000000, 0x8000000000000000ULL
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Mzero, Site> dispatching_Mzero(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Mzero, Site>();
   }
   template<class... Args>
   struct impl_Mzero;
  }
  /*!
    Generates value -0

    @par Semantic:

    @code
    T r = Mzero<T>();
    @endcode

    is similar to:

    @code
    T r = -T(0);
    @endcode

    @ par Note:

    This is a special constant as it can be used and considered
    identical to zero, except that for floating point numbers,
    there is two different representation of zero with different bit of sign.

    The existence of the sign can be used in special circumstances as
    choosing between \f$+\infty\f$ and \f$-\infty\f$ instead of nan in computing 1/0.
    \par

    The sign of zero can be accessed through the @c is_negative
    and @c is_positive predicates or the @funcref{boost::simd::bitofsign} function.
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Mzero, Mzero)
} }

#include <boost/simd/constant/common.hpp>

#endif


/*--------------------------------------------------------------------------*\
 |                                                                          |
 |  Copyright (C) 2017                                                      |
 |                                                                          |
 |         , __                 , __                                        |
 |        /|/  \               /|/  \                                       |
 |         | __/ _   ,_         | __/ _   ,_                                |
 |         |   \|/  /  |  |   | |   \|/  /  |  |   |                        |
 |         |(__/|__/   |_/ \_/|/|(__/|__/   |_/ \_/|/                       |
 |                           /|                   /|                        |
 |                           \|                   \|                        |
 |                                                                          |
 |      Enrico Bertolazzi                                                   |
 |      Dipartimento di Ingegneria Industriale                              |
 |      Universita` degli Studi di Trento                                   |
 |      email: enrico.bertolazzi@unitn.it                                   |
 |                                                                          |
\*--------------------------------------------------------------------------*/

///
/// file: Utils_eigen.hh
///
#pragma once

#ifndef DOXYGEN_SHOULD_SKIP_THIS

#ifndef UTILS_EIGEN_dot_HH
#define UTILS_EIGEN_dot_HH

#include "Utils.hh"

#define EIGEN_DONT_PARALLELIZE
#define EIGEN_NO_AUTOMATIC_RESIZING

#include <Eigen/Core>

#endif

#endif

///
/// eof: Utils_eigen.hh
///


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

#include <boost/contract.hpp>
#include <vector>
#include <algorithm>
#include <iostream>
#include <cassert>

//[assertion_level_no_impl
// If valid iterator range (cannot implement in C++ but OK to use in AXIOM).
template<typename Iter>
bool valid(Iter first, Iter last); // Only declared, not actually defined.
//]

//[assertion_level_class_begin
template<typename T>
class vector {
//]

public:
    typedef typename std::vector<T>::iterator iterator;

    // Could program class invariants and contracts for the following.
    iterator begin() { return vect_.begin(); }
    iterator end() { return vect_.end(); }
    unsigned capacity() const { return vect_.capacity(); }
    bool operator==(vector const& other) { return vect_ == other.vect_; }

//[assertion_level_axiom
public:
    iterator insert(iterator where, T const& value) {
        iterator result;
        boost::contract::old_ptr<unsigned> old_capacity =
                BOOST_CONTRACT_OLDOF(capacity());
        boost::contract::check c = boost::contract::public_function(this)
            .postcondition([&] {
                BOOST_CONTRACT_ASSERT(capacity() >= *old_capacity);
                if(capacity() > *old_capacity) {
                    BOOST_CONTRACT_ASSERT_AXIOM(!valid(begin(), end()));
                } else {
                    BOOST_CONTRACT_ASSERT_AXIOM(!valid(where, end()));
                }
            })
        ;

        return result = vect_.insert(where, value);
    }
//]

//[assertion_level_audit_old
public:
    void swap(vector& other) {
        boost::contract::old_ptr<vector> old_me, old_other;
        #ifdef BOOST_CONTRACT_AUDITS
            old_me = BOOST_CONTRACT_OLDOF(*this);
            old_other = BOOST_CONTRACT_OLDOF(other);
        #endif // Else, skip old value copies...
        boost::contract::check c = boost::contract::public_function(this)
            .postcondition([&] {
                // ...and also skip related assertions.
                BOOST_CONTRACT_ASSERT_AUDIT(*this == *old_other);
                BOOST_CONTRACT_ASSERT_AUDIT(other == *old_me);
            })
        ;

        vect_.swap(other.vect_);
    }
//]

//[assertion_level_class_end
    /* ... */

private:
    std::vector<T> vect_;
};
//]

//[assertion_level_audit
template<typename RandomIter, typename T>
RandomIter random_binary_search(RandomIter first, RandomIter last,
        T const& value) {
    RandomIter result;
    boost::contract::check c = boost::contract::function()
        .precondition([&] {
            BOOST_CONTRACT_ASSERT(first <= last); // Default, not expensive.
            // Expensive O(n) assertion (use AXIOM if prohibitive instead).
            BOOST_CONTRACT_ASSERT_AUDIT(std::is_sorted(first, last));
        })
        .postcondition([&] {
            if(result != last) BOOST_CONTRACT_ASSERT(*result == value);
        })
    ;

    /* ... */
//]

    RandomIter begin = first, end = last;
    while(begin < end) {
        RandomIter middle = begin + ((end - begin) >> 1);
        BOOST_CONTRACT_CHECK(*begin <= *middle || value < *middle ||
                *middle < value);

        if(value < *middle) end = middle;
        else if(value > *middle) begin = middle + 1;
        else return result = middle;
    }
    return result = last;
}

int main() {
    vector<char> v;
    v.insert(v.begin() + 0, 'a');
    v.insert(v.begin() + 1, 'b');
    v.insert(v.begin() + 2, 'c');

    vector<char>::iterator i = random_binary_search(v.begin(), v.end(), 'b');
    assert(i != v.end());
    assert(*i == 'b');

    vector<char> w;
    w.insert(w.begin() + 0, 'x');
    w.insert(w.begin() + 1, 'y');

    w.swap(v);
    assert(*(w.begin() + 0) == 'a');
    assert(*(w.begin() + 1) == 'b');
    assert(*(w.begin() + 2) == 'c');

    return 0;
}


#include "Astronomy.h"
#include "AstronomyHelperFunctions.h"
#include "Exception.h"
#include <boost/date_time/c_local_time_adjustor.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/local_time_adjustor.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>
#include <cmath>
#include <limits>
#include <vector>

/*=== Public interface =====================*/

namespace Fmi
{
namespace Astronomy
{
namespace
{
std::vector<double> quad(double ym, double yz, double yp)
{
  try
  {
    double nz(0.0);
    double z1(0.0);
    double z2(0.0);
    double a = 0.5 * (ym + yp) - yz;
    double b = 0.5 * (yp - ym);
    double c = yz;
    double xe = -b / (2 * a);
    double ye = (a * xe + b) * xe + c;
    double dis = b * b - 4 * a * c;
    if (dis > 0)
    {
      double dx = 0.5 * sqrt(dis) / fabs(a);
      z1 = xe - dx;
      z2 = xe + dx;
      nz = fabs(z1) < 1 ? nz + 1 : nz;
      nz = fabs(z2) < 1 ? nz + 1 : nz;
      z1 = z1 < -1 ? z2 : z1;
    }

    std::vector<double> ret;
    ret.push_back(nz);
    ret.push_back(z1);
    ret.push_back(z2);
    ret.push_back(xe);
    ret.push_back(ye);

    return ret;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/**
 *      returns an angle in degrees in the range 0 to 360
 */
double degRange(double x)
{
  try
  {
    double b = x / 360;
    double a = 360 * (b - static_cast<int>(b));
    double retVal = (a < 0 ? a + 360 : a);
    return retVal;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double lmst(double mjd, double glon)
{
  try
  {
    double d = mjd - 51544.5;
    double t = d / 36525;
    double lst =
        degRange(280.46061839 + 360.98564736629 * d + 0.000387933 * t * t - t * t * t / 38710000);
    return lst / 15 + glon / 15;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/**
 *      returns the self::fractional part of x
 */
double frac(double x)
{
  try
  {
    x -= static_cast<int>(x);
    return x < 0 ? x + 1 : x;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/**
 * takes t and returns the geocentric ra and dec in an array mooneq
 * claimed good to 5' (angle) in ra and 1' in dec
 * tallies with another approximate method and with ICE for a couple of dates
 */
std::vector<double> minimoon(double t)
{
  try
  {
    double p2 = 6.283185307;
    double arc = 206264.8062;
    double coseps = 0.91748;
    double sineps = 0.39778;

    double lo = frac(0.606433 + 1336.855225 * t);
    double l = p2 * frac(0.374897 + 1325.552410 * t);
    double l2 = l * 2;
    double ls = p2 * frac(0.993133 + 99.997361 * t);
    double d = p2 * frac(0.827361 + 1236.853086 * t);
    double d2 = d * 2;
    double f = p2 * frac(0.259086 + 1342.227825 * t);
    double f2 = f * 2;

    double sinls = sin(ls);
    double sinf2 = sin(f2);

    double dl = 22640 * sin(l);
    dl += -4586 * sin(l - d2);
    dl += 2370 * sin(d2);
    dl += 769 * sin(l2);
    dl += -668 * sinls;
    dl += -412 * sinf2;
    dl += -212 * sin(l2 - d2);
    dl += -206 * sin(l + ls - d2);
    dl += 192 * sin(l + d2);
    dl += -165 * sin(ls - d2);
    dl += -125 * sin(d);
    dl += -110 * sin(l + ls);
    dl += 148 * sin(l - ls);
    dl += -55 * sin(f2 - d2);

    double s = f + (dl + 412 * sinf2 + 541 * sinls) / arc;
    double h = f - d2;
    double n = -526 * sin(h);
    n += 44 * sin(l + h);
    n += -31 * sin(-l + h);
    n += -23 * sin(ls + h);
    n += 11 * sin(-ls + h);
    n += -25 * sin(-l2 + f);
    n += 21 * sin(-l + f);

    double L_moon = p2 * frac(lo + dl / 1296000);
    double B_moon = (18520.0 * sin(s) + n) / arc;

    double cb = cos(B_moon);
    double x = cb * cos(L_moon);
    double v = cb * sin(L_moon);
    double w = sin(B_moon);
    double y = coseps * v - sineps * w;
    double z = sineps * v + coseps * w;
    double rho = sqrt(1 - z * z);
    double dec = (360 / p2) * atan(z / rho);
    double ra = (48 / p2) * atan(y / (x + rho));
    ra = ra < 0 ? ra + 24 : ra;

    std::vector<double> retval;
    retval.push_back(dec);
    retval.push_back(ra);

    return retval;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double sinAlt(double mjd, double hour, double glon, double cglat, double sglat)
{
  try
  {
    mjd += (hour / 24.0);
    double t = (mjd - 51544.5) / 36525;
    std::vector<double> objpos = minimoon(t);

    double ra = objpos[1];
    double dec = objpos[0];
    double decRad = deg2rad(dec);
    double tau = 15 * (lmst(mjd, glon) - ra);

    return sglat * sin(decRad) + cglat * cos(decRad) * cos(deg2rad(tau));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace

const boost::local_time::local_date_time& lunar_time_t::risesettime(SetAndRiseOccurence occ) const
{
  try
  {
    if (occ == FIRST_RISE)
      return moonrise;
    if (occ == SECOND_RISE)
      return moonrise2;
    if (occ == FIRST_SET)
      return moonset;

    return moonset2;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::string lunar_time_t::as_string(SetAndRiseOccurence occ) const
{
  try
  {
    std::stringstream ss;

    bool rise_occurence(occ == FIRST_RISE || occ == SECOND_RISE);

    const boost::local_time::local_date_time& occ_ldt =
        (rise_occurence ? (occ == FIRST_RISE ? moonrise : moonrise2)
                        : (occ == FIRST_SET ? moonset : moonset2));

    if (!occ_ldt.is_not_a_date_time())
    {
      ss << std::setfill('0') << std::setw(2) << occ_ldt.local_time().time_of_day().hours()
         << std::setfill('0') << std::setw(2) << occ_ldt.local_time().time_of_day().minutes();
    }

    return ss.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::string lunar_time_t::as_string_long(SetAndRiseOccurence occ) const
{
  try
  {
    std::stringstream ss;

    bool rise_occurence(occ == FIRST_RISE || occ == SECOND_RISE);

    const boost::local_time::local_date_time& occ_ldt =
        (rise_occurence ? (occ == FIRST_RISE ? moonrise : moonrise2)
                        : (occ == FIRST_SET ? moonset : moonset2));

    std::cout << occ_ldt;

    if (occ_ldt.is_not_a_date_time())
      ss << occ_ldt;
    else
      ss << occ_ldt.date() << " " << as_string(occ);

    return ss.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::ostream& operator<<(std::ostream& ostream, const lunar_time_t& lt)
{
  try
  {
    std::string risestr(lt.as_string(FIRST_RISE));
    std::string setstr(lt.as_string(FIRST_SET));

    if (risestr.empty())
    {
      if (!lt.moonset_today())
      {
        if (lt.above_hz_24h)
          risestr = "****";
        else
          risestr = "----";
      }
      else
      {
        risestr = "    ";
      }
    }
    if (setstr.empty())
    {
      if (!lt.moonrise_today())
      {
        if (lt.above_hz_24h)
          setstr = "****";
        else
          setstr = "----";
      }
      else
      {
        setstr = "    ";
      }
    }

    ostream << risestr << " " << setstr;

    return ostream;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

bool dst_on(const boost::posix_time::ptime& theTime, const boost::local_time::local_date_time& ldt)
{
  try
  {
    auto zone = ldt.zone();

    bool dst_on(false);
    if (zone->has_dst())
    {
      auto dst_starttime = zone->dst_local_start_time(ldt.local_time().date().year());
      auto dst_endtime = zone->dst_local_end_time(ldt.local_time().date().year());

      if (dst_starttime < dst_endtime)
      {
        if (theTime >= dst_starttime && theTime <= dst_endtime)
          dst_on = true;
      }
      else
      {
        if (theTime >= dst_starttime || theTime <= dst_endtime)
          dst_on = true;
      }
    }
    return dst_on;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double timezone_offset(const boost::local_time::local_date_time& ldt)
{
  try
  {
    auto zone = ldt.zone();

    double base_offset(zone->base_utc_offset().hours() +
                       (static_cast<float>(zone->base_utc_offset().minutes()) / 60.0));
    double dst_offset(dst_on(ldt.local_time(), ldt)
                          ? (zone->dst_offset().hours() + zone->dst_offset().minutes())
                          : 0.0);

    return (base_offset + dst_offset);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void get_hours_and_minutes(double hours, int& hr, int& min)
{
  try
  {
    double hrs(hours);

    // if time is greater than 23:59:29 use value 23:59, so that we do not move to 00:00 (backwards)
    if (hrs > 23.991 && hrs < 24.0)
      hrs = 23.991;

    hr = static_cast<int>(floor(hrs));
    min = static_cast<int>(round(60.0 * (hrs - hr)));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

boost::local_time::local_date_time parse_local_date_time(
    const boost::local_time::local_date_time& ldt, double hours)
{
  try
  {
    double hrs(hours);
    int hour(0);
    int min(0);

    get_hours_and_minutes(hrs, hour, min);
    // utc time of the beginning of the day
    auto utc_ptime = ldt.utc_time();
    // add hour offset
    utc_ptime += boost::posix_time::time_duration(hour, min, 0, 0);

    // return local time
    boost::local_time::local_date_time ldt_riseset(utc_ptime, ldt.zone());

    return ldt_riseset;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Calculate moonrise, moonset for given location
 */
lunar_time_t lunar_time_calculation(const boost::local_time::local_date_time& ldt,
                                    double offset,
                                    double lon,
                                    double lat)
{
  try
  {
    double utrise(0.0);
    double utset(0.0);
    double utrise2(0.0);
    double utset2(0.0);

    // beginning of the day
    boost::local_time::local_date_time ldt_beg(
        ldt.local_time().date(),
        boost::posix_time::time_duration(0, 0, 0, 0),
        ldt.zone(),
        boost::local_time::local_date_time::NOT_DATE_TIME_ON_ERROR);

    double date = ldt_beg.local_time().date().modjulian_day();

    date -= (static_cast<float>(offset) / 24.0);

    double latRad = deg2rad(lat);
    double sinho = 0.0023271056;
    double sglat = sin(latRad);
    double cglat = cos(latRad);

    bool rise = false;
    bool set = false;
    bool rise2 = false;
    bool set2 = false;
    bool above = false;
    double hour = 1.0;

    double ym = sinAlt(date, hour - 1.0, lon, cglat, sglat) - sinho;

    above = (ym > 0);
    while (hour < 25)
    {
      double yz = sinAlt(date, hour, lon, cglat, sglat) - sinho;
      double yp = sinAlt(date, hour + 1.0, lon, cglat, sglat) - sinho;

      std::vector<double> quadout = quad(ym, yz, yp);
      double nz = quadout[0];
      double z1 = quadout[1];
      double z2 = quadout[2];
      // double xe = quadout[3];
      double ye = quadout[4];

      if (nz == 1)
      {
        if (ym < 0)
        {
          if (!set || !rise)
          {
            utrise = hour + z1;
            rise = true;
          }
          else
          {
            utrise2 = hour + z1;
            rise2 = true;
          }
        }
        else
        {
          if (!set || !rise)
          {
            utset = hour + z1;
            set = true;
          }
          else
          {
            utset2 = hour + z1;
            set2 = true;
          }
        }
      }

      if (nz == 2)
      {
        if (ye < 0)
        {
          if (!set || !rise)
          {
            utrise = hour + z2;
            utset = hour + z1;
          }
          else
          {
            utrise2 = hour + z2;
            utset2 = hour + z1;
          }
        }
        else
        {
          if (!set || !rise)
          {
            utrise = hour + z1;
            utset = hour + z2;
          }
          else
          {
            utrise2 = hour + z1;
            utset2 = hour + z2;
          }
        }
      }
#ifdef MYDEBUG
      std::cout << "nz: " << nz << endl;
      std::cout << "z1: " << z1 << endl;
      std::cout << "z2: " << z2 << endl;
      // std::cout << "xe: " << xe << endl;
      std::cout << "ye: " << ye << endl;
      std::cout << "utrise: " << utrise << endl;
      std::cout << "utset: " << utset << endl;
      std::cout << "utrise2: " << utrise2 << endl;
      std::cout << "utset2: " << utset2 << endl;
      std::cout << "rise: " << rise << endl;
      std::cout << "rise2: " << rise2 << endl;
      std::cout << "set: " << set << endl;
      std::cout << "set2: " << set2 << endl << endl;
#endif

      ym = yp;
      hour += 2.0;
    }

#ifdef MYDEBUG
    cout << "\noffset: " << offset << endl;
    cout << "rise: " << rise << endl;
    cout << "rise2: " << rise2 << endl;
    cout << "utrise: " << utrise << endl;
    cout << "utrise2: " << utrise2 << endl;
    cout << "set: " << set << endl;
    cout << "set2: " << set2 << endl;
    cout << "utset: " << utset << endl;
    cout << "utset2: " << utset2 << endl;
    cout << "julian: " << date << endl;
    cout << "ldt: " << ldt << endl;
    cout << "ldt.local_time(): " << ldt.local_time() << endl;
    cout << "ldt.utc_time(): " << ldt.utc_time() << endl;
    cout << "ldt_beg: " << ldt_beg << endl;
#endif

    lunar_time_t retval(
        (rise ? parse_local_date_time(ldt_beg, utrise)
              : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        (set ? parse_local_date_time(ldt_beg, utset)
             : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        (rise2 ? parse_local_date_time(ldt_beg, utrise2)
               : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        (set2 ? parse_local_date_time(ldt_beg, utset2)
              : boost::local_time::local_date_time(boost::posix_time::not_a_date_time)),
        rise,
        set,
        rise2,
        set2,
        (!rise && !set && above));

    return retval;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

lunar_time_t lunar_time_i(const boost::local_time::local_date_time& ldt, double lon, double lat)
{
  try
  {
    // beginning of the day
    boost::local_time::local_date_time ldt_beg(
        ldt.local_time().date(),
        boost::posix_time::time_duration(0, 0, 0, 0),
        ldt.zone(),
        boost::local_time::local_date_time::NOT_DATE_TIME_ON_ERROR);
    boost::local_time::local_date_time ldt_end(
        ldt.local_time().date(),
        boost::posix_time::time_duration(23, 59, 59, 0),
        ldt.zone(),
        boost::local_time::local_date_time::NOT_DATE_TIME_ON_ERROR);

    boost::posix_time::ptime dst_endtime(
        ldt.zone()->dst_local_end_time(ldt.local_time().date().year()));
    bool dst_ends_today(ldt_beg.local_time().date() == dst_endtime.date());

    double offset_before_dst_ends = timezone_offset(ldt_beg);

    lunar_time_t retval;

    // if dst ends today we have to handle the extended time
    if (dst_ends_today)
    {
      double offset_after_dst_ends = timezone_offset(ldt_end);

      lunar_time_t lt_before = lunar_time_calculation(ldt_beg, offset_before_dst_ends, lon, lat);

      // if rise and set is found return
      if (lt_before.moonrise_today() && lt_before.moonset_today())
        return lt_before;

      // do calculation using wintertime offset
      lunar_time_t lt_after = lunar_time_calculation(ldt_beg, offset_after_dst_ends, lon, lat);
      if (lt_after.moonrise_today())
        lt_after.moonrise += boost::posix_time::hours(1);
      if (lt_after.moonset_today())
        lt_after.moonset += boost::posix_time::hours(1);
      if (lt_after.moonrise2_today())
        lt_after.moonrise2 += boost::posix_time::hours(1);
      if (lt_after.moonset2_today())
        lt_after.moonset2 += boost::posix_time::hours(1);

      lunar_time_t lt_combined(
          lt_before.moonrise_today() ? lt_before.moonrise : lt_after.moonrise,
          lt_before.moonset_today() ? lt_before.moonset : lt_after.moonset,
          lt_before.moonrise2_today() ? lt_before.moonrise2 : lt_after.moonrise2,
          lt_before.moonset2_today() ? lt_before.moonset2 : lt_after.moonset2,
          lt_before.rise_today,
          lt_before.set_today,
          lt_before.rise2_today,
          lt_before.set2_today,
          lt_before.above_hz_24h && lt_after.above_hz_24h);

      retval = lt_combined;
    }
    else
    {
      retval = lunar_time_calculation(ldt_beg, offset_before_dst_ends, lon, lat);
    }
    return retval;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

lunar_time_t lunar_time(const boost::local_time::local_date_time& ldt,
                        double lon,
                        double lat,
                        bool allow_missing_dates /*= false*/)
{
  try
  {
    lunar_time_t lt_ret = lunar_time_i(ldt, lon, lat);

    if (allow_missing_dates)
      return lt_ret;

    unsigned int iteration_limit(366);
    if (!lt_ret.moonrise_today())
    {
      unsigned int counter(0);  // just in case something goes wrong
      boost::local_time::local_date_time ldt_iter(ldt);
      if (lt_ret.above_hz_24h || lt_ret.moonset_today())
      {
        // find the previous moonrise
        lunar_time_t lt_prev;
        while (!lt_prev.moonrise_today() && counter < iteration_limit)
        {
          ldt_iter -= boost::posix_time::hours(24);
          lt_prev = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonrise = lt_prev.moonrise;
      }
      else
      {
        // find the next moonrise
        lunar_time_t lt_next;
        while (!lt_next.moonrise_today() && counter < iteration_limit)
        {
          ldt_iter += boost::posix_time::hours(24);
          lt_next = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonrise = lt_next.moonrise;
      }
    }
    if (!lt_ret.moonset_today())
    {
      unsigned int counter(0);  // just in case something goes wrong
      boost::local_time::local_date_time ldt_iter(ldt);
      if (lt_ret.above_hz_24h || lt_ret.moonrise_today())
      {
        // find the next moonset
        lunar_time_t lt_next;
        while (!lt_next.moonset_today() && counter < iteration_limit)
        {
          ldt_iter += boost::posix_time::hours(24);
          lt_next = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonset = lt_next.moonset;
      }
      else
      {
        // find the previous moonset
        lunar_time_t lt_prev;
        while (!lt_prev.moonset_today() && counter < iteration_limit)
        {
          ldt_iter -= boost::posix_time::hours(24);
          lt_prev = lunar_time_i(ldt_iter, lon, lat);
          counter++;
        }
        if (counter < iteration_limit)
          lt_ret.moonset = lt_prev.moonset;
      }
    }
    return lt_ret;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Astronomy
}  // namespace Fmi

// ======================================================================


#include "stupid.h"
#include <boost/range/algorithm_ext/push_back.hpp>

#include "engine.h"
#include "log.h"
#include "nmms.h"
#include "state.h"
#include "system.h"

namespace {

Trace reassembly(ProblemType problem_type, const Matrix& src_matrix, const Matrix& dst_matrix) {
    ASSERT_RETURN(problem_type == ProblemType::Reassembly, Trace());

    printf("stupid_solver\n");
    const int R = src_matrix.R;
    System system(R);
    Trace trace;

    const Vec3 start(0, 0, 0);
    const Vec3 top(0, R - 1, 0);
    const Vec3 diagonal(R - 1, 0, R - 1);

    std::vector<Vec3> scan = NEditPoints::move_naive_ii(start, top);
    boost::push_back(scan, NEditPoints::fill_zigzag_ii(top, diagonal));
    boost::push_back(scan, NEditPoints::move_naive_ii(scan.back(), start));
    NEditPoints::dedup(scan);
    ASSERT(NEditPoints::is_connected_6(scan));

    trace.push_back(CommandFlip{}); // high.
    for (size_t i = 0; i < scan.size(); ++i) {
        if (i + 1 < scan.size()) {
            if (src_matrix(scan[i + 1])) {
                trace.push_back(CommandVoid{scan[i + 1] - scan[i]});
            }
            trace.push_back(CommandSMove{scan[i + 1] - scan[i]});
        }
        // now we are at i+1.
        if (dst_matrix(scan[i])) {
            trace.push_back(CommandFill{scan[i] - scan[i + 1]});
        }
    }
    trace.push_back(CommandFlip{}); // low.
    trace.push_back(CommandHalt{});
    LOG() << "trace " << trace.size() << ", scan " << scan.size() << "\n";

    return trace;
}

Trace disassembly(ProblemType problem_type, const Matrix& src_matrix, const Matrix& dst_matrix) {
    ASSERT_RETURN(problem_type == ProblemType::Disassembly, Trace());

    LOG() << "stupid_solver\n";
    const int R = src_matrix.R;
    System system(R);
    Trace trace;

    const Vec3 start(0, 0, 0);
    const Vec3 top(0, R - 1, 0);
    const Vec3 diagonal(R - 1, 0, R - 1);
    const Vec3 down(0, -1, 0);
    std::vector<Vec3> scan = NEditPoints::move_naive_ii(start, top);
    boost::push_back(scan, NEditPoints::fill_zigzag_ii(top, diagonal));
    boost::push_back(scan, NEditPoints::move_naive_ii(scan.back(), start));
    NEditPoints::dedup(scan);
    ASSERT(NEditPoints::is_connected_6(scan));

    trace.push_back(CommandFlip{}); // high.
    for (size_t i = 0; i < scan.size(); ++i) {
        Vec3 bottom = scan[i] + down;
        if (src_matrix.is_in_matrix(bottom) && src_matrix(bottom)) {
            trace.push_back(CommandVoid{down});
        }
        if (i + 1 < scan.size()) {
            trace.push_back(CommandSMove{scan[i + 1] - scan[i]});
        }
    }
    trace.push_back(CommandFlip{}); // low.
    trace.push_back(CommandHalt{});
    LOG() << "trace " << trace.size() << ", scan " << scan.size() << "\n";

    return trace;
}

Trace assembly(ProblemType problem_type, const Matrix& src_matrix, const Matrix& dst_matrix) {
    ASSERT_RETURN(problem_type == ProblemType::Assembly, Trace());

    LOG() << "stupid_solver\n";
    System system(dst_matrix.R);

    // use a single nanobot.
    // always in the high harmonics.
    // zig-zag scanning in the XZ plane.
    // only fill the "previous" voxels.
    // go back to the home using BFS. (it is possible to lock out itself)

    Trace trace;
    trace.push_back(CommandFlip{}); // high.

    Vec3 p(system.bots[0].pos);
    Vec3 prev = p;
    while (true) {
        int zdir = p.y % 2 == 0 ? +1 : -1;
        while (0 <= p.z + zdir && p.z + zdir < system.matrix.R) {
            int xdir = p.z % 2 == 0 ? +1 : -1;
            while (0 <= p.x + xdir && p.x + xdir < system.matrix.R) {
                p.x += xdir;
                trace.push_back(CommandSMove{Vec3(xdir, 0, 0)});
                if (dst_matrix(prev)) {
                    trace.push_back(CommandFill{prev - p});
                }
                prev = p;
            }
            // turn around.
            p.z += zdir;
            trace.push_back(CommandSMove{Vec3(0, 0, zdir)});
            if (dst_matrix(prev)) {
                trace.push_back(CommandFill{prev - p});
            }
            prev = p;
        }
        if (p.y + 1 < system.matrix.R) {
            // up.
            p.y += 1;
            trace.push_back(CommandSMove{Vec3(0, 1, 0)});
            if (dst_matrix(prev)) {
                trace.push_back(CommandFill{prev - p});
            }
            prev = p;
        } else {
            break;
        }
    }

    // go home.
    std::vector<Vec3> trajectory;
    if (!bfs_shortest_in_void(dst_matrix, p, system.final_pos(),
        &trace, &trajectory)) {
        std::cout << "sorry, stupid algorithm failed.." << std::endl;
        return trace;
    }

    if (false) {
        for (auto p : trajectory) {
            p.print();
        }
    }

    // finalize at the origin pos.
    trace.push_back(CommandFlip{});
    trace.push_back(CommandHalt{});
    return trace;
}

Trace solver(ProblemType problem_type, const Matrix& src_matrix, const Matrix& dst_matrix) {
    switch (problem_type) {
        case ProblemType::Assembly:
            return assembly(problem_type, src_matrix, dst_matrix);
        case ProblemType::Disassembly:
            return disassembly(problem_type, src_matrix, dst_matrix);
        case ProblemType::Reassembly:
        default:
            return reassembly(problem_type, src_matrix, dst_matrix);
    }
    ASSERT_RETURN(false, Trace());
}

}  // anonymous namespace

REGISTER_ENGINE(stupid, solver);


#include <boost/test/unit_test.hpp>

#include "defines.hpp"

#include <scorum/protocol/asset.hpp>
#include <scorum/rewards_math/formulas.hpp>

using namespace scorum::rewards_math;
using namespace scorum::protocol;

using scorum::protocol::curve_id;

namespace database_fixture {
struct rewards_math_calculate_curations_payout_fixture
{
    const share_type payout = ASSET_SCR(100e+3).amount;
    const uint64_t weight = SCORUM_PERCENT(10);
    const uint64_t total_weight = weight * 2;
};
}

using namespace database_fixture;

BOOST_FIXTURE_TEST_SUITE(rewards_math_calculate_curations_payout_tests, rewards_math_calculate_curations_payout_fixture)

BOOST_AUTO_TEST_CASE(calculate_curations_payout_check)
{
    BOOST_CHECK_GT(calc_curations_payout(payout, SCORUM_CURATION_REWARD_PERCENT), share_type());
    BOOST_CHECK_LT(calc_curations_payout(payout, SCORUM_CURATION_REWARD_PERCENT), payout);
}

BOOST_AUTO_TEST_CASE(calculate_curation_payout_check)
{
    BOOST_CHECK_GT(calculate_curation_payout(payout, total_weight, weight), share_type(0));
    BOOST_CHECK_LT(calculate_curation_payout(payout, total_weight, weight), payout);
}

BOOST_AUTO_TEST_SUITE_END()


// original source:https://github.com/AgileDrones/OptiTrack-Motive-2-Client
// by Winter Guerra
// winterg@mit.edu
//https://winter.industries/

// Include motion capture framework
#include "optitrack_motive_2_client/motionCaptureClientFramework.h"
// Include ACL message types (https://bitbucket.org/brettlopez/acl_msgs.git)
#include "acl_msgs/ViconState.h"

// Includes for ROS
#include "ros/ros.h"

// Includes for node
#include <boost/program_options.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>

namespace po = boost::program_options;
using namespace Eigen;
const double pi  = 3.1415926535897932384626433832795028841971;

// Used to convert mocap frame (NUE) to LCM NED.
// static Eigen::Matrix3d R_NUE2NED = [] {
//     Eigen::Matrix3d tmp;
//     tmp <<  1, 0, 0,
//             0, 0, 1,
//             0, -1, 0;
//     return tmp;
// }();

// Used to convert mocap frame (NUE) to ROS ENU.
static Eigen::Matrix3d R_NUE2ENU = [] {
    Eigen::Matrix3d tmp;
    tmp <<  0, 0, 1,
            1, 0, 0,
            0, 1, 0;
    return tmp;
}();

Vector3d positionConvertNUE2ENU(double* positionNUE){
  Vector3d positionNUEVector, positionENUVector;
  positionNUEVector << positionNUE[0], positionNUE[1], positionNUE[2];

  positionENUVector = R_NUE2ENU * positionNUEVector;
  return positionENUVector;
}

Quaterniond quaternionConvertNUE2ENU(double* quaternionNUE){
    Quaterniond quaternionInNUE;
    quaternionInNUE.x() = quaternionNUE[0];
    quaternionInNUE.y() = quaternionNUE[1];
    quaternionInNUE.z() = quaternionNUE[2];
    quaternionInNUE.w() = quaternionNUE[3];

    Quaterniond quaternionInENU = Quaterniond(R_NUE2ENU * quaternionInNUE.normalized().toRotationMatrix()
                              * R_NUE2ENU.transpose());
    return quaternionInENU;
}

int main(int argc, char *argv[])
{
  // Keep track of ntime offset.
  int64_t offset_between_windows_and_linux = std::numeric_limits<int64_t>::max();
  ROS_INFO("1");
  // Init ROS
  ros::init(argc, argv, "optitrack_motive_2_client_node");
  ros::NodeHandle n;

  std::string szMyIPAddress;
  std::string szServerIPAddress;

  // Get CMDline arguments for server and local IP addresses.

 //自己加的，方便调整IP read ip from launch, added by Peter Li
  n.getParam(
      "/optitrack_motive_2_client_node/MyIPAddress",
      szMyIPAddress); //因为这个参数不是server那边发送的，是这个节点私有的，因此要加范围
  ROS_INFO("MyIPAddress is %s", szMyIPAddress.c_str());

  n.getParam(
      "/optitrack_motive_2_client_node/ServerIPAddress",
      szServerIPAddress); //因为这个参数不是server那边发送的，是这个节点私有的，因此要加范围
  ROS_INFO("ServerIPAddress is %s", szServerIPAddress.c_str());

/*
// test for direct input here，added by Peter Li
szMyIPAddress = "192.168.50.170";
szServerIPAddress = "192.168.50.171";
*/

  try {
    po::options_description desc ("Options");
    desc.add_options()
        ("help,h", "print usage message")
        ("local",po::value<std::string>(&szMyIPAddress),"local IP Address")
        ("server",po::value<std::string>(&szServerIPAddress), "server address");

    po::variables_map vm;
    try {
      po::store(po::parse_command_line(argc, argv, desc), vm);
      po::notify (vm);
    }
    catch (po::error& e) {
      std::cerr << e.what() << std::endl;
      return 0;
    }
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
  }
  catch (...) {}

  // Init mocap framework
  agile::motionCaptureClientFramework mocap_ = agile::motionCaptureClientFramework(szMyIPAddress, szServerIPAddress);

  // Some vars to calculate twist/acceleration and dts
  // Also keeps track of the various publishers
  std::map<int, ros::Publisher> rosPublishers;
  std::map<int, acl_msgs::ViconState> pastStateMessages;

  while (true){
    // Wait for mocap packet
    mocap_.spin();

    std::vector<agile::Packet> mocap_packets = mocap_.getPackets();

    for (agile::Packet mocap_packet : mocap_packets){

      // @TODO: Make getPackets return a list.

      // Skip this rigid body if tracking is invalid
      if (!mocap_packet.tracking_valid)
        continue;

      // estimate the windows to linux constant offset by taking the minimum seen offset.
      // @TODO: Make offset a rolling average instead of a latching offset.
      int64_t offset = mocap_packet.transmit_timestamp - mocap_packet.receive_timestamp;
      if (offset < offset_between_windows_and_linux ){
        offset_between_windows_and_linux = offset;
      }
      uint64_t packet_ntime = mocap_packet.mid_exposure_timestamp - offset_between_windows_and_linux;

      // Get past state and publisher (if they exist)
      bool hasPreviousMessage = (rosPublishers.find(mocap_packet.rigid_body_id) != rosPublishers.end());
      ros::Publisher publisher;
      acl_msgs::ViconState lastState;
      acl_msgs::ViconState currentState;

      // Initialize publisher for rigid body if not exist.
      if (!hasPreviousMessage){
        std::string topic = "/" + mocap_packet.model_name + "/vicon";

        publisher = n.advertise<acl_msgs::ViconState>(topic, 1);
        rosPublishers[mocap_packet.rigid_body_id] = publisher;
      } else {
        // Get saved publisher and last state
        publisher = rosPublishers[mocap_packet.rigid_body_id];
        lastState = pastStateMessages[mocap_packet.rigid_body_id];
      }

      // Add timestamp
      currentState.header.stamp = ros::Time(packet_ntime/1e9, packet_ntime%(int64_t)1e9);

      // Convert rigid body position from NUE to ROS ENU
      Vector3d positionENUVector = positionConvertNUE2ENU(mocap_packet.pos);
      currentState.pose.position.x = positionENUVector(0);
      currentState.pose.position.y = positionENUVector(1);
      currentState.pose.position.z = positionENUVector(2);
      // Convert rigid body rotation from NUE to ROS ENU
      Quaterniond quaternionENUVector = quaternionConvertNUE2ENU(mocap_packet.orientation);
      currentState.pose.orientation.x = quaternionENUVector.x();
      currentState.pose.orientation.y = quaternionENUVector.y();
      currentState.pose.orientation.z = quaternionENUVector.z();
      currentState.pose.orientation.w = quaternionENUVector.w();
     /***************************************************/
      //加上欧拉角 convert Quaterniond to Eulers, added by Peter Li
      //四元数->旋转矩阵->欧拉角 Quaterniond->RotationMatrix->Eulers
      ////!!!!!Be aware you can convert rad to degree here， which is convenient for comparing with Mtoive panels.
      ///Custom for your need.注意此处是 rad。
      Matrix3d RotationMatrix_cur = quaternionENUVector.toRotationMatrix();
      Vector3d Eulers_cur = RotationMatrix_cur.eulerAngles(2,1,0);
      currentState.Eulers.z = Eulers_cur(0);//yaw(why only within [0,pi] why?)
      currentState.Eulers.y = Eulers_cur(1);//pitch (-pi,pi]
      currentState.Eulers.x = Eulers_cur(2);//roll (-pi,pi]
      /***************************************************/
      currentState.has_pose = true;

      // Loop through markers and convert positions from NUE to ENU
      // @TODO since the state message does not understand marker locations.

      if (hasPreviousMessage){
        int64_t dt_nsec = packet_ntime - (lastState.header.stamp.sec*1e9 + lastState.header.stamp.nsec);

       // printf("time interval = %ld",dt_nsec);

        // Calculate twist. Requires last state message.
        currentState.twist.linear.x = (currentState.pose.position.x - lastState.pose.position.x)/(dt_nsec / 1e9);
        currentState.twist.linear.y = (currentState.pose.position.y - lastState.pose.position.y)/(dt_nsec / 1e9);
        currentState.twist.linear.z = (currentState.pose.position.z - lastState.pose.position.z)/(dt_nsec / 1e9);

        // @TODO: not sure how to calculate the angular twist. Is it in local frame or global frame?
        //计算角速度 calculate the derivation of Eulers in global frame, i.e. angular velocity, added by Peter Li

        currentState.twist.angular.x = (currentState.Eulers.x - lastState.Eulers.x)/(dt_nsec / 1e9);//roll
        currentState.twist.angular.y = (currentState.Eulers.y - lastState.Eulers.y)/(dt_nsec / 1e9);//pitch
        currentState.twist.angular.z = (currentState.Eulers.z - lastState.Eulers.z)/(dt_nsec / 1e9);//yaw
        currentState.has_twist = true;

        // Calculate accelerations of both positon and Eulers. Requires last state message.
        currentState.pos_accel.x = (currentState.twist.linear.x - lastState.twist.linear.x)/(dt_nsec / 1e9);
        currentState.pos_accel.y = (currentState.twist.linear.y - lastState.twist.linear.y)/(dt_nsec / 1e9);
        currentState.pos_accel.z = (currentState.twist.linear.z - lastState.twist.linear.z)/(dt_nsec / 1e9);
        currentState.Eulers_accel.x = (currentState.twist.angular.x - lastState.twist.angular.x)/(dt_nsec / 1e9);
        currentState.Eulers_accel.y = (currentState.twist.angular.y - lastState.twist.angular.y)/(dt_nsec / 1e9);
        currentState.Eulers_accel.z = (currentState.twist.angular.z - lastState.twist.angular.z)/(dt_nsec / 1e9);

        currentState.has_accel = true;
      }

      // Save state for future acceleration and twist computations
      pastStateMessages[mocap_packet.rigid_body_id] = currentState;

      // Publish ROS state.
      publisher.publish(currentState);

    }
  }
}