// 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 "bempp/common/config_ahmed.hpp"

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

#include "assembly/context.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/identity_operator.hpp"
#include "assembly/laplace_3d_adjoint_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_hypersingular_boundary_operator.hpp"
#include "assembly/laplace_3d_single_layer_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"
#include "grid/grid_factory.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>
#include "grid/grid.hpp"

using namespace Bempp;

// Tests

namespace
{

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_V_plus_K
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
            context, domain, range, dualToRange) +
                laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_V_plus_3_K
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                3. * laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_3_V_plus_K
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return 3. * laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_3_V_plus_4_K
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return 3. * laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                4. * laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_V_plus_K_plus_Kp
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dAdjointDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_3_V_plus_K_plus_Kp
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return 3. * laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dAdjointDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_V_plus_3_K_plus_Kp
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                3. * laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dAdjointDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_V
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_3_V
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return 3. * laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_I
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_3_I
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return 3. * identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_I_plus_V
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_V_plus_I
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                identityOperator<BasisFunctionType, ResultType>(
                                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_3_I_plus_V
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return 3. * identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_I_plus_3_V
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                3. * laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_2_I_plus_V_plus_I_plus_4_K
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return 2. * identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange) +
                2. * laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange);
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_K_plus_adj_Kp
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange, "DLP") +
                adjoint(laplace3dAdjointDoubleLayerBoundaryOperator<
                        BasisFunctionType, ResultType>(
                            context, dualToRange, dualToRange /* hack */, domain, "ADLP"));
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_K_plus_I_plus_4_adj_Kp
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange, "DLP") +
                identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange, "Id") +
                4. * adjoint(laplace3dAdjointDoubleLayerBoundaryOperator<
                        BasisFunctionType, ResultType>(
                            context, dualToRange, dualToRange /* hack */, domain, "ADLP"));
    }
};

template <typename BasisFunctionType, typename ResultType>
struct CreateOperator_K_plus_I_plus_5_V_plus_4_adj_Kp
{
    BoundaryOperator<BasisFunctionType, ResultType> operator() (
            const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
            const shared_ptr<const Space<BasisFunctionType> >& domain,
            const shared_ptr<const Space<BasisFunctionType> >& range,
            const shared_ptr<const Space<BasisFunctionType> >& dualToRange) const
    {
        return laplace3dDoubleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange, "DLP") +
                identityOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange, "Id") +
                5. * laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
                    context, domain, range, dualToRange, "SLP") +
                4. * adjoint(laplace3dAdjointDoubleLayerBoundaryOperator<
                        BasisFunctionType, ResultType>(
                            context, dualToRange, dualToRange /* hack */, domain, "ADLP"));
    }
};

template <typename BasisFunctionType, typename ResultType,
          template<typename BFT, typename RT> class CreateOperatorFunctor>
struct TestInDenseMode
{
    void test()
    {
        typedef ResultType RT;
        typedef typename ScalarTraits<ResultType>::RealType RealType;
        typedef BasisFunctionType BFT;

        CreateOperatorFunctor<BFT, RT> createOperator;

        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));

        AccuracyOptions accuracyOptions;
        accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
        shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                    new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

        AssemblyOptions assemblyOptionsRef;
        assemblyOptionsRef.setVerbosityLevel(VerbosityLevel::LOW);
        shared_ptr<Context<BFT, RT> > contextRef(
                    new Context<BFT, RT>(quadStrategy, assemblyOptionsRef));

        BoundaryOperator<BFT, RT> opRef = createOperator(
                    contextRef, pwiseLinears, pwiseLinears, pwiseConstants);
        arma::Mat<RT> weakFormRef = opRef.weakForm()->asMatrix();

        AssemblyOptions assemblyOptionsTest;
        assemblyOptionsTest.setVerbosityLevel(VerbosityLevel::LOW);
        assemblyOptionsTest.enableJointAssembly();
        shared_ptr<Context<BFT, RT> > contextTest(
                    new Context<BFT, RT>(quadStrategy, assemblyOptionsTest));

        BoundaryOperator<BFT, RT> opTest = createOperator(
                    contextTest, pwiseLinears, pwiseLinears, pwiseConstants);
        arma::Mat<RT> weakFormTest = opTest.weakForm()->asMatrix();

        BOOST_CHECK(check_arrays_are_close<RT>(
                        weakFormTest, weakFormRef,
                        10. * std::numeric_limits<RealType>::epsilon()));
    }
};

template <typename BasisFunctionType, typename ResultType,
          template<typename BFT, typename RT> class CreateOperatorFunctor>
struct TestInAcaMode
{
    void test()
    {
        typedef ResultType RT;
        typedef typename ScalarTraits<ResultType>::RealType RealType;
        typedef BasisFunctionType BFT;

        CreateOperatorFunctor<BFT, RT> createOperator;

        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));

        AccuracyOptions accuracyOptions;
        accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
        shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                    new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

        AcaOptions acaOptions;
        acaOptions.minimumBlockSize = 2;

        AssemblyOptions assemblyOptionsRef;
        assemblyOptionsRef.setVerbosityLevel(VerbosityLevel::LOW);
        assemblyOptionsRef.switchToAcaMode(acaOptions);
        assemblyOptionsRef.setMaxThreadCount(1);
        shared_ptr<Context<BFT, RT> > contextRef(
                    new Context<BFT, RT>(quadStrategy, assemblyOptionsRef));

        std::cout << "Assembling ref op.\n";
        BoundaryOperator<BFT, RT> opRef = createOperator(
                    contextRef, pwiseLinears, pwiseLinears, pwiseConstants);
        arma::Mat<RT> weakFormRef = opRef.weakForm()->asMatrix();

        AssemblyOptions assemblyOptionsTest;
        //assemblyOptionsTest.setVerbosityLevel(VerbosityLevel::LOW);
        assemblyOptionsTest.enableJointAssembly();
        assemblyOptionsTest.switchToAcaMode(acaOptions);
        assemblyOptionsTest.setMaxThreadCount(1);
        shared_ptr<Context<BFT, RT> > contextTest(
                    new Context<BFT, RT>(quadStrategy, assemblyOptionsTest));

        std::cout << "Assembling test op.\n";
        BoundaryOperator<BFT, RT> opTest = createOperator(
                    contextTest, pwiseLinears, pwiseLinears, pwiseConstants);
        arma::Mat<RT> weakFormTest = opTest.weakForm()->asMatrix();

        BOOST_CHECK(check_arrays_are_close<RT>(
                        weakFormTest, weakFormRef, 2. * acaOptions.eps));
    }
};

} // namespace

BOOST_AUTO_TEST_SUITE(JointAssembly)

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_V_plus_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_V_plus_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_3_V_plus_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_3_V_plus_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_V_plus_3_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_V_plus_3_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_3_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_3_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_V_plus_K_plus_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_V_plus_K_plus_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_3_V_plus_K_plus_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_3_V_plus_K_plus_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_V_plus_3_K_plus_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_V_plus_3_K_plus_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_I,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_I> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_3_I,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_3_I> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_V_plus_I,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_V_plus_I> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_3_I_plus_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_3_I_plus_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_I_plus_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_I_plus_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_I_plus_3_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_I_plus_3_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_2_I_plus_V_plus_I_plus_4_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_2_I_plus_V_plus_I_plus_4_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_K_plus_I_plus_4_adj_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_K_plus_I_plus_4_adj_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_K_plus_I_plus_5_V_plus_4_adj_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_K_plus_I_plus_5_V_plus_4_adj_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_dense_mode_for_K_plus_adj_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInDenseMode<BFT, RT, CreateOperator_K_plus_adj_Kp> test;
    test.test();
}

#ifdef WITH_AHMED

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_V_plus_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_V_plus_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_3_V_plus_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_3_V_plus_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_V_plus_3_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_V_plus_3_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_3_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_3_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_V_plus_K_plus_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_V_plus_K_plus_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_3_V_plus_K_plus_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_3_V_plus_K_plus_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_V_plus_3_K_plus_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_V_plus_3_K_plus_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_I,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_I> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_3_I,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_3_I> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_V_plus_I,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_V_plus_I> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_3_I_plus_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_3_I_plus_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_I_plus_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_I_plus_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_I_plus_3_V,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_I_plus_3_V> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_2_I_plus_V_plus_I_plus_4_K,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_2_I_plus_V_plus_I_plus_4_K> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_K_plus_adj_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_K_plus_adj_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_K_plus_I_plus_4_adj_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_K_plus_I_plus_4_adj_Kp> test;
    test.test();
}

BOOST_AUTO_TEST_CASE_TEMPLATE(joint_assembly_works_in_aca_mode_for_K_plus_I_plus_5_V_plus_4_adj_Kp,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    TestInAcaMode<BFT, RT, CreateOperator_K_plus_I_plus_5_V_plus_4_adj_Kp> test;
    test.test();
}

#endif // WITH_AHMED

BOOST_AUTO_TEST_SUITE_END()


/*****************************************************************************
*
* Functions for calculating the gap
*
* Copyright (C) 2018 by Hidemaro Suwa
* e-mail:suwamaro@phys.s.u-tokyo.ac.jp
*
*****************************************************************************/

// #include <functional>
#include <armadillo>
#include <cuba.h>
#include "calc_gap.h"
#include "rpa_util.h"
#include "calc_intensity.h"

/* Index: NCOMP = (ope * nelem + elem) | (real=0 or imag=1) */
std::tuple<int, int> comp_to_ope_elem(int comp, int nelem){
  comp >>= 1;
  int ope = comp / nelem;
  int elem = comp % nelem;
  return std::make_tuple(ope, elem);
}

/* Member functions of ResponseFuncIntegrand */
void ResponseFuncIntegrand::update_parameters(double _T, double _delta, double _mu, cx_double _omega, Polarization const& _Pz){
  T_ = _T;
  delta_ = _delta;
  mu_ = _mu;
  omega_ = _omega;
  Pz_ = _Pz;  // without copying the tables
  Pz_.is_table_set_ = false;
};

/* Member functions of ResponseFuncIntegrandBilayer */
void ResponseFuncIntegrandBilayer::set_parameters(hoppings_bilayer2 const& h, double T, double delta, double mu, cx_double omega, Polarization const& Pz){
  hb_ = h;
  ResponseFuncIntegrand::update_parameters(T, delta, mu, omega, Pz);
}

int ResponseFuncIntegrandBilayer::calc(const int *ndim, const cubareal xx[], const int *ncomp, cubareal ff[], void *userdata) const {
   /* Reset */
   for(int comp=0; comp<*ncomp; comp++){ ff[comp] = 0; }

   /* Wavenumbers */
   double k1 = xx[0] * 2 * M_PI;
   double k2 = xx[1] * 2 * M_PI;

   double kx = 0.5 * (k2 + k1);
   double ky = 0.5 * (k2 - k1);

   /* Polarizaiton */
   cx_double Ppmk[NSUBL*NSUBL];
   cx_double Pzzk[NSUBL*NSUBL];

   /* Sum over kz */
   for(int z=0; z < 2; z++){
     double kz = M_PI * z;

     /* Sum over bands (signs) */
     for(int sg1=-1; sg1<=1; sg1+=2){
       for(int sg2=-1; sg2<=1; sg2+=2){
	 // int sg2 = - sg1; /* Opposite sign */
	 cx_double prefactor = calc_prefactor_bare_res_func_bilayer(sg1, sg2, *ts(), T(), kx, ky, kz, Pz()->qx(), Pz()->qy(), Pz()->qz(), omega(), delta(), mu());

	 /* Getting the polarization */
	 Pz()->calc_polarization(hb_, delta(), kx, ky, kz, sg1, sg2, Ppmk, Pzzk);

	 /* Integrand */
	 for(int comp=0; comp<*ncomp; comp+=2){
	   int ope, elem;
	   std::tie(ope, elem) = comp_to_ope_elem(comp, NSUBL*NSUBL);
	   if ( ope == 0 ) {  /* Ppmk */
	     ff[comp] += std::real(prefactor * Ppmk[elem]);
	     ff[comp+1] += std::imag(prefactor * Ppmk[elem]);
	   } else /* ope == 1 */ { /* Pzzk */
	     ff[comp] += std::real(prefactor * Pzzk[elem]);
	     ff[comp+1] += std::imag(prefactor * Pzzk[elem]);
	   }
	 }
       }
     }
   }

   return 0;
}

cx_double calc_intensity_bilayer(int L, hoppings const& ts, double mu, double U, double delta, double qx, double qy, double qz, cx_double omega, bool zz){
  double k1 = 2. * M_PI / (double)L;

  cx_double A = 0, B = 0, C = 0, D = 0;
  for(int z=0; z < 2; z++){
    double kz = M_PI * z;
    for(int x=-L/2; x < L/2; x++){
      double kx = k1 * x;
      for(int y=-L/2; y < L/2; y++){
	double ky = k1 * y;
	add_to_sus_mat2( ts, mu, A, B, C, D, qx, qy, qz, kx, ky, kz, delta, omega, zz );
      }
    }
  }

  int n_sites = L * L * 2;
  double norm = 2. / (double)n_sites;
  A *= norm;
  B *= norm;
  C *= norm;
  D *= norm;

  /* RPA */
  arma::cx_mat chi0_mat(2,2);
  chi0_mat(0,0) = A;   // (A, A) correlation
  chi0_mat(0,1) = B;   // (A, B)
  chi0_mat(1,0) = C;   // (B, A)
  chi0_mat(1,1) = D;   // (B, B)

  /* Transverse = < \sigma^- \sigma^+ >; Longitudinal (zz) = < \sigma^z \sigma^z > */
  /* Note that 2 < \sigma^- \sigma^+ > = < \sigma^z \sigma^z > (U -> 0 for the SU(2) case) */
  double factor_channel = 1.0;
  if ( zz ) { factor_channel = 0.5; }

  arma::cx_mat denom = arma::eye<arma::cx_mat>(NSUBL,NSUBL) - factor_channel * U * chi0_mat;
  arma::cx_mat chi_mat = chi0_mat * arma::inv(denom);

  // sigma-to-spin factor
  double factor_operator = 0.5;
  cx_double chi = factor_operator * factor_operator * ( chi_mat(0,0) - chi_mat(1,0) - chi_mat(0,1) + chi_mat(1,1) );

  // // for check
  // std::cout << chi0_mat << std::endl;
  // std::cout << chi_mat << std::endl;

  return chi;
}

/* Instantiation */
ResponseFuncIntegrandBilayer rfib;

int integrand_wrapper(const int *ndim, const cubareal xx[], const int *ncomp, cubareal ff[], void *userdata){
  return rfib.calc(ndim, xx, ncomp, ff, userdata);
}

std::tuple<arma::cx_mat, arma::cx_mat> calc_bare_response_bilayer(int L, hoppings_bilayer2 const& ts, double mu, double U, double T, double delta, CubaParam const& cbp, Polarization const& Pz, cx_double omega, bool continuous_k){
  arma::cx_mat chi0_pm(NSUBL,NSUBL,arma::fill::zeros);
  arma::cx_mat chi0_zz_u(NSUBL,NSUBL,arma::fill::zeros);

  if ( continuous_k ) {
    /* Changing the parameters */
    rfib.set_parameters(ts, T, delta, mu, omega, Pz);

    /* For Cuba */
    int nregions, neval, fail;
    cubareal integral[cbp.NCOMP], error[cbp.NCOMP], prob[cbp.NCOMP];

    /* Cuhre */
    Cuhre(cbp.NDIM, cbp.NCOMP, integrand_wrapper, cbp.userdata, cbp.nvec, cbp.epsrel, cbp.epsabs, cbp.flags, cbp.mineval, cbp.maxeval, cbp.key, cbp.statefile, cbp.spin, &nregions, &neval, &fail, integral, error, prob);

    // for check
    printf("CUHRE RESULT:\tnregions %d\tneval %d\tfail %d\n", nregions, neval, fail);
    // for(int comp = 0; comp < cbp.NCOMP; comp++ )
    //   printf("CUHRE RESULT:\t%.8f +- %.8f\tp = %.3f\n",
    // 	     (double)integral[comp], (double)error[comp], (double)prob[comp]);

    /* Extracting the integral results */
    for(int comp=0; comp<cbp.NCOMP; comp+=2){
      int ope, elem;
      std::tie(ope, elem) = comp_to_ope_elem(comp, NSUBL*NSUBL);
      int g1 = elem / NSUBL;
      int g2 = elem % NSUBL;
      cx_double int_ope_elem(integral[comp], integral[comp+1]);
      if ( ope == 0 ) {  /* Ppmk */
	chi0_pm(g1,g2) = int_ope_elem;
      } else /* ope == 1 */ { /* Pzzk */
	chi0_zz_u(g1,g2) = int_ope_elem;
      }
    }

    chi0_pm /= 2; /* A factor (2*M_PI) cancels because of the scale change of the integration variables. */
    chi0_zz_u /= 2;
  } else { /* Integral for a finite-size system */
    double k1 = 2. * M_PI / L;
    for(int z=0; z < 2; z++){
      double kz = M_PI * z;
      for(int x=-L/2; x < L/2; x++){
	double kx = k1 * x;
	for(int y=-L/2; y < L/2; y++){
	  double ky = k1 * y;
	  add_to_sus_mat4( ts, T, mu, chi0_pm, chi0_zz_u, kx, ky, kz, Pz, delta, omega );
	}
      }
    }

    int n_units = L * L;  // Number of unit cells
    chi0_pm /= (double)(n_units);
    chi0_zz_u /= (double)(n_units);
  }

  /* Adding the contribution from the down spin */
  arma::cx_mat chi0_zz_d(chi0_zz_u);
  /* Assume NSUBL == 2. */
  if ( NSUBL == 2 ) {
    chi0_zz_d.swap_rows( 0, 1 );
    chi0_zz_d.swap_cols( 0, 1 );
  } else {
    std::cerr << "NSUBL is not 2.\n";
    std::exit(EXIT_FAILURE);
  }
  arma::cx_mat chi0_zz = chi0_zz_u + chi0_zz_d;
  return std::make_tuple(chi0_pm, chi0_zz);
}

std::tuple<cx_double, cx_double> calc_intensity_bilayer2(int L, hoppings_bilayer2& ts, double mu, double U, double T, double delta, CubaParam const& cbp, Polarization const& Pz, cx_double omega, bool continuous_k){
  arma::cx_mat chi0_pm(NSUBL,NSUBL,arma::fill::zeros);
  arma::cx_mat chi0_zz(NSUBL,NSUBL,arma::fill::zeros);

  /* Calculating the bare response functions */
  std::tie(chi0_pm, chi0_zz) = calc_bare_response_bilayer(L, ts, mu, U, T, delta, cbp, Pz, omega, continuous_k);

  /* RPA */
  /* Transverse = < \sigma^- \sigma^+ >; Longitudinal (zz) = < \sigma^z \sigma^z > */
  /* Note that 2 < \sigma^- \sigma^+ > = < \sigma^z \sigma^z > (U -> 0 for the SU(2) case) */
  arma::cx_mat denom_pm = arma::eye<arma::cx_mat>(NSUBL,NSUBL) - U * chi0_pm;
  arma::cx_mat denom_zz = arma::eye<arma::cx_mat>(NSUBL,NSUBL) - 0.5 * U * chi0_zz;
  arma::cx_mat chi_pm = arma::inv(denom_pm) * chi0_pm;
  arma::cx_mat chi_zz = arma::inv(denom_zz) * chi0_zz;

  // sigma-to-spin factor
  cx_double chi_xy = 0.5 * arma::accu(chi_pm);
  cx_double chi_z = 0.25 * arma::accu(chi_zz);

  // // for check
  // std::cout << chi0_pm << std::endl;
  // std::cout << chi_pm << std::endl;
  // std::cout << chi0_zz << std::endl;
  // std::cout << chi_zz << std::endl;

  return std::make_tuple(chi_xy, chi_z);
}


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

#include <boost/hana/assert.hpp>
#include <boost/hana/ext/std/ratio.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/less_equal.hpp>

#include <ratio>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(hana::less(std::ratio<3, 12>{}, std::ratio<5, 12>{}));
BOOST_HANA_CONSTANT_CHECK(hana::less_equal(std::ratio<6, 12>{}, std::ratio<4, 8>{}));

int main() { }


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      US Standard Atmosphere 1976,
 *          http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770009539_1977009539.pdf.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include "tudat/astro/basic_astro/physicalConstants.h"

#include "tudat/astro/aerodynamics/exponentialAtmosphere.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_exponential_atmosphere )

// Summary of tests.
// Test 1: Test set- and get-functions of constants.
// Test 2: Test exponential atmosphere at sea level.
// Test 3: Test exponential atmosphere at 10 km altitude.
// Test 4: Test if the position-independent functions work.

//! Test set- and get-functions of constants.
BOOST_AUTO_TEST_CASE( testExponentialAtmosphereGetSet )
{
    // Initialize constants that need to be set.
    const double constantTemperature = 288.16;
    const double densityAtZeroAltitude = 1.225;
    const double scaleHeight = 7.050e3;

    // Create an exponential atmosphere object.
    aerodynamics::ExponentialAtmosphere exponentialAtmosphere(
                scaleHeight, constantTemperature, densityAtZeroAltitude );

    BOOST_CHECK_EQUAL( constantTemperature, exponentialAtmosphere.getConstantTemperature( ) );
    BOOST_CHECK_EQUAL( densityAtZeroAltitude, exponentialAtmosphere.getDensityAtZeroAltitude( ) );
    BOOST_CHECK_EQUAL( scaleHeight, exponentialAtmosphere.getScaleHeight( ) );
}

//! Check whether the atmosphere is calculated correctly at sea level.
// Values from (US Standard Atmosphere, 1976).
BOOST_AUTO_TEST_CASE( testExponentialAtmosphereSeaLevel )
{
    // Initialize constants that need to be set.
    const double constantTemperature = 288.16;
    const double densityAtZeroAltitude = 1.225;
    const double scaleHeight = 7.050e3;
    const double altitude = 0.0;

    // Initialize the pressure at zero altitude.
    const double pressureAtZeroAltitude = 101325.0;

    // Create an exponential atmosphere object.
    aerodynamics::ExponentialAtmosphere exponentialAtmosphere(
                scaleHeight, constantTemperature, densityAtZeroAltitude );

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    BOOST_CHECK_CLOSE_FRACTION( constantTemperature, exponentialAtmosphere.
                                getTemperature( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( densityAtZeroAltitude, exponentialAtmosphere.
                                getDensity( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( pressureAtZeroAltitude, exponentialAtmosphere.
                                getPressure( altitude ), 0.002 * pressureAtZeroAltitude );
}

//! Test exponential atmosphere at 10 km altitude.
// The given value for pressure was calculated off-line with a calculator.
BOOST_AUTO_TEST_CASE( testExponentialAtmosphereAt10km )
{
    // Initialize constants that need to be set.
    const double constantTemperature = 246.0;//288.16;
    const double densityAtZeroAltitude = 1.225;
    const double scaleHeight = 7.200e3;//7.050e3;
    const double altitude = 10.0e3;

    // Longitude, latitude and time to check overloading.
    double longitude = 0.0;
    double latitude = 0.0;
    double time = 0.0;

    // Create an exponential atmosphere object.
    aerodynamics::ExponentialAtmosphere exponentialAtmosphere(
                scaleHeight, constantTemperature, densityAtZeroAltitude );

    // Create an exponential atmosphere object.
    aerodynamics::ExponentialAtmosphere defaultExponentialAtmosphere( aerodynamics::earth );

    // Declare and set expected density.
    const double expectedDensity  = densityAtZeroAltitude * std::exp ( -altitude / scaleHeight );
    const double expectedPressure = expectedDensity *
            physical_constants::SPECIFIC_GAS_CONSTANT_AIR * constantTemperature;//24526.24934607106;

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    BOOST_CHECK_CLOSE_FRACTION( constantTemperature, exponentialAtmosphere.
                                getTemperature( altitude, longitude, latitude, time ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( constantTemperature, defaultExponentialAtmosphere.
                                getTemperature( altitude, longitude, latitude, time ), tolerance );

    BOOST_CHECK_CLOSE_FRACTION( expectedDensity, exponentialAtmosphere.
                                getDensity( altitude, longitude, latitude, time ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedDensity, defaultExponentialAtmosphere.
                                getDensity( altitude, longitude, latitude, time ), tolerance );

    BOOST_CHECK_CLOSE_FRACTION( expectedPressure, exponentialAtmosphere.
                                getPressure( altitude, longitude, latitude, time ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedPressure, defaultExponentialAtmosphere.
                                getPressure( altitude, longitude, latitude, time ), tolerance );
}

//! Test if the position-independent functions work.
BOOST_AUTO_TEST_CASE( testExponentialAtmospherePositionIndependentFunctions )
{
    // Initialize constants that need to be set.
    const double constantTemperature = 288.16;
    const double densityAtZeroAltitude = 1.225;
    const double scaleHeight = 7.050e3;
    const double altitude = 10.0e3;

    // Longitude, latitude and time to check overloading.
    double longitude = 0.0;
    double latitude = 0.0;
    double time = 0.0;

    // Create an exponential atmosphere object.
    aerodynamics::ExponentialAtmosphere exponentialAtmosphere(
                scaleHeight, constantTemperature, densityAtZeroAltitude );

    const double density1 = exponentialAtmosphere.getDensity( altitude );
    const double density2 = exponentialAtmosphere.getDensity( altitude, longitude, latitude,
                                                              time );

    const double pressure1 = exponentialAtmosphere.getPressure( altitude );
    const double pressure2 = exponentialAtmosphere.getPressure( altitude, longitude, latitude,
                                                                time );

    const double temperature1 = exponentialAtmosphere.getTemperature( altitude );
    const double temperature2 = exponentialAtmosphere.getTemperature( altitude, longitude,
                                                                      latitude, time );

    BOOST_CHECK_EQUAL( density1, density2 );
    BOOST_CHECK_EQUAL( pressure1, pressure2 );
    BOOST_CHECK_EQUAL( temperature1, temperature2 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright Oleg Maximenko 2014.
// 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://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/config.hpp>
#include <svgpp/definitions.hpp>
#include <svgpp/parser/detail/common.hpp>
#include <svgpp/parser/detail/finite_function_iterator.hpp>
#include <svgpp/parser/detail/parse_list_iterator.hpp>
#include <svgpp/parser/detail/value_parser_parameters.hpp>
#include <svgpp/parser/value_parser_fwd.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>

namespace svgpp {

template<SVGPP_TEMPLATE_ARGS>
struct value_parser<tag::type::number, SVGPP_TEMPLATE_ARGS_PASS>
{
  template<class AttributeTag, class Context, class AttributeValue, class PropertySource>
  static bool parse(AttributeTag tag, Context & context, AttributeValue const & attribute_value,
                                    PropertySource property_source)
  {
    namespace qi = boost::spirit::qi;

    typedef detail::value_parser_parameters<Context, SVGPP_TEMPLATE_ARGS_PASS> args_t;
    typedef typename args_t::number_type coordinate_t;
    typedef typename boost::range_const_iterator<AttributeValue>::type iterator_t;

    iterator_t it = boost::begin(attribute_value), end = boost::end(attribute_value);
    SVGPP_STATIC_IF_SAFE const qi::real_parser<coordinate_t, detail::number_policies<coordinate_t, PropertySource> > number;
    coordinate_t value;
    if (qi::parse(it, end, number, value) && it == end)
    {
      args_t::value_events_policy::set(args_t::value_events_context::get(context), tag, property_source, value);
      return true;
    }
    else
    {
      return args_t::error_policy::parse_failed(args_t::error_policy_context::get(context), tag, attribute_value);
    }
  }
};

template<SVGPP_TEMPLATE_ARGS>
struct value_parser<tag::type::list_of<tag::type::number>, SVGPP_TEMPLATE_ARGS_PASS>
{
  template<class AttributeTag, class Context, class AttributeValue, class PropertySource>
  static bool parse(AttributeTag tag, Context & context, AttributeValue const & attribute_value,
                                    PropertySource property_source)
  {
    namespace qi = boost::spirit::qi;

    typedef detail::value_parser_parameters<Context, SVGPP_TEMPLATE_ARGS_PASS> args_t;
    typedef typename args_t::number_type coordinate_t;
    typedef typename boost::range_const_iterator<AttributeValue>::type iterator_t;
    typedef qi::real_parser<coordinate_t, detail::number_policies<coordinate_t, PropertySource> > grammar_t;
    typedef detail::comma_wsp_rule_no_skip<iterator_t> separator_t;
    typedef detail::parse_list_iterator<coordinate_t, iterator_t, grammar_t, separator_t> parse_list_iterator_t;
    typedef detail::finite_function_iterator<parse_list_iterator_t> output_iterator_t;

    SVGPP_STATIC_IF_SAFE const grammar_t number_grammar;
    SVGPP_STATIC_IF_SAFE const separator_t separator_grammar;
    parse_list_iterator_t parse_list(
      boost::begin(attribute_value), boost::end(attribute_value),
      number_grammar, separator_grammar);

    args_t::value_events_policy::set(args_t::value_events_context::get(context), tag, property_source,
      boost::make_iterator_range(output_iterator_t(parse_list), output_iterator_t()));
    if (parse_list.error())
    {
      return args_t::error_policy::parse_failed(args_t::error_policy_context::get(context), tag, attribute_value);
    }
    else
      return true;
  }
};

template<SVGPP_TEMPLATE_ARGS>
struct value_parser<tag::type::number_optional_number, SVGPP_TEMPLATE_ARGS_PASS>
{
  template<class AttributeTag, class Context, class AttributeValue>
  static bool parse(AttributeTag tag, Context & context, AttributeValue const & attribute_value,
                                    tag::source::attribute property_source)
  {
    namespace qi = boost::spirit::qi;
    namespace phx = boost::phoenix;
    using qi::_1;

    typedef detail::value_parser_parameters<Context, SVGPP_TEMPLATE_ARGS_PASS> args_t;
    typedef typename args_t::number_type coordinate_t;
    typedef typename boost::range_const_iterator<AttributeValue>::type iterator_t;

    iterator_t it = boost::begin(attribute_value), end = boost::end(attribute_value);
    SVGPP_STATIC_IF_SAFE const qi::real_parser<coordinate_t, detail::number_policies<coordinate_t, tag::source::attribute> > number;
    SVGPP_STATIC_IF_SAFE const detail::comma_wsp_rule_no_skip<iterator_t> comma_wsp;
    coordinate_t value1, value2;
    bool two_values = false;
    if (qi::parse(it, end,
          number [phx::ref(value1) = _1]
          >> -(
              comma_wsp
              >> number [phx::ref(value2) = _1, phx::ref(two_values) = true]
              )
      ) && it == end)
    {
      if (two_values)
        args_t::value_events_policy::set(args_t::value_events_context::get(context), tag, property_source, value1, value2);
      else
        args_t::value_events_policy::set(args_t::value_events_context::get(context), tag, property_source, value1);
      return true;
    }
    else
    {
      return args_t::error_policy::parse_failed(args_t::error_policy_context::get(context), tag, attribute_value);
    }
  }
};

}


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * 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.
 *******************************************************************************/

#ifndef GaussianProcessLDPLMultiModel_hpp
#define GaussianProcessLDPLMultiModel_hpp

#include <stdio.h>
#include <cmath>

#include <boost/bimap/bimap.hpp>

#include "bleloc.h"
#include "KernelFunction.hpp"
#include "GaussianProcess.hpp"
#include "ObservationModel.hpp"
#include "ObservationModelTrainer.hpp"

namespace loc{

    /**
     ITUModelFunction
     **/

    enum GPType{
        GPNORMAL,
        GPLIGHT
    };

    class ITUModelFunction{

    private:
        double distanceOffset_=1.0;
    public:
        using Ptr = std::shared_ptr<ITUModelFunction>;

        static const int ndim_ = 4;
        int ndim(){return ndim_;}

        ITUModelFunction& distanceOffset(double distanceOffset);
        //void transformFeature(const Location& stateReceiver, const Location& stateTransmitter, double features[]) const;
        std::vector<double> transformFeature(const Location& stateReceiver, const Location& stateTransmitter) const;
        double predict(const double parameters[], const double features[]) const;
        double predict(const std::vector<double>& parameters, const std::vector<double>& features) const;

        template<class Archive>
        void serialize(Archive& ar);
    };

    /**
      GaussianProcess based model
     **/
    struct GaussianProcessLDPLMultiModelParameters{
        // Parameters for optimization
        int maxIteration_ = 10000;
        double tolranceOptimization_ = 0.00001;

        std::vector<double> lambdas{1000.0, 0.001, 1000, 1000};
        std::vector<double> rhos{0, 0, 100, 100};

    };

    template<class Tstate, class Tinput>
    class GaussianProcessLDPLMultiModel;

    template<class Tstate, class Tinput>
    class GaussianProcessLDPLMultiModelTrainer;

    template<class Tstate, class Tinput>
    class GaussianProcessLDPLMultiModel : public ObservationModel<Tstate, Tinput>{
    private:

        GaussianProcessLDPLMultiModelParameters trainParams;

        //std::shared_ptr<KernelFunction> mKernel;
        BLEBeacons mBLEBeacons;

        //ITUModelFunction mITUModel;
        std::map<BeaconId, ITUModelFunction> mITUModelMap;

        std::vector<std::vector<double>> mITUParameters;

        //GaussianProcess mGP;
        std::shared_ptr<GaussianProcess> mGP;
        std::map<BeaconId, int> mBeaconIdIndexMap;
        //boost::bimaps::bimap<long, int> mBeaconIdIndexBimap;
        std::vector<double> mRssiStandardDeviations;
        bool mFillsUnknownBeaconRssi = false;
        double mStdevRssiForUnknownBeacon = 0.0;
        double computeNormalStandardDeviation(std::vector<double> standardDeviations);
        double mCoeffDiffFloorStdev = 5.0;

        // Private function to train the model
        //GaussianProcessLDPLMultiModel& kernelFunction(std::shared_ptr<KernelFunction> kernel);
        GaussianProcessLDPLMultiModel& bleBeacons(BLEBeacons bleBeacons);
        GaussianProcessLDPLMultiModel& train(Samples samples);
        std::vector<std::vector<double>> fitITUModel(Samples samples);
        std::vector<double> computeRssiStandardDeviations(Samples samples);
        std::vector<int> extractKnownBeaconIndices(const Tinput& beacons) const;

        friend class GaussianProcessLDPLMultiModelTrainer<Tstate, Tinput>;
        int version = 3;
        GPType gpType = GPNORMAL;

        //parameters for delayed prediction
        int mTDelay = 1;
        double mDTDelay = 1000; //ms
        double mDTDelayMargin = 200; //ms

    public:
        GaussianProcessLDPLMultiModel() = default;
        ~GaussianProcessLDPLMultiModel() = default;

        std::function<double(double, double, double)> normFunc = MathUtils::logProbaNormal;

        std::vector<Tstate>* update(const std::vector<Tstate> & states, const Tinput & input) override {
            std::cout << "GaussianProcessLDPLMultiModel::update is not supported." << std::endl;
            std::vector<Tstate>* statesCopy = new std::vector<Tstate>(states);
            return statesCopy;
        }

        Tinput convertInput(const Tinput& input);
        // predict mean and stdev given state for input beacon id
        std::map<BeaconId, NormalParameter> predict(const Tstate& state, const Tinput& input) const;

        double computeLogLikelihood(const Tstate& state, const Tinput& input);
        std::vector<double> computeLogLikelihood(const std::vector<Tstate> & states, const Tinput& input) override;

        std::vector<double> computeLogLikelihoodRelatedValues(const Tstate& state, const Tinput& input);
        std::vector<std::vector<double>> computeLogLikelihoodRelatedValues(const std::vector<Tstate> & states, const Tinput& input) override;

        GaussianProcessLDPLMultiModel& fillsUnknownBeaconRssi(bool fills);
        bool fillsUnknownBeaconRssi() const;

        GaussianProcessLDPLMultiModel& rssiStandardDeviationForUnknownBeacons(double stdevRssi);
        double rssiStandardDeviationForUnknownBeacons() const;

        GaussianProcessLDPLMultiModel& coeffDiffFloorStdev(double);
        GaussianProcessLDPLMultiModel& tDelay(int);

        template<class Archive>
        void save(Archive& ar) const;
        template<class Archive>
        void load(Archive& ar);

        void save(std::ofstream& ofs) const;
        void save(std::ostringstream& oss) const;
        void load(std::ifstream& ifs);
        void load(std::istringstream& iss);

        bool applyLowestLogLikelihood = false;
    };

    class MLAdapter{
    public:
        static std::vector<double> locationToVec(const Location& loc){
            std::vector<double> vec{loc.x(), loc.y(), loc.z(), loc.floor()};
            return vec;
        }
    };

    /**
     Definition of trainer
     **/
    template<class Tstate, class Tinput>
    class GaussianProcessLDPLMultiModelTrainer : public ObservationModelTrainer<Tstate, Tinput>{

    public:
        ~GaussianProcessLDPLMultiModelTrainer() override {}
        GaussianProcessLDPLMultiModel<Tstate, Tinput>* train() override;
        GaussianProcessLDPLMultiModelTrainer<Tstate, Tinput>& dataStore(std::shared_ptr<DataStore> dataStore) override{
            mDataStore = dataStore;
            return *this;
        }

        void setGPType(GPType gt){
            gpType = gt;
        }

    private:
        std::shared_ptr<DataStore> mDataStore;
        GPType gpType = GPNORMAL;
    };

}
#endif /* GaussianProcessLDPLMultiModel_hpp */


#include <boost/date_time/posix_time/posix_time.hpp>

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>

#include "Util.h"

int Util::getUnixTime()
{
	return time(NULL);
}

int64 Util::getUnixMsTime()
{
	return Util::getPTime().time_of_day().total_milliseconds();
}

PTime Util::getPTime()
{
	return boost::posix_time::microsec_clock::local_time();
}

int64 Util::getDuration(PTime p1, PTime p2)
{
	boost::posix_time::time_duration p = p1-p2;
	return p.total_milliseconds();
}

float Util::getDurationMicro(PTime p1, PTime p2)
{
	boost::posix_time::time_duration p = p1-p2;
	return p.total_microseconds()/(float)1000000;
}

std::string ts("2000-01-01 00:00:00.001");
PTime begin(boost::posix_time::time_from_string(ts));

double Util::getMSTime()
{
	PTime now = boost::posix_time::microsec_clock::local_time();

	boost::posix_time::time_duration p = now-begin;
	return p.total_microseconds()/1000000.0;
}

boost::minstd_rand fRnd(time(0));
double Util::getRand(double start, double end)
{
	boost::uniform_real<> uni_dist(start,end);
	boost::variate_generator<boost::minstd_rand&, boost::uniform_real<> > uni(fRnd, uni_dist);
	return uni();
}

boost::minstd_rand intRnd(time(0));
int Util::getRand(int start, int end)
{
	if (start == end)
	{
		return start;
	}
	int random_variable = std::rand();
	return random_variable % (end-start+1) + start;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//
#include "InferenceTestImage.hpp"
#include "ImagePreprocessor.hpp"
#include "Permute.hpp"
#include <armnn/TypesUtils.hpp>

#include <boost/numeric/conversion/cast.hpp>
#include <boost/assert.hpp>
#include <boost/format.hpp>

#include <iostream>
#include <fcntl.h>
#include <array>

template <typename TDataType>
unsigned int ImagePreprocessor<TDataType>::GetLabelAndResizedImageAsFloat(unsigned int testCaseId,
                                                                          std::vector<float> & result)
{
    testCaseId = testCaseId % boost::numeric_cast<unsigned int>(m_ImageSet.size());
    const ImageSet& imageSet = m_ImageSet[testCaseId];
    const std::string fullPath = m_BinaryDirectory + imageSet.first;

    InferenceTestImage image(fullPath.c_str());

    // this ResizeBilinear result is closer to the tensorflow one than STB.
    // there is still some difference though, but the inference results are
    // similar to tensorflow for MobileNet

    result = image.Resize(m_Width, m_Height, CHECK_LOCATION(),
                          InferenceTestImage::ResizingMethods::BilinearAndNormalized,
                          m_Mean, m_Stddev);

    // duplicate data across the batch
    for (unsigned int i = 1; i < m_BatchSize; i++)
    {
        result.insert(result.end(), result.begin(), result.begin() + boost::numeric_cast<int>(GetNumImageElements()));
    }

    if (m_DataFormat == DataFormat::NCHW)
    {
        const armnn::PermutationVector NHWCToArmNN = { 0, 2, 3, 1 };
        armnn::TensorShape dstShape({m_BatchSize, 3, m_Height, m_Width});
        std::vector<float> tempImage(result.size());
        armnnUtils::Permute(dstShape, NHWCToArmNN, result.data(), tempImage.data(), sizeof(float));
        result.swap(tempImage);
    }

    return imageSet.second;
}

template <>
std::unique_ptr<ImagePreprocessor<float>::TTestCaseData>
ImagePreprocessor<float>::GetTestCaseData(unsigned int testCaseId)
{
    std::vector<float> resized;
    auto label = GetLabelAndResizedImageAsFloat(testCaseId, resized);
    return std::make_unique<TTestCaseData>(label, std::move(resized));
}

template <>
std::unique_ptr<ImagePreprocessor<uint8_t>::TTestCaseData>
ImagePreprocessor<uint8_t>::GetTestCaseData(unsigned int testCaseId)
{
    std::vector<float> resized;
    auto label = GetLabelAndResizedImageAsFloat(testCaseId, resized);

    size_t resizedSize = resized.size();
    std::vector<uint8_t> quantized(resized.size());

    for (size_t i=0; i<resizedSize; ++i)
    {
        quantized[i] = armnn::Quantize<uint8_t>(resized[i],
                                                m_Scale,
                                                m_Offset);
    }
    return std::make_unique<TTestCaseData>(label, std::move(quantized));
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/traversable/traverse_mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/applicative.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/traversable.hpp>

#include <tuple>
using namespace boost::hana;

template <typename mcd>
void test() {
    constexpr auto traversable = detail::minimal::traversable<mcd>;
    constexpr auto applicative = detail::minimal::applicative<>;
    constexpr auto comparable = detail::minimal::comparable<>;
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = [=](auto x) {
        return applicative(std::make_tuple(x));
    };

    BOOST_HANA_CONSTEXPR_ASSERT(
        traverse<detail::minimal::Applicative<>>(f, traversable(comparable(1)))
            ==
        applicative(traversable(std::make_tuple(comparable(1))))
    );
}

int main() {
    test<Traversable::traverse_mcd>();
}


//  Copyright (c) 2017 Zahra Khatami
//  Copyright (c) 2016 David Pfander
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#include <stdlib.h>
#include <vector>

#include <hpx/hpx_init.hpp>
#include <hpx/parallel/algorithms/for_each.hpp>
#include <hpx/util/high_resolution_timer.hpp>
#include <boost/range.hpp>
#include <vector>
#include <initializer_list>
#include <algorithm>
#include <typeinfo>
#include <iterator>
#include <hpx/parallel/executors/sequential_execution_wrapper.hpp>
#include <hpx/parallel/seq_or_par.hpp>
#include <hpx/parallel/chunk_size_determination.hpp>
#include <hpx/parallel/prefetching_distance_determination.hpp>
#include <hpx/parallel/executors/dynamic_chunk_size.hpp>
#include <hpx/parallel/executors/adaptive_chunk_size.hpp>

#define vector_size 200

double mysecond()
{
    return hpx::util::high_resolution_clock::now() * 1e-9;
}

template<typename T>
void comparing_perfromances(std::vector<T>& A, std::vector<T>& B, std::vector<T>& C) {

    auto time_range = boost::irange(0, 200);

    auto f = [&](int i) {
        if(i % 4 == 0) {
            for (int j = 0; j < vector_size; j += 4) {

                T result1 = 0.0;
                T result2 = 0.0;
                T result3 = 0.0;
                T result4 = 0.0;

                T result5 = 0.0;
                T result6 = 0.0;
                T result7 = 0.0;
                T result8 = 0.0;

                T result9 = 0.0;
                T result10 = 0.0;
                T result11 = 0.0;
                T result12 = 0.0;

                T result13 = 0.0;
                T result14 = 0.0;
                T result15 = 0.0;
                T result16 = 0.0;

                for (int k = 0; k < vector_size; k++) {

                    result1 += A[i * vector_size + k] * B[j * vector_size + k];
                    result2 += A[i * vector_size + k] * B[(j + 1) * vector_size + k];
                    result3 += A[i * vector_size + k] * B[(j + 2) * vector_size + k];
                    result4 += A[i * vector_size + k] * B[(j + 3) * vector_size + k];

                    result5 += A[(i + 1) * vector_size + k] * B[j * vector_size + k];
                    result6 += A[(i + 1) * vector_size + k] * B[(j + 1) * vector_size + k];
                    result7 += A[(i + 1) * vector_size + k] * B[(j + 2) * vector_size + k];
                    result8 += A[(i + 1) * vector_size + k] * B[(j + 3) * vector_size + k];

                    result9 += A[(i + 2) * vector_size + k] * B[j * vector_size + k];
                    result10 += A[(i + 2) * vector_size + k] * B[(j + 1) * vector_size + k];
                    result11 += A[(i + 2) * vector_size + k] * B[(j + 2) * vector_size + k];
                    result12 += A[(i + 2) * vector_size + k] * B[(j + 3) * vector_size + k];

                    result13 += A[(i + 3) * vector_size + k] * B[j * vector_size + k];
                    result14 += A[(i + 3) * vector_size + k] * B[(j + 1) * vector_size + k];
                    result15 += A[(i + 3) * vector_size + k] * B[(j + 2) * vector_size + k];
                    result16 += A[(i + 3) * vector_size + k] * B[(j + 3) * vector_size + k];
                }

                C[i * vector_size + j] = result1;
                C[i * vector_size + (j + 1)] = result2;
                C[i * vector_size + (j + 2)] = result3;
                C[i * vector_size + (j + 3)] = result4;

                C[(i + 1) * vector_size + j] = result5;
                C[(i + 1) * vector_size + (j + 1)] = result6;
                C[(i + 1) * vector_size + (j + 2)] = result7;
                C[(i + 1) * vector_size + (j + 3)] = result8;

                C[(i + 2) * vector_size + j] = result9;
                C[(i + 2) * vector_size + (j + 1)] = result10;
                C[(i + 2) * vector_size + (j + 2)] = result11;
                C[(i + 2) * vector_size + (j + 3)] = result12;

                C[(i + 3) * vector_size + j] = result13;
                C[(i + 3) * vector_size + (j + 1)] = result14;
                C[(i + 3) * vector_size + (j + 2)] = result15;
                C[(i + 3) * vector_size + (j + 3)] = result16;
            }
        }
    };

    ////////////////////////////////////////////////////////////////////////
    // [1] Original code implemantion with HPX
    double t_origin = mysecond();

    hpx::parallel::for_each(hpx::parallel::par, time_range.begin(), time_range.end(), f);

    double elapsed_origin = mysecond() - t_origin;

    ////////////////////////////////////////////////////////////////////////
    // [2] Efficient chunk size
    double t_chunk = mysecond();

	hpx::parallel::for_each(hpx::parallel::par.with(hpx::parallel::adaptive_chunk_size()), time_range.begin(), time_range.end(), f);

    double elapsed_chunk = mysecond() - t_chunk;

    ////////////////////////////////////////////////////////////////////////
    // [3] Prefetching:
    std::size_t pref_dist_fac = 2;
    auto policy = hpx::parallel::par;

    double t_prefetch = mysecond();

	hpx::parallel::for_each(hpx::parallel::execution::make_prefetcher_policy(policy, pref_dist_fac, A, B, C), time_range.begin(), time_range.end(), f);

    double elapsed_prefetch = mysecond() - t_prefetch;

    ////////////////////////////////////////////////////////////////////////
    // Printing results

    double byte = vector_size * (16 +
        (vector_size * ((7 + 3 * 8) + (8 + 3 * 9) + (8 + 3 * 9) + (8 + 3 * 9))) +
        ((3 + 3 * 4) + (4 + 3 * 5) + (4 + 3 * 5) + (4 + 3 * 5)));

    // MB
    std::cout << std::left << "rate_origin = ";
    std::cout << std::left << (1.0E-09 * (byte * sizeof(double)) / (elapsed_origin)) << " [GB/s]";
    std::cout << std::left << "\nrate_chunk = ";
    std::cout << std::left << (1.0E-09 * (byte * sizeof(double)) / (elapsed_chunk)) << " [GB/s]";
    std::cout << std::left << "\nrate_prefetching = ";
    std::cout << std::left << (1.0E-09 * (byte * sizeof(double)) / (elapsed_prefetch)) << " [GB/s]";
    std::cout << std::endl;
}

int hpx_main(int argc, char* argv[])
{
    // Initialization
    std::size_t size_of_mat = vector_size * vector_size;
    std::vector<double> A(size_of_mat, 0.0);
    std::vector<double> B(size_of_mat, 0.0);
    std::vector<double> C(size_of_mat, 0.0);

    for(std::size_t r = 0; r < size_of_mat; r++) {
        srand(r);
        A[r] = (r + 1) * (r + 10) * (rand() % 10 + 1);
        B[r] = (r + 1) * (r + 10) * (rand() % 10 + 1);
        C[r] = (r + 1) * (r + 10) * (rand() % 10 + 1);
    }

    comparing_perfromances(A, B, C);

    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    return hpx::init(argc, argv);
}


/* vim: set tabstop=4 : */
#pragma once

#include <assert.h>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/split_free.hpp>

#include "var_int.hpp"

namespace boost { namespace serialization {

template<class Archive>
uint32_t load_var_uint32(Archive& ar)
{
	uint32_t v = 0;
	for (int shift = 0; shift < 35; shift += 7)
	{
		unsigned char b; ar >> b;
		v |= uint32_t(b & 0x7F) << shift;
		if ((b & 0x80) == 0)
			return v;
	}
	assert(0); // should not get here
	return v; // avoid compile warning
}

template<class Archive>
uint64_t load_var_uint64(Archive& ar)
{
	uint64_t v = 0;
	unsigned char b;
	for (int shift = 0; shift < 56; shift += 7)
	{
		ar >> b;
		v |= uint64_t(b & 0x7F) << shift;
		if ((b & 0x80) == 0)
			return v;
	}
	ar >> b;
	v |= uint64_t(b) << 56;
	return v;
}

//////////////////////////////////////////////////////////////////////////

template<class Archive>
void load(Archive& ar, terark::var_uint32_t& x, unsigned/*version*/)
{
	x.t = load_var_uint32(ar);
}

template<class Archive>
void load(Archive& ar, terark::var_int32_t& x, unsigned/*version*/)
{
	x.t = var_int32_u2s(load_var_uint32(ar));
}

template<class Archive>
void save(Archive& ar, terark::var_uint32_t x, unsigned/*version*/)
{
	unsigned char buf[5];
	ar.save_binary(buf, terark::save_var_uint32(buf, x.t) - buf);
}

template<class Archive>
void save(Archive& ar, terark::var_int32_t x, unsigned/*version*/)
{
	unsigned char buf[5];
	ar.save_binary(buf, terark::save_var_int32(buf, x.t) - buf);
}

//////////////////////////////////////////////////////////////////////////
template<class Archive>
void load(Archive& ar, terark::var_uint64_t& x, unsigned/*version*/)
{
	x.t = terark::load_var_uint64(ar);
}

template<class Archive>
void load(Archive& ar, terark::var_int64_t& x, unsigned/*version*/)
{
	x.t = terark::var_int64_u2s(load_var_uint64(ar));
}

template<class Archive>
void save(Archive& ar, terark::var_uint64_t x, unsigned/*version*/)
{
	unsigned char buf[9];
	ar.save_binary(buf, terark::save_var_uint64(buf, x.t) - buf);
}

template<class Archive>
void save(Archive& ar, terark::var_int64_t x, unsigned/*version*/)
{
	unsigned char buf[9];
	ar.save_binary(buf, terark::save_var_int64(buf, x.t) - buf);
}

} } // namespace boost::serialization

BOOST_SERIALIZATION_SPLIT_FREE(terark::var_uint32_t)
BOOST_SERIALIZATION_SPLIT_FREE(terark::var_int32_t)
BOOST_SERIALIZATION_SPLIT_FREE(terark::var_uint64_t)
BOOST_SERIALIZATION_SPLIT_FREE(terark::var_int64_t)


#include "stdafx.h"

#include <boost\test\unit_test.hpp>

#include <boost\gil\extension\opencv\convert_color.hpp>

#include <boost\gil\extension\io_new\png_all.hpp>

using namespace boost::gil;
using namespace boost::gil::opencv;

BOOST_AUTO_TEST_CASE( test_convert_color )
{
    rgb32f_image_t src;
    read_and_convert_image( "..\\in\\in.png", src, png_tag() );

    xyz32f_image_t dst( src.dimensions() );

    cvtcolor( view( src )
            , view( dst )
            );

    rgb32f_image_t out( src.dimensions() );

    cvtcolor( view( dst )
            , view( out )
            );

    write_view( "..\\out\\convert_color.png"
              , color_converted_view< rgb8_pixel_t >( view( out ))
              , png_tag()
              );
}

BOOST_AUTO_TEST_CASE( test_convert_color_bit_aligned )
{
    bgr565_image_t src;
    rgba8_image_t dst( src.dimensions() );

/*
    // @todo: not implemented yet
    cvtcolor( view( src )
            , view( dst )
            );
*/
}
/*
BOOST_AUTO_TEST_CASE( test_convert_color_using_xyz_colorspace )
{
    xyz32f_image_t src;
    rgb32f_image_t dst( src.dimensions() );

    cvtcolor( view( src )
            , view( dst )
            );
}
*/


#include "insertImage.h"
#include "globalfunctions.h"
#include <QFileDialog>
#include <QMessageBox>
#include <boost/shared_ptr.hpp>
#include <kernel/pdfoperators.h>
#include <kernel/factories.h>
#include <kernel/cinlineimage.h>
#include <kernel/displayparams.h>
#include <QToolTip>

using namespace boost;
using namespace pdfobjects;

void InsertImage::closeEvent ( QCloseEvent * event )
{
	emit ImageClosedSignal();
}

InsertImage::InsertImage( QWidget * parent) : QWidget(parent)
{
	ui.setupUi(this);
	connect(this->ui.rotation, SIGNAL(sliderMoved(int)), this, SLOT(showAngleToolTip(int)));
#ifndef _DEBUG
	ui.positionX->hide();
	ui.positionY->hide();
#endif
}

void InsertImage::setSize(float w,float h)
{
	this->ui.imageChooseFrame->show();
	ui.sizeX->setValue(fabs(w));
	ui.sizeY->setValue(fabs(h));
	ui.rotation->setValue(0);
}
void InsertImage::setPosition(float pdfX,float pdfY)
{
	init();
	ui.positionX->setValue(pdfX);
	ui.positionY->setValue(pdfY);
}
void InsertImage::setImagePath()
{
	//open dialog and get file
	QString fileName = QFileDialog::getOpenFileName(this, tr("Open File"), "/home", tr("Images (*.png *.jpg *.bmp)"));
	if (!fileName.size() == 0)
		this->ui.lineEdit->setText(fileName);
}
//v tomto okamihu mame nazov obrazka
//void InsertImage::rotationCm(int angle)
//{
//	float radian = toRadians(angle);
//	float scale = 1; //this->ui.scale->value();
//	double cs = scale*cos(static_cast<double>(radian));
//	QVariant cs1(cs);
//	double sn = scale*sin(static_cast<double>(radian));
//	QVariant sn1(sn);
//	QVariant sn2(sn*-1);
//	ui.cm->setItem(0,0,new QTableWidgetItem(cs1.toString()));
//	ui.cm->setItem(0,1,new QTableWidgetItem(sn1.toString()));
//	ui.cm->setItem(1,0,new QTableWidgetItem(sn2.toString()));
//	ui.cm->setItem(1,1,new QTableWidgetItem(cs1.toString()));
//}
void InsertImage::createInlineImage()
{
	if (ui.lineEdit->text().isEmpty())
	{
		close();
		return;
	}
	QImage image;
	if (!image.load(tr(ui.lineEdit->text().toAscii().data())))
	{
		QMessageBox::warning(this,tr("Unable to load file"),tr("Qt4 cannot load the image"),QMessageBox::Ok);
		return;
	}
	image = image.scaled(ui.sizeX->value(),ui.sizeY->value(),Qt::IgnoreAspectRatio); //proporcie
	CDict image_dict;
	image_dict.addProperty ("W", CInt (image.width()));
	image_dict.addProperty ("H", CInt (image.height()));
	image_dict.addProperty ("CS", CName ("RGB"));
	image_dict.addProperty ("BPC", CInt (8)); //bits per component, kontanta, ine pdf nevie:)

	int depth = image.depth(); //8 - 8 bitov per pixel
	std::vector<char> imageData; //32 bitove sa musia pretypovat na QRGB

	for(int h = 0; h< image.height(); h++)
	{
		for ( int w = 0; w < image.width(); w++)
		{
			QRgb color = image.pixel(w,h);
			int r = qRed(color);
			int g = qGreen(color);
			int b = qBlue(color);
			imageData.push_back(r);
			imageData.push_back(g);
			imageData.push_back(b);
		}
	}
	PdfInlineImage im = boost::shared_ptr<CInlineImage>(new CInlineImage (image_dict,imageData));
	biOp = boost::shared_ptr<pdfobjects::InlineImageCompositePdfOperator>(new InlineImageCompositePdfOperator (im));
}
void InsertImage::init()
{
	ui.positionX->setMaximum(DisplayParams::DEFAULT_PAGE_RX);//o kolko a poze posunut doprava
	ui.positionX->setMinimum(0);//o kolko a poze posunut doprava
	ui.positionX->setValue(0);

	ui.positionY->setMaximum(DisplayParams::DEFAULT_PAGE_RY);//o kolko a poze posunut doprava
	ui.positionY->setMinimum(0);//o kolko a poze posunut doprava
	ui.positionY->setValue(0);
}
void InsertImage::apply()
{
	bool create = false;
	if (biOp == NULL)
	{
		createInlineImage();
		create = true;
	}
	if (biOp == NULL)
		return;
	//add to buffer everything that is in image
	shared_ptr<UnknownCompositePdfOperator> q(new UnknownCompositePdfOperator("q", "Q"));

	int pixW = ui.sizeX->value();
	int pixH = ui.sizeY->value();
	float scaleX = (float)ui.scaleX->value()/(100.0f);
	float scaleY = (float)ui.scaleY->value()/(100.0f); //POCET KOMPONENT
	//mame maticy translacie & scale ( mozeme to dat dokopy))
	PdfOperator::Operands posOperands;
	if (create)
	{//translation rotation scale
		q->push_back(createOperatorTranslation(ui.positionX->value(),ui.positionY->value()-pixH ),getLastOperator(q));
		q->push_back(createOperatorRotation(toRadians(ui.rotation->value() )),getLastOperator(q));
		q->push_back(createOperatorScale(pixW*scaleX,pixH*scaleY ),getLastOperator(q));
	}
	else
	{
		q->push_back(_invertCm,getLastOperator(q));
		q->push_back(createOperatorTranslation(0,0),getLastOperator(q));
		q->push_back(createOperatorRotation(toRadians(ui.rotation->value())),getLastOperator(q));
		q->push_back(createOperatorScale(pixW*scaleX,pixH*scaleY ),getLastOperator(q));
	}
	//vyhod alpha channel
	q->push_back(biOp,getLastOperator(q));
	PdfOperator::Operands o;
	q->push_back(createOperator("Q", o), getLastOperator(q));
	if (create)
		emit(insertImage(q));
	else
		emit(changeImage(q));
	biOp = boost::shared_ptr<InlineImageCompositePdfOperator>();
	this->close();
}
/** \brief pdf matrix convertor */
struct Cm
{
	/// matrix
	double matrix[6];
	/// constructor
	Cm (double a=1.0f, double b=0.0f, double c=0.0f, double d=1.0f, double e=0.0f, double f=0.0f)
	{
		matrix[0] = a;matrix[1] = b;matrix[2] = c;
		matrix[3] = d;matrix[4] = e;matrix[5] = f;
	}
};
void InsertImage::getInvertMatrix( PdfOp ii, double * act, double * res )
{
	DisplayParams params = emit NeedDisplayParamsSignal();
	PDFRectangle pdfRect ( params.pageRect.xleft, params.pageRect.yleft, params.pageRect.xright, params.pageRect.yright );
	GfxState state(params.hDpi, params.vDpi, &pdfRect, params.rotate, params.upsideDown );
PdfOperator::Iterator iter = PdfOperator::getIterator(ii);
	iter = iter.next();
	bool addCm = false;
	std::vector<Cm> cms;
	int ballance =0; //kolko Q sme nasli ( neamju am byt )
	while (true)
	{
		iter = iter.prev();
		if (!iter.valid())
			break;
		std::string name;
		iter.getCurrent()->getOperatorName(name);
		if (name=="Q") //koniec nasho hladania
		{//zmaz poslednych 6
			ballance++;
			continue;
		}
		if (name == "q" && ballance>0)
		{
			ballance--;//uzatvara nejake Q
			continue;
		}
		if (name== "cm" && !ballance)
		{
			Cm cm;
			PdfOperator::Operands opers;
			iter.getCurrent()->getParameters(opers);
			assert(opers.size()==6);
			shared_ptr<CArray> rect;
			for ( int i = 0; i<6;i++)
			{
				cm.matrix[i] = utils::getDoubleFromIProperty(opers[i]);
			}
			cms.push_back(cm);
		}
	}
	while (cms.size())
	{
		Cm cm = cms.back();
		state.concatCTM(cm.matrix[0],cm.matrix[1],cm.matrix[2],cm.matrix[3],cm.matrix[4],cm.matrix[5]);
		cms.pop_back();
	}
	const double * d = state.getCTM();
	memcpy(act,d,sizeof(double)*6);
	double s = -d[3]*d[0] + d[1]*d[2];
	Cm c(-d[3]/s,d[1]/s,-d[2]/s,d[0]/s,0,0);//ziadne posuny
#if _DEBUG
	state.concatCTM(c.matrix[0],c.matrix[1],c.matrix[2],c.matrix[3],c.matrix[4],c.matrix[5]);
	d = state.getCTM();
	/*assert(fabs(d[0] - 1)<1e-4);
	assert(fabs(d[1])<1e-4);
	assert(fabs(d[2])<1e-4);
	assert(fabs(d[3]-1)<1e-4);*/
#endif
	/*c.matrix[0]=1;
	c.matrix[0]=0;
	c.matrix[0]=0;
	c.matrix[3]=1;*/
	memcpy(res,c.matrix,sizeof(double)*6);
}
void InsertImage::setImage(PdfOp ii)
{
	this->ui.imageChooseFrame->hide();
	biOp = boost::dynamic_pointer_cast<InlineImageCompositePdfOperator>(ii->clone());
	ui.sizeX->setValue(biOp->getWidth());
	ui.sizeY->setValue(biOp->getHeight());
	//najdeme inverznu maticu
	Cm c;
	double actual[6];
	getInvertMatrix(ii,actual, c.matrix);
	PdfOperator::Operands operands;
	for ( int i =0; i<6; i++)
		operands.push_back(shared_ptr<IProperty>(CRealFactory::getInstance(c.matrix[i])));
	_invertCm = createOperator("cm",operands);
	BBox b = ii->getBBox();
	//ui.positionX->setMaximum(DisplayParams::DEFAULT_PAGE_RX*_scale-min(b.xleft,b.xleft));//o kolko a poze posunut doprava
	//ui.positionX->setMinimum(-min(b.xleft,b.xleft));//o kolko a poze posunut doprava
	ui.positionX->setValue(0);

	//ui.positionY->setMaximum(DisplayParams::DEFAULT_PAGE_RY*_scale-min(b.yleft,b.yleft));//o kolko a poze posunut doprava
	//ui.positionY->setMinimum(-min(b.yleft,b.yleft));//o kolko a poze posunut doprava
	ui.positionY->setValue(0);
}

void InsertImage::showAngleToolTip( int value )
{
	QVariant v(value);
	QString str(QString("Angle:") + v.toString());
	QToolTip::showText( QCursor::pos() , str, this );
	this->ui.rotation->setToolTip(str);
	this->ui.angleText->setText(v.toString());
}


#ifndef HIDDEN_MARKOV_MODEL
#define HIDDEN_MARKOV_MODEL

#include "util/integer_range.hpp"

#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include <cmath>

#include <limits>
#include <vector>

namespace osrm
{
namespace engine
{
namespace map_matching
{

static const double log_2_pi = std::log(2. * boost::math::constants::pi<double>());
static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits<double>::infinity();
static const double MINIMAL_LOG_PROB = std::numeric_limits<double>::lowest();
static const std::size_t INVALID_STATE = std::numeric_limits<std::size_t>::max();

// closures to precompute log -> only simple floating point operations
struct EmissionLogProbability
{
    double sigma_z;
    double log_sigma_z;

    EmissionLogProbability(const double sigma_z) : sigma_z(sigma_z), log_sigma_z(std::log(sigma_z))
    {
    }

    double operator()(const double distance) const
    {
        return -0.5 * (log_2_pi + (distance / sigma_z) * (distance / sigma_z)) - log_sigma_z;
    }
};

struct TransitionLogProbability
{
    double beta;
    double log_beta;
    TransitionLogProbability(const double beta) : beta(beta), log_beta(std::log(beta)) {}

    double operator()(const double d_t) const { return -log_beta - d_t / beta; }
};

template <class CandidateLists> struct HiddenMarkovModel
{
    std::vector<std::vector<double>> viterbi;
    std::vector<std::vector<std::pair<unsigned, unsigned>>> parents;
    std::vector<std::vector<float>> path_distances;
    std::vector<std::vector<bool>> pruned;
    std::vector<bool> breakage;

    const CandidateLists &candidates_list;
    const std::vector<std::vector<double>> &emission_log_probabilities;

    HiddenMarkovModel(const CandidateLists &candidates_list,
                      const std::vector<std::vector<double>> &emission_log_probabilities)
        : breakage(candidates_list.size()), candidates_list(candidates_list),
          emission_log_probabilities(emission_log_probabilities)
    {
        viterbi.resize(candidates_list.size());
        parents.resize(candidates_list.size());
        path_distances.resize(candidates_list.size());
        pruned.resize(candidates_list.size());
        breakage.resize(candidates_list.size());
        for (const auto i : util::irange<std::size_t>(0UL, candidates_list.size()))
        {
            const auto &num_candidates = candidates_list[i].size();
            // add empty vectors
            if (num_candidates > 0)
            {
                viterbi[i].resize(num_candidates);
                parents[i].resize(num_candidates);
                path_distances[i].resize(num_candidates);
                pruned[i].resize(num_candidates);
            }
        }

        Clear(0);
    }

    void Clear(std::size_t initial_timestamp)
    {
        BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_distances.size() &&
                     path_distances.size() == pruned.size() && pruned.size() == breakage.size());

        for (const auto t : util::irange(initial_timestamp, viterbi.size()))
        {
            std::fill(viterbi[t].begin(), viterbi[t].end(), IMPOSSIBLE_LOG_PROB);
            std::fill(parents[t].begin(), parents[t].end(), std::make_pair(0U, 0U));
            std::fill(path_distances[t].begin(), path_distances[t].end(), 0.0);
            std::fill(pruned[t].begin(), pruned[t].end(), true);
        }
        std::fill(breakage.begin() + initial_timestamp, breakage.end(), true);
    }

    std::size_t initialize(std::size_t initial_timestamp)
    {
        auto num_points = candidates_list.size();
        do
        {
            BOOST_ASSERT(initial_timestamp < num_points);

            for (const auto s : util::irange<std::size_t>(0UL, viterbi[initial_timestamp].size()))
            {
                viterbi[initial_timestamp][s] = emission_log_probabilities[initial_timestamp][s];
                parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                pruned[initial_timestamp][s] = viterbi[initial_timestamp][s] < MINIMAL_LOG_PROB;

                breakage[initial_timestamp] =
                    breakage[initial_timestamp] && pruned[initial_timestamp][s];
            }

            ++initial_timestamp;
        } while (initial_timestamp < num_points && breakage[initial_timestamp - 1]);

        if (initial_timestamp >= num_points)
        {
            return INVALID_STATE;
        }

        BOOST_ASSERT(initial_timestamp > 0);
        --initial_timestamp;

        BOOST_ASSERT(breakage[initial_timestamp] == false);

        return initial_timestamp;
    }
};
} // namespace map_matching
} // namespace engine
} // namespace osrm

#endif // HIDDEN_MARKOV_MODEL