/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#define GRAPHBLAS_LOGGING_LEVEL 0

#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE ewiseadd_matrix_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

/// @todo add better tests with accumulate (only a few using Second)

//****************************************************************************

namespace
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 0, 7},
                                                    {1,  0, 0},
                                                    {3,  6, 9}};

    std::vector<std::vector<double> > eye3x3_dense = {{1, 0, 0},
                                                      {0, 1, 0},
                                                      {0, 0, 1}};

    std::vector<std::vector<double> > twos3x3_dense = {{2, 2, 2},
                                                       {2, 2, 2},
                                                       {2, 2, 2}};

    std::vector<std::vector<double> > zero3x3_dense = {{0, 0, 0},
                                                       {0, 0, 0},
                                                       {0, 0, 0}};

    std::vector<std::vector<double> > ans_twos3x3_dense = {{14,  2,  9},
                                                           { 3,  2,  2},
                                                           { 5,  8, 11}};

    std::vector<std::vector<double> > ans_eye3x3_dense = {{13,  0,  7},
                                                          { 1,  1,  0},
                                                          { 3,  6, 10}};

    std::vector<std::vector<double> > m3x4_dense = {{5, 0, 1, 2},
                                                    {6, 7, 0, 0},
                                                    {4, 5, 0, 1}};

    std::vector<std::vector<double> > m4x3_dense = {{5, 6, 4},
                                                    {0, 7, 5},
                                                    {1, 0, 0},
                                                    {2, 0, 1}};

    std::vector<std::vector<double> > twos4x3_dense = {{2, 2, 2},
                                                       {2, 2, 2},
                                                       {2, 2, 2},
                                                       {2, 2, 2}};

    std::vector<std::vector<double> > twos3x4_dense = {{2, 2, 2, 2},
                                                       {2, 2, 2, 2},
                                                       {2, 2, 2, 2}};

    std::vector<std::vector<double> > ans_twos4x3_dense = {{7, 8,  6},
                                                           {2, 9,  7},
                                                           {3, 2,  2},
                                                           {4, 2,  3}};
    std::vector<std::vector<double> > ans_twos3x4_dense = {{7, 2, 3, 4},
                                                           {8, 9, 2, 2},
                                                           {6, 7, 2, 3}};
}

//****************************************************************************
// Tests without mask
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(3, 3);

    // incompatible input dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(), mA, mB)),
        grb::DimensionException);

    // incompatible output matrix dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(), mB, mB)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_bad_dimensions2)
{
    IndexArrayType i_m1    = {0, 0, 1, 1, 2, 2, 3};
    IndexArrayType j_m1    = {0, 1, 1, 2, 2, 3, 3};
    std::vector<double> v_m1 = {1, 2, 2, 3, 3, 4, 4};
    Matrix<double, DirectedMatrixTag> m1(4, 4);
    m1.build(i_m1, j_m1, v_m1);

    IndexArrayType i_m2    = {0, 0, 1, 1, 1, 2, 2};
    IndexArrayType j_m2    = {0, 1, 0, 1, 2, 1, 2};
    std::vector<double> v_m2 = {2, 2, 1, 4, 4, 4, 6};
    Matrix<double, DirectedMatrixTag> m2(3, 4);
    m2.build(i_m2, j_m2, v_m2);

    Matrix<double, DirectedMatrixTag> m3(4, 4);

    BOOST_CHECK_THROW(
        eWiseAdd(m3, NoMask(), NoAccumulate(),
                 Plus<double>(), m1, m2),
        DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_eWiseadd_matrix_normal)
{
    // Build some sparse matrices.
    IndexArrayType i_mat    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_mat    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_mat = {1, 1, 1, 2, 2, 2, 3, 3, 3, 4};
    Matrix<double, DirectedMatrixTag> mat(4, 4);
    mat.build(i_mat, j_mat, v_mat);

    Matrix<double, DirectedMatrixTag> m3(4, 4);

    IndexArrayType i_answer    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_answer    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_answer = {2, 2, 2, 4, 4, 4, 6, 6, 6, 8};
    Matrix<double, DirectedMatrixTag> answer(4, 4);
    answer.build(i_answer, j_answer, v_answer);

    // Now try simple's ewiseapply.
    eWiseAdd(m3, NoMask(), NoAccumulate(), Plus<double>(), mat, mat);

    BOOST_CHECK_EQUAL(m3, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_eWiseadd_matrix_semiring)
{
    // Build some sparse matrices.
    IndexArrayType i_mat    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_mat    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_mat = {1, 1, 1, 2, 2, 2, 3, 3, 3, 4};
    Matrix<double, DirectedMatrixTag> mat(4, 4);
    mat.build(i_mat, j_mat, v_mat);

    Matrix<double, DirectedMatrixTag> m3(4, 4);

    IndexArrayType i_answer    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j_answer    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v_answer = {2, 2, 2, 4, 4, 4, 6, 6, 6, 8};
    Matrix<double, DirectedMatrixTag> answer(4, 4);
    answer.build(i_answer, j_answer, v_answer);

    eWiseAdd(m3, NoMask(), NoAccumulate(),
             add_monoid(ArithmeticSemiring<double>()),
             mat, mat);

    BOOST_CHECK_EQUAL(m3, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> A(m3x3_dense, 0.);

    // ewise add with dense matrix
    grb::Matrix<double, grb::DirectedMatrixTag> B(twos3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(
        ans_twos3x3_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(3,3);

    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), A, B);
    BOOST_CHECK_EQUAL(Result, Ans);

    // ewise add with sparse matrix
    grb::Matrix<double, grb::DirectedMatrixTag> B2(eye3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans2(
        ans_eye3x3_dense, 0.);
    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), A, B2);
    BOOST_CHECK_EQUAL(Result, Ans2);

    // ewise add with empty matrix
    grb::Matrix<double, grb::DirectedMatrixTag> B3(zero3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans3(
        m3x3_dense, 0.);
    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), A, B3);
    BOOST_CHECK_EQUAL(Result, Ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_semiring_matrix_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> A(m3x3_dense, 0.);

    // ewise add with dense matrix
    grb::Matrix<double, grb::DirectedMatrixTag> B(twos3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(
        ans_twos3x3_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(3,3);

    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        add_monoid(grb::ArithmeticSemiring<double>()),
                        A, B);
    BOOST_CHECK_EQUAL(Result, Ans);

    // ewise add with sparse matrix
    grb::Matrix<double, grb::DirectedMatrixTag> B2(eye3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans2(
        ans_eye3x3_dense, 0.);
    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        add_monoid(grb::ArithmeticSemiring<double>()),
                        A, B2);
    BOOST_CHECK_EQUAL(Result, Ans2);

    // ewise add with empty matrix
    grb::Matrix<double, grb::DirectedMatrixTag> B3(zero3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans3(
        m3x3_dense, 0.);
    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        add_monoid(grb::ArithmeticSemiring<double>()),
                        A, B3);
    BOOST_CHECK_EQUAL(Result, Ans3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_stored_zero_result)
{
    grb::Matrix<double, grb::DirectedMatrixTag> A(m3x3_dense, 0.);

    // Add a stored zero
    A.setElement(1, 2, 0);
    BOOST_CHECK_EQUAL(A.nvals(), 7);

    grb::Matrix<double, grb::DirectedMatrixTag> B2(eye3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans2(
        ans_eye3x3_dense, 0.);
    // Add a stored zero
    Ans2.setElement(1, 2, 0);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(3,3);

    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), A, B2);
    BOOST_CHECK_EQUAL(Result.nvals(), 8);
    BOOST_CHECK_EQUAL(Result, Ans2);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> A(m3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> B(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Result(4,3);

    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Plus<double>(),
                        transpose(A), B);
    BOOST_CHECK_EQUAL(Result, Ans);

    grb::Matrix<double, grb::DirectedMatrixTag> B2(m3x4_dense, 0.);
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(),
                             transpose(A), A)),
        grb::DimensionException);

    grb::Matrix<double, grb::DirectedMatrixTag> Result2(3,3);
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result2,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(),
                             transpose(A), B)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> A(m3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> B(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Result(3, 4);

    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Plus<double>(),
                        A, transpose(B));
    BOOST_CHECK_EQUAL(Result, Ans);

    grb::Matrix<double, grb::DirectedMatrixTag> B2(m3x4_dense, 0.);
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(),
                             B, transpose(B))),
        grb::DimensionException);

    grb::Matrix<double, grb::DirectedMatrixTag> Result2(3,3);
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result2,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(),
                             A, transpose(B))),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_a_and_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> A(m4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> B(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Result(3, 4);

    grb::eWiseAdd(Result,
                        grb::NoMask(),
                        grb::NoAccumulate(),
                        grb::Plus<double>(),
                        transpose(A), transpose(B));
    BOOST_CHECK_EQUAL(Result, Ans);

    grb::Matrix<double, grb::DirectedMatrixTag> B2(m3x4_dense, 0.);
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(),
                             transpose(Ans), transpose(B))),
        grb::DimensionException);

    grb::Matrix<double, grb::DirectedMatrixTag> Result2(3,3);
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result2,
                             grb::NoMask(),
                             grb::NoAccumulate(),
                             grb::Plus<double>(),
                             transpose(A), transpose(B))),
        grb::DimensionException);
}

//****************************************************************************
// Tests using a mask with REPLACE
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_replace_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(twos3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Result(4, 3);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             Mask,
                             grb::NoAccumulate(),
                             grb::Plus<double>(), mA, mB,
                             REPLACE)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_replace_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                       {0, 9, 7},
                                                       {0, 0, 2},
                                                       {0, 0, 0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7, 8,  6},
                                                       {0, 9,  7},
                                                       {0, 0,  2},
                                                       {0, 0,  0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::Second<double>(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_replace_reg_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense); //, 0.);
    BOOST_CHECK_EQUAL(Mask.nvals(), 12);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                       {0, 9, 7},
                                                       {0, 0, 2},
                                                       {0, 0, 0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {0,  9,  7},
                                                       {0,  0,  2},
                                                       {0,  0,  0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::Second<double>(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_replace_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                   {0, 9, 7},
                                                   {0, 0, 2},
                                                   {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        Mask,
                        grb::NoAccumulate(),
                        grb::Plus<double>(), transpose(mA), mB,
                        REPLACE);

    BOOST_CHECK_EQUAL(Result.nvals(), 6);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_replace_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                   {0, 9, 7},
                                                   {0, 0, 2},
                                                   {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        Mask,
                        grb::NoAccumulate(),
                        grb::Plus<double>(), mA, transpose(mB),
                        REPLACE);

    BOOST_CHECK_EQUAL(Result.nvals(), 6);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_replace_a_and_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                   {0, 9, 7},
                                                   {0, 0, 2},
                                                   {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        Mask,
                        grb::NoAccumulate(),
                        grb::Plus<double>(), transpose(mA), transpose(mB),
                        REPLACE);

    BOOST_CHECK_EQUAL(Result.nvals(), 6);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
// Tests using a mask with MERGE semantics
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(twos3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Result(4, 3);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             Mask,
                             grb::NoAccumulate(),
                             grb::Plus<double>(), mA, mB)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::Second<double>(),
                            grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_reg_stored_zero)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense); //, 0.);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            Mask,
                            grb::Second<double>(),
                            grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                   {2,  9,  7},
                                                   {2,  2,  2},
                                                   {2,  2,  2}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        Mask,
                        grb::NoAccumulate(),
                        grb::Plus<double>(), transpose(mA), mB);

    BOOST_CHECK_EQUAL(Result.nvals(), 12);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                   {2,  9,  7},
                                                   {2,  2,  2},
                                                   {2,  2,  2}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        Mask,
                        grb::NoAccumulate(),
                        grb::Plus<double>(), mA, transpose(mB));

    BOOST_CHECK_EQUAL(Result.nvals(), 12);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_masked_a_and_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{1, 1, 1},
                                                    {0, 1, 1},
                                                    {0, 0, 1},
                                                    {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                   {2,  9,  7},
                                                   {2,  2,  2},
                                                   {2,  2,  2}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        Mask,
                        grb::NoAccumulate(),
                        grb::Plus<double>(), transpose(mA), transpose(mB));

    BOOST_CHECK_EQUAL(Result.nvals(), 12);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
// Tests using a complemented mask with REPLACE semantics
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_replace_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(twos3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Result(4, 3);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             grb::complement(Mask),
                             grb::NoAccumulate(),
                             grb::Plus<double>(), mA, mB,
                             REPLACE)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_replace_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                       {0, 9, 7},
                                                       {0, 0, 2},
                                                       {0, 0, 0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            grb::complement(Mask),
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                       {0, 9, 7},
                                                       {0, 0, 2},
                                                       {0, 0, 0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            grb::complement(Mask),
                            grb::Second<double>(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_replace_reg_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense); //, 0.);
    BOOST_CHECK_EQUAL(Mask.nvals(), 12);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                       {0, 9, 7},
                                                       {0, 0, 2},
                                                       {0, 0, 0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            grb::complement(Mask),
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                       {0, 9, 7},
                                                       {0, 0, 2},
                                                       {0, 0, 0}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            grb::complement(Mask),
                            grb::Second<double>(),
                            grb::Plus<double>(), mA, mB,
                            REPLACE);

        BOOST_CHECK_EQUAL(Result.nvals(), 6);
        BOOST_CHECK_EQUAL(Result, Ans);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_replace_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                   {0, 9, 7},
                                                   {0, 0, 2},
                                                   {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        grb::complement(Mask),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), transpose(mA), mB,
                        REPLACE);

    BOOST_CHECK_EQUAL(Result.nvals(), 6);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_replace_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                   {0, 9, 7},
                                                   {0, 0, 2},
                                                   {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        grb::complement(Mask),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), mA, transpose(mB),
                        REPLACE);

    BOOST_CHECK_EQUAL(Result.nvals(), 6);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_replace_a_and_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7, 8, 6},
                                                   {0, 9, 7},
                                                   {0, 0, 2},
                                                   {0, 0, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        grb::complement(Mask),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), transpose(mA), transpose(mB),
                        REPLACE);

    BOOST_CHECK_EQUAL(Result.nvals(), 6);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
// Tests using a complemented mask (with merge semantics)
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(twos3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> Result(4, 3);

    // incompatible Mask-Output dimensions
    BOOST_CHECK_THROW(
        (grb::eWiseAdd(Result,
                             grb::complement(Mask),
                             grb::NoAccumulate(),
                             grb::Plus<double>(), mA, mB)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            grb::complement(Mask),
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            grb::complement(Mask),
                            grb::Second<double>(),
                            grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_reg_stored_zero)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense);//, 0.);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                            grb::complement(Mask),
                            grb::NoAccumulate(),
                            grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }

    {
        grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

        std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                       {2,  9,  7},
                                                       {2,  2,  2},
                                                       {2,  2,  2}};
        grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

        grb::eWiseAdd(Result,
                      grb::complement(Mask),
                      grb::Second<double>(),
                      grb::Plus<double>(), mA, mB);

        BOOST_CHECK_EQUAL(Result.nvals(), 12);
        BOOST_CHECK_EQUAL(Result, Ans);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                   {2,  9,  7},
                                                   {2,  2,  2},
                                                   {2,  2,  2}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                  grb::complement(Mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), transpose(mA), mB);

    BOOST_CHECK_EQUAL(Result.nvals(), 12);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos4x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                   {2,  9,  7},
                                                   {2,  2,  2},
                                                   {2,  2,  2}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                        grb::complement(Mask),
                        grb::NoAccumulate(),
                        grb::Plus<double>(), mA, transpose(mB));

    BOOST_CHECK_EQUAL(Result.nvals(), 12);
    BOOST_CHECK_EQUAL(Result, Ans);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_ewiseadd_matrix_scmp_masked_a_and_b_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(twos3x4_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);

    std::vector<std::vector<double> > mask_dense = {{0, 0, 0},
                                                    {1, 0, 0},
                                                    {1, 1, 0},
                                                    {1, 1, 1}};
    grb::Matrix<double, grb::DirectedMatrixTag> Mask(mask_dense, 0.);

    grb::Matrix<double, grb::DirectedMatrixTag> Result(twos4x3_dense, 0.);

    std::vector<std::vector<double> > ans_dense = {{7,  8,  6},
                                                   {2,  9,  7},
                                                   {2,  2,  2},
                                                   {2,  2,  2}};
    grb::Matrix<double, grb::DirectedMatrixTag> Ans(ans_dense, 0.);

    grb::eWiseAdd(Result,
                  grb::complement(Mask),
                  grb::NoAccumulate(),
                  grb::Plus<double>(), transpose(mA), transpose(mB));

    BOOST_CHECK_EQUAL(Result.nvals(), 12);
    BOOST_CHECK_EQUAL(Result, Ans);
}

BOOST_AUTO_TEST_SUITE_END()


/****************************************************************************
**
** Copyright (C) 2016 The Qt Company Ltd.
** Contact: https://www.qt.io/licensing/
**
** This file is part of the Qt Charts module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:GPL$
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and The Qt Company. For licensing terms
** and conditions see https://www.qt.io/terms-conditions. For further
** information use the contact form at https://www.qt.io/contact-us.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3 or (at your option) any later version
** approved by the KDE Free Qt Foundation. The licenses are as published by
** the Free Software Foundation and appearing in the file LICENSE.GPL3
** included in the packaging of this file. Please review the following
** information to ensure the GNU General Public License requirements will
** be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "view.h"
#include <QtGui/QResizeEvent>
#include <QtWidgets/QGraphicsScene>
#include <QtCharts/QChart>
#include <QtCharts/QLineSeries>
#include <QtCharts/QSplineSeries>
#include <QtWidgets/QGraphicsTextItem>
#include "callout.h"
#include <QtGui/QMouseEvent>
#include <Eigen/Dense>
#include "kalman.h"

View::View(QWidget *parent)
    : QGraphicsView(new QGraphicsScene, parent),
      m_coordX(0),
      m_coordY(0),
      m_chart(0),
      m_tooltip(0)
{
    int n = 9; // Number of states
    int m = 3; // Number of measurements
    unsigned int dt = 5.; // delta
    unsigned int endTime = 60;
    Eigen::MatrixXd A(n, n); // System dynamics matrix (Transformation Matrix)
    Eigen::MatrixXd C(m, n); // Output matrix
    Eigen::MatrixXd Q(n, n); // Process noise covariance
    Eigen::MatrixXd R(m, m); // Measurement noise covariance
    Eigen::MatrixXd P(n, n); // Estimate error covariance
    // Discrete LTI projectile motion, measuring position only
    double tt = 0.5 * dt * dt;
    A << 1, 0, 0, dt,  0,  0, tt,  0,  0,
         0, 1, 0,  0, dt,  0,  0, tt,  0,
         0, 0, 1,  0,  0, dt,  0,  0, dt,
         0, 0, 0,  1,  0,  0, dt,  0,  0,
         0, 0, 0,  0,  1,  0,  0, dt,  0,
         0, 0, 0,  0,  0,  1,  0,  0, dt,
         0, 0, 0,  0,  0,  0,  1,  0,  0,
         0, 0, 0,  0,  0,  0,  0,  1,  0,
         0, 0, 0,  0,  0,  0,  0,  0,  1;
    C << 1, 0, 0, 0, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0, 0, 0, 0,
         0, 0, 1, 0, 0, 0, 0, 0, 0;
    // Reasonable covariance matrices
    Q << tt, .05, .05, .0, .0, .0, .0, .0, .0,
         .05, tt, .05, .0, .0, .0, .0, .0, .0,
         .05, .5, tt, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0;
    R << 5, 0, 0,
         0, 5, 0,
         0, 0, 5;
    P << 1.3, 2, 3, 1, 1, 1, 1, 1, 1,
         4, 1.5, 5, 1, 1, 1, 1, 1, 1,
         6, 7, 9, 1, 1, 1, 1, 1, 1,
         1, 1, 1, 1.6, 1, 1, 1, 1, 1,
         1, 1, 1, 1, .9, 1, 1, 1, 1,
         1, 1, 1, 1, 1, .2, 1, 1, 1,
         1, 1, 1, 1, 1, 1, .1, 1, 1,
         1, 1, 1, 1, 1, 1, 1, 1.54, 1,
         1, 1, 1, 1, 1, 1, 1, 1, 1.2;
    KalmanFilter k(dt, A, C, Q, R, P);
    Eigen::VectorXd s0(n), y(m);
    s0 <<  7.0,  8.0,  9.0, 1.0,  2.0,  3.0, 0.33, 0.66, 0.99;
    //      px,   py,   pz,  vx,   vy,   vz,   ax,   ay,   az
    k.init(0, s0);

    setDragMode(QGraphicsView::NoDrag);
    setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
    setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);

    // chart
    m_chart = new QChart;
    m_chart->setMinimumSize(640, 480);
    m_chart->setTitle("Distance to Origin (m) x Instant (s)");
    //m_chart->legend()->hide();
    m_chart->legend()->setVisible(true);
    m_chart->legend()->setAlignment(Qt::AlignBottom);

    QLineSeries *series = new QLineSeries;
    series->setName("Observed");
    QSplineSeries *series2 = new QSplineSeries;
    series2->setName("Predicted");
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(n, n);
    for (unsigned int t = 0; t <= endTime; t += dt)
    {
        Eigen::VectorXd u = Eigen::VectorXd::Random(n);
        Eigen::VectorXd y_ = A * s0 + B * u;
        Eigen::Vector3d v;
        v << y_[0], y_[1], y_[2];
        k.update(v);
        series->append(t, v.norm());// distance to origin
        series2->append(t, k.state().norm()); // distance to origin
        s0 = y_;
    }
    m_chart->addSeries(series);
    m_chart->addSeries(series2);

    m_chart->createDefaultAxes();
    m_chart->setAcceptHoverEvents(true);

    setRenderHint(QPainter::Antialiasing);
    scene()->addItem(m_chart);

    m_coordX = new QGraphicsSimpleTextItem(m_chart);
    m_coordX->setPos(m_chart->size().width()/2 - 50, m_chart->size().height());
    m_coordX->setText("X: ");
    m_coordY = new QGraphicsSimpleTextItem(m_chart);
    m_coordY->setPos(m_chart->size().width()/2 + 50, m_chart->size().height());
    m_coordY->setText("Y: ");

    connect(series, &QLineSeries::clicked, this, &View::keepCallout);
    connect(series, &QLineSeries::hovered, this, &View::tooltip);

    connect(series2, &QSplineSeries::clicked, this, &View::keepCallout);
    connect(series2, &QSplineSeries::hovered, this, &View::tooltip);

    this->setMouseTracking(true);
}

void View::resizeEvent(QResizeEvent *event)
{
    if (scene()) {
        scene()->setSceneRect(QRect(QPoint(0, 0), event->size()));
         m_chart->resize(event->size());
         m_coordX->setPos(m_chart->size().width()/2 - 50, m_chart->size().height() - 20);
         m_coordY->setPos(m_chart->size().width()/2 + 50, m_chart->size().height() - 20);
         const auto callouts = m_callouts;
         for (Callout *callout : callouts)
             callout->updateGeometry();
    }
    QGraphicsView::resizeEvent(event);
}

void View::mouseMoveEvent(QMouseEvent *event)
{
    m_coordX->setText(QString("X: %1").arg(m_chart->mapToValue(event->pos()).x()));
    m_coordY->setText(QString("Y: %1").arg(m_chart->mapToValue(event->pos()).y()));
    QGraphicsView::mouseMoveEvent(event);
}

void View::keepCallout()
{
    m_callouts.append(m_tooltip);
    m_tooltip = new Callout(m_chart);
}

void View::tooltip(QPointF point, bool state)
{
    if (m_tooltip == 0)
        m_tooltip = new Callout(m_chart);

    if (state) {
        m_tooltip->setText(QString("X: %1 \nY: %2 ").arg(point.x()).arg(point.y()));
        m_tooltip->setAnchor(point);
        m_tooltip->setZValue(11);
        m_tooltip->updateGeometry();
        m_tooltip->show();
    } else {
        m_tooltip->hide();
    }
}


// A note to a potential future debugger, the -fno-use-cxa-atexit is needed solely for lli (the llvm interpreter) to work
//   Actually compiling it works fine, and moreover, the one place atexit is used is nowhere near the enzyme code

// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -mllvm -force-vector-width=1 -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O3 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O2 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
// RUN: %clang++ -Xclang -new-struct-path-tbaa -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O1 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
//   note not doing O0 below as to ensure we get tbaa
// RUN: %clang++ -Xclang -new-struct-path-tbaa -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O1 -Xclang -disable-llvm-optzns %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O3 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O2 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -
// Note the below ends up with a memcpy from undefined memory data for type analysis to handle
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O1 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -
// TODO: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O0 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -

#include "test_utils.h"

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_NO_EXCEPTIONS
#include <iostream>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include <boost/throw_exception.hpp>
void boost::throw_exception(std::exception const & e){
    //do nothing
}

using namespace std;
using namespace boost::numeric::odeint;

#include <stdio.h>

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

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    const double a = 1.2;
    dxdt[0] = -a * x[0];
}

double foobar(double t=10.0) {
    state_type x = { 1.0 }; // initial conditions

    typedef controlled_runge_kutta< runge_kutta_dopri5< state_type , typename state_type::value_type , state_type , double > > stepper_type;
    integrate_const( stepper_type(), lorenz , x , 0.0 , t, t/100 );

    //printf("final result t=%f x(t)=%f, exp(-1.2* t)=%f\n", t, x[0], exp(- 1.2 * t));
    return x[0];
}

extern "C" {
extern double __enzyme_autodiff(void*, double);
}

int main(int argc, char **argv)
{
    for(int i=1; i<=100; i++) {
        double t=i/10.;
        double res = __enzyme_autodiff((void*)foobar, t);
        double realanswer = -1.2*exp(-1.2*t);
        printf("t=%f d/dt(exp(-1.2*t))=%f, -1.2*exp(-1.2*t)=%f\n", t, res, realanswer);
        // see if approximation is within 10%
        APPROX_EQ(res, realanswer, max(fabs(realanswer)/10., 2.0e-5) );
    }
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    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
 *      Vallado, D.A. Fundamentals of Astrodynamics and Applications website.
 *        URL http://www.smad.com/vallado, 2001. Last accessed: 25-07-2012.
 *      Wakker, K.F. Astrodynamics I, reader of course AE4874 I. Delft University of Technology,
 *        2007.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>

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

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/clohessyWiltshirePropagator.h"

namespace tudat
{
namespace unit_tests
{

//! Test suite for Clohessy-Wiltshire propagation.
BOOST_AUTO_TEST_SUITE( test_clohessyWiltshirePropagation )

// Testcase 1: propagation of full state in low-Earth orbit.
// This test benchmarks the propagation of a full state against external data.
BOOST_AUTO_TEST_CASE( test_ClohessyWiltshirePropagation_fullState )
{
    // Set central body gravitational parameter [m^3 s^-2].
    // In this case: central body is Earth.
    const double centralBodyGravitationalParameter1 = 3.986004418e14;

    // Set propagation duration [s].
    // In this case: propagation duration is 30 minutes.
    const double propagationDuration1 = 1800.0;

    // Set reference orbit radius [m].
    // In this case: reference orbit is at 400 km altitude above Earth.
    const double referenceOrbitRadius1 = 6.778137e6;

    // Set initial state [m], [m], [m], [m/s], [m,s], [m/s].
    // In this case: arbitrary non-zero distances and velocities.
    const Eigen::Vector6d initialState1 =
            ( Eigen::Vector6d( ) << 45.0, 37.0, 12.0, 0.08,
              0.03, 0.01 ).finished( );

    // Calculate final state according to Tudat function.
    const Eigen::Vector6d computedFinalState1
            = basic_astrodynamics::propagateClohessyWiltshire(
                initialState1,
                propagationDuration1,
                centralBodyGravitationalParameter1,
                referenceOrbitRadius1 );

    // Set final state according to the MATLAB routine "hillsr" from Vallado [2001].
    const Eigen::Vector6d expectedFinalState1 =
            ( Eigen::Vector6d( ) << 3.806450080201250e2,
              -5.437424675454679e2, 2.509547637285142,
              1.541620605755606e-1, -7.294751390499470e-1,
              -1.662099488431618e-2 ).finished( );

    // Check if computed final state matches expected state.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedFinalState1, expectedFinalState1, 1.0e-14 );
}

// Testcase 2: pure harmonic relative motion in low-Earth orbit.
//   According to Wakker [2007] pure harmonic relative motion occurs if two conditions are met:
//   (1) initialRadialVelocity = 0.5 * meanAngularMotion * initialAlongTrackPosition.
//   (2) initialAlongTrackVelocity = -2.0 * meanAngularMotion * initialRadialPosition.
//   Therefore the final state after one orbital revolution must be exactly equal to the initial
//   state. This testcase verifies this.
BOOST_AUTO_TEST_CASE( test_ClohessyWiltshirePropagation_harmonicMotion )
{
    // Set central body gravitational parameter [m^3 s^-2].
    // In this case: central body is Earth.
    const double centralBodyGravitationalParameter2 = 3.986004418e14;

    // Set reference orbit radius [m].
    // In this case: reference orbit is at 400 km altitude above Earth.
    const double referenceOrbitRadius2 = 6.778137e6;

    // Calculate mean angular motion of reference orbit [rad].
    const double meanAngularMotion =
            std::sqrt( centralBodyGravitationalParameter2
                       / ( referenceOrbitRadius2 * referenceOrbitRadius2
                           * referenceOrbitRadius2 ) );

    // Set propagation duration [s].
    // In this case: orbital period of the reference orbit.
    const double propagationDuration2 = 2.0 * mathematical_constants::PI
            / meanAngularMotion;

    // Set initial state [m], [m], [m], [m/s], [m,s], [m/s].
    // In this case: arbitrary values for initial positions and initialCrossTrackVelocity. The
    // initialRadialVelocity and initialAlongTrackVelocity are set to achieve harmonic motion.
    const Eigen::Vector6d initialState2 =
            ( Eigen::Vector6d( )
              << 34.0, 49.0 , 17.0,
              0.5 * meanAngularMotion * 49.0,
              -2.0 * meanAngularMotion * 34.0, 0.04 ).finished( );

    // Calculate final state according to Tudat function.
    const Eigen::Vector6d computedFinalState2
            = basic_astrodynamics::propagateClohessyWiltshire(
                initialState2,
                propagationDuration2,
                centralBodyGravitationalParameter2,
                referenceOrbitRadius2 );

    // Check if computed final state matches the initial state.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( initialState2, computedFinalState2, 1.0e-14 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_math_multidimintegrator_hpp
#define quantlib_math_multidimintegrator_hpp

#include <vector>

#include <boost/function.hpp>
#include <boost/bind.hpp>

#include <ql/types.hpp>
#include <ql/errors.hpp>
#include <ql/math/integrals/integral.hpp>

namespace QuantLib {

    /*! \brief Integrates a vector or scalar function of vector domain.

        Uses a collection of arbitrary 1D integrators along each of the
        dimensions. A template recursion along dimensions avoids calling depth
        test or virtual functions.\par
        This class generalizes to an arbitrary number of dimensions the
        functionality in class TwoDimensionalIntegral
    */
    class MultidimIntegral {
    public:
        explicit MultidimIntegral(
            const std::vector<boost::shared_ptr<Integrator> >& integrators);

        // scalar variant:
        /*!
            @param f Integrand function.
            @param a Lower integration limit domain for each dimension.
            @param b Upper integration limit domain for each dimension.
        */
        Real operator()(
            const boost::function<Real (const std::vector<Real>&)>& f,
            const std::vector<Real>& a,
            const std::vector<Real>& b) const
        {
            QL_REQUIRE((a.size()==b.size())&&(b.size()==integrators_.size()),
                "Incompatible integration problem dimensions");
            return integrationLevelEntries_[integrators_.size()-1](f, a, b);
        }
        // to do: write std::vector<Real> operator()(...) version

    private:
        static const Size maxDimensions_ = 15;

        /* Here is the tradeoff; this is avoiding the dimension limits checks
        during integration at the price of these asignments during construction.
        Explicit template instantiation is of no use, an object is needed
        (notice 'this' is needed for the asignment.)
        If not all the dimensions up the maximum number are used the waste goes
        into storage of the functions (in fact only one is used)
        */
        template<Size depth>
        void spawnFcts() const;
        // Splits the integration in cross-sections per dimension.
        template<int T_N>
        Real vectorBinder (
            const boost::function<Real (const std::vector<Real>&)>& f,
            Real z,
            const std::vector<Real>& a,
            const std::vector<Real>& b) const ;
        // actual integration of dimension nT
        template<int nT>
        Real integrate(
            const boost::function<Real (const std::vector<Real>&)>& f,
            const std::vector<Real>& a,
            const std::vector<Real>& b) const;
    private:
        const std::vector<boost::shared_ptr<Integrator> > integrators_;

        /* typedef (const boost::function<Real
            (const std::vector<Real>&arg1)>&arg2) integrableFunctType;
        */

        /* vector of, functions returning reals And taking as argument:
        1.- a const ref to a function taking vectors
        2.- a vector, 3. another vector. typedefs eventually...
         at first sight this might look like mimicking a virtual table, it isnt
         that. The reason is to be able to select the correct integration
         dimension at run time, this can not be done before because of the
         template argument restriction to be constant known at compilation.
        */
        mutable std::vector<boost::function<Real (//<- members: integrate<N>
            // integrable function:
            const boost::function<Real (const std::vector<Real>&)>&,
            const std::vector<Real>&, //<- a
            const std::vector<Real>&) //<- b
            > >
            integrationLevelEntries_;

        /* One can avoid the passing around of the ct refs to a and b but the
        price is to keep a copy of them (they are unknown at construction time)
         On the other hand the vector integration variable has to be created.*/
        mutable std::vector<Real> varBuffer_;

    };

    // spez last call/dimension
    template<>
    Real inline MultidimIntegral::vectorBinder<0> (
        const boost::function<Real (const std::vector<Real>&)>& f,
        Real z,
        const std::vector<Real>& a,
        const std::vector<Real>& b) const
    {
        varBuffer_[0] = z;
        return f(varBuffer_);
    }

    template<>
    void inline MultidimIntegral::spawnFcts<1>() const {
        integrationLevelEntries_[0] =
            boost::bind(&MultidimIntegral::integrate<0>, this, _1, _2, _3);
    }

	template<int nT>
	inline Real MultidimIntegral::integrate(
        const boost::function<Real (const std::vector<Real>&)>& f,
        const std::vector<Real>& a,
        const std::vector<Real>& b) const
    {
        return
            (*integrators_[nT])(
                boost::bind(&MultidimIntegral::vectorBinder<nT>, this, f,
                    _1, boost::cref(a), boost::cref(b)), a[nT], b[nT]);
    }

    template<int T_N>
    inline Real MultidimIntegral::vectorBinder (
        const boost::function<Real (const std::vector<Real>&)>& f,
        Real z,
        const std::vector<Real>& a,
        const std::vector<Real>& b) const
    {
        varBuffer_[T_N] = z;
        return integrate<T_N-1>(f, a, b);
    }

    template<Size depth>
    void MultidimIntegral::spawnFcts() const {
        integrationLevelEntries_[depth-1] =
          boost::bind(&MultidimIntegral::integrate<depth-1>, this,
            _1, _2, _3);
        spawnFcts<depth-1>();
    }

}

#endif


//  (C) Copyright Raffi Enficiaud 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://www.boost.org/libs/test for the library home page.
//
//  snippets included in the dataset documentation
// ***************************************************************************

#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>

#include <boost/test/data/monomorphic.hpp>

#include <boost/test/data/monomorphic/generators/xrange.hpp>
#include <boost/test/data/monomorphic/zip.hpp>

// generation of a sequence/range
namespace data=boost::unit_test::data;

//[snippet_dataset1_1
BOOST_DATA_TEST_CASE( test_case_arity1_implicit, data::xrange(5) )
{
  BOOST_TEST((sample <= 4 && sample >= 0));
}
//]

//[snippet_dataset1_2
BOOST_DATA_TEST_CASE( test_case_arity1, data::xrange(5), my_var )
{
  BOOST_TEST((my_var <= 4 && my_var >= 0));
}
//]

//[snippet_dataset1_3
BOOST_DATA_TEST_CASE( test_case_arity2, data::xrange(2) ^ data::xrange(5), apples, potatoes)
{
  BOOST_TEST((apples <= 1 && apples >= 0));
  BOOST_TEST((potatoes <= 4 && potatoes >= 0));
}
//]

//[snippet_dataset1_4
std::vector<int> generate()
{
  std::vector<int> out;
  out.push_back(3);
  out.push_back(1);
  out.push_back(7);
  return out;
}

const std::vector<int> v = generate();
BOOST_DATA_TEST_CASE( test_case_3, data::make(v), var1)
{
  BOOST_TEST_MESSAGE(var1);
  BOOST_CHECK(true);
}
//]

#include <vector>
#include <map>

std::vector<int> generate_vector()
{
  std::vector<int> out;
  out.push_back(3);
  out.push_back(1);
  out.push_back(7);
  return out;
}

typedef std::pair<const int, int> pair_int;
BOOST_TEST_DONT_PRINT_LOG_VALUE( pair_int )

const std::vector<int> v = generate_vector();
BOOST_DATA_TEST_CASE( test_case_1, data::make(v), var1)
{
  std::cout << var1 << std::endl;
  BOOST_TEST(true);
}

std::map<int, int> generate_map()
{
  std::vector<int> v = generate_vector();
  std::map<int, int> out;
  for(std::size_t i = 0; i < v.size(); i++)
  {
    out[v[i]] = (i * 7) % 19;
  }
  return out;
}

const std::map<int, int> m = generate_map();
BOOST_DATA_TEST_CASE( test_case_2, data::make(m), var1)
{
  std::cout << var1.first << " -- " << var1.second << std::endl;
  BOOST_TEST(true);
}


#ifndef __FOLDING_ESTIMATION_ALGORITHM__
#define __FOLDING_ESTIMATION_ALGORITHM__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <pr2_algorithms/algorithm_base.hpp>
#include <limits>
#include <stdexcept>

namespace manipulation_algorithms{

  /**
    Class that implements the contact point estimator
    used in the folding assembly problem.
  **/
  class FoldingAssemblyEstimator : public AlgorithmBase
  {
  public:
    FoldingAssemblyEstimator();
    ~FoldingAssemblyEstimator();

    /**
      Computes an estimate of the free vector that represents a rigidly grasped object.

      Makes use of the process model \f$\dot{\mathbf{r}} = \mathbf{S}(\boldsymbol{\omega_1}}) \mathbf{r}\f$
      and of the observation model \f$\boldsymbol{\tau} = -\mathbf{S}(\mathbf{f})\mathbf{r}\f$.

      @param omega The end-effector angular velocity.
      @param force The measured contact force.
      @param torque The measured torque at the end-effector wrist.
      @param dt Elapsed time between calls (in seconds).
      @return A free vector representing the estimate of the vector that connects the wrench measurement point to the contact point.
    **/
    Eigen::Vector3d estimate(const Eigen::Vector3d &omega, const Eigen::Vector3d &force, const Eigen::Vector3d &torque, const double dt);

    /**
      Initialize the estimator

      @param r The initial virtual stick estimate.
    **/
    void initialize(const Eigen::Vector3d &r);

    virtual bool getParams(const ros::NodeHandle &n);

  private:
    Eigen::Vector3d r_;
    Eigen::Matrix3d P_, Q_, R_;
  };
}
#endif


#include "ParallelEigenvectorsEvaluator.hh"

#include <Eigen/Geometry>

#include <boost/algorithm/cxx11/any_of.hpp>
#include <boost/range/algorithm/min_element.hpp>
#include <boost/range/algorithm/max_element.hpp>

#include <vector>

using namespace cpp_utils;

namespace tl
{

template <typename T, std::size_t... Degrees>
using TPBT = TensorProductBezierTriangle<T, double, Degrees...>;

std::array<TPBT<double, 1, 2>, 6>
parallelEigenvectorsCoeffs(const TensorInterp& s,
                           const TensorInterp& t,
                           const Triangle& r)
{
    using Coords = TPBT<double, 1, 2>::Coords;

    // (T * r) x r
    auto eval_ev =
            [&](const Coords& coords, const TensorInterp& t, int i) -> double {
        auto rv = r(coords.tail<3>());
        auto tv = t(coords.head<3>());
        return (tv * rv).cross(rv)[i];
    };

    auto eval1 = [&](const Coords& coords) { return eval_ev(coords, s, 0); };
    auto eval2 = [&](const Coords& coords) { return eval_ev(coords, s, 1); };
    auto eval3 = [&](const Coords& coords) { return eval_ev(coords, s, 2); };

    auto eval4 = [&](const Coords& coords) { return eval_ev(coords, t, 0); };
    auto eval5 = [&](const Coords& coords) { return eval_ev(coords, t, 1); };
    auto eval6 = [&](const Coords& coords) { return eval_ev(coords, t, 2); };

    return {TPBT<double, 1, 2>{eval1},
            TPBT<double, 1, 2>{eval2},
            TPBT<double, 1, 2>{eval3},
            TPBT<double, 1, 2>{eval4},
            TPBT<double, 1, 2>{eval5},
            TPBT<double, 1, 2>{eval6}};
}

using PEVE = ParallelEigenvectorsEvaluator;

PEVE::ParallelEigenvectorsEvaluator(const DoubleTri& tri,
                                    const TensorInterp& s,
                                    const TensorInterp& t,
                                    const Options& opts)
        : _tri(tri),
          _target_funcs(parallelEigenvectorsCoeffs(s, t, tri.dir_tri)),
          _opts(opts)
{
}

std::array<PEVE, 4> PEVE::split() const
{
    if(_last_split_dir)
    {
        return split<0>();
    }
    return split<1>();
}

Result PEVE::eval()
{
    // Check if any of the error components can not become zero in the
    // current subdivision triangles
    auto has_nonzero = boost::algorithm::any_of(
            _target_funcs,
            [](const auto& c) { return sameSign(c.coefficients()) != 0; });

    // Discard triangles if no roots can occur inside
    if(has_nonzero)
    {
        return Result::Discard;
    }

    // Compute upper bound for target function
    auto max_error = abs_max_upper_bound(_target_funcs);

    if(max_error < _opts.tolerance)
    {
        return Result::Accept;
    }

    return Result::Split;
}

double PEVE::error() const
{
    return upper_bound_norm(_target_funcs);
}

double PEVE::condition() const
{
    auto gradients = std::vector<Eigen::Vector4d>{};
    gradients.reserve(_target_funcs.size());

    auto min_cos = 0.;

    auto center0 = (TensorProductDerivativeType_t<0, double, double, 1, 2>::
                            Coords::Ones()
                    / 3.).eval();
    auto center1 = (TensorProductDerivativeType_t<1, double, double, 1, 2>::
                            Coords::Ones()
                    / 3.).eval();

    for(const auto& poly: _target_funcs)
    {
        auto deriv0 = derivatives<0>(poly);
        auto deriv1 = derivatives<1>(poly);
        auto grad = Eigen::Vector4d(deriv0[0](center0),
                                    deriv0[1](center0),
                                    deriv1[0](center1),
                                    deriv1[1](center1));
        grad.normalize();
        for(const auto& g: gradients)
        {
            min_cos = std::min(min_cos, std::abs(grad.dot(g)));
        }
        gradients.push_back(grad);
    }

    auto angle = std::acos(min_cos);
    return min_cos / std::sin(angle);
}

double distance(const PEVE& t1, const PEVE& t2)
{
    return distance(t1.tris(), t2.tris());
}

bool operator==(const PEVE& t1, const PEVE& t2)
{
    return t1._tri == t2._tri && t1._target_funcs == t2._target_funcs
           && t1._last_split_dir == t2._last_split_dir
           && t1._split_level == t2._split_level && t1._opts == t2._opts;
}

bool operator!=(const PEVE& t1, const PEVE& t2)
{
    return !(t1 == t2);
}

}// namespace tl


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

#include <cmath>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/fabs.hpp>
#include <boost/math/tools/config.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
void test()
{
    static_assert(boost::math::ccmath::abs(T(3)) == 3);
    static_assert(boost::math::ccmath::abs(T(-3)) == 3);
    static_assert(boost::math::ccmath::abs(T(-0)) == 0);
    static_assert(boost::math::ccmath::abs(-std::numeric_limits<T>::infinity()) == std::numeric_limits<T>::infinity());

    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(boost::math::ccmath::abs(-std::numeric_limits<T>::quiet_NaN()) != std::numeric_limits<T>::quiet_NaN());
    }
}

template <typename T>
void gpp_test()
{
    static_assert(std::sin(T(0)) == 0);

    constexpr T sin_1 = boost::math::ccmath::abs(std::sin(T(-1)));
    static_assert(sin_1 > 0);
    static_assert(sin_1 == T(0.8414709848078965066525l));
}

template <typename T>
void fabs_test()
{
    static_assert(boost::math::ccmath::fabs(T(3)) == 3);
    static_assert(boost::math::ccmath::fabs(T(-3)) == 3);
    static_assert(boost::math::ccmath::fabs(T(-0)) == 0);
    static_assert(boost::math::ccmath::fabs(-std::numeric_limits<T>::infinity()) == std::numeric_limits<T>::infinity());

    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(boost::math::ccmath::fabs(-std::numeric_limits<T>::quiet_NaN()) != std::numeric_limits<T>::quiet_NaN());
    }
}

// Only test on platforms that provide BOOST_MATH_IS_CONSTANT_EVALUATED
#ifndef BOOST_MATH_NO_CONSTEXPR_DETECTION
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #if defined(BOOST_HAS_FLOAT128) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
    test<boost::multiprecision::float128>();
    #endif

    test<int>();
    test<long>();
    test<long long>();
    test<std::int32_t>();
    test<std::int64_t>();

    // Types that are convertible to int
    test<short>();
    test<char>();

    // fabs
    fabs_test<float>();
    fabs_test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    fabs_test<long double>();
    #endif

    #if defined(BOOST_HAS_FLOAT128) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
    fabs_test<boost::multiprecision::float128>();
    #endif

    // Tests using glibcxx extensions that allow for some constexpr cmath
    #if __GNUC__ >= 10
    gpp_test<float>();
    gpp_test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    gpp_test<long double>();
    #endif

    #endif // glibcxx tests

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


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

#include <boost/hana/integral_constant.hpp>

#include <boost/hana/tuple.hpp>

#include <laws/enumerable.hpp>
#include <laws/group.hpp>
#include <laws/integral_domain.hpp>
#include <laws/monoid.hpp>
#include <laws/ring.hpp>
using namespace boost::hana;

int main() {
    auto int_constants = tuple_c<int, -10, -2, 0, 1, 3, 4>;

    //////////////////////////////////////////////////////////////////////////
    // Enumerable, Monoid, Group, Ring, IntegralDomain
    //////////////////////////////////////////////////////////////////////////
    {
        // operators
        static_assert(has_operator<IntegralConstant<int>, decltype(plus)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(minus)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(negate)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(mult)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(quot)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(rem)>{}, "");

        // laws
        test::TestEnumerable<IntegralConstant<int>>{int_constants};
        test::TestMonoid<IntegralConstant<int>>{int_constants};
        test::TestGroup<IntegralConstant<int>>{int_constants};
        test::TestRing<IntegralConstant<int>>{int_constants};
        test::TestIntegralDomain<IntegralConstant<int>>{int_constants};
    }
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/inverse_gamma.hpp>

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

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

#include <dpMM/distribution.hpp>
#include <dpMM/vmf.hpp>

using namespace Eigen;
using std::endl;
using std::cout;
using std::vector;

template<typename T>
class vMFpriorFull : public Distribution<T>
{
public:
  vMFpriorFull(const Matrix<T,Dynamic,1>& m0, T t0, T a0, T b0, boost::mt19937
    *pRndGen);
  vMFpriorFull(const vMFpriorFull<T>& vmfPriorFull);
  ~vMFpriorFull();

//  vMFpriorFull<T> posterior(const Matrix<T,Dynamic,Dynamic>& x, const
//      VectorXu& z, uint32_t k);
//  vMFpriorFull<T> posterior() const;

  void resetSufficientStatistics();
  void getSufficientStatistics(const Matrix<T,Dynamic,Dynamic> &x,
    const VectorXu& z, uint32_t k);

  vMF<T> sample();
  vMF<T> sampleFromPosterior(const vMF<T>& vmf);

//  T logPdf(const vMF<T>& vmf) const;
//  T logPdfMarginalized() const; // log pdf of SS under NIW prior
//  T logPdfUnderPriorMarginalizedMerged(const NIW<T>& other) const;

  vMF<T> vmf0_; // prior on the mean
  T a0_;
  T b0_;

  uint32_t D_;
  // sufficient statistics
  Matrix<T,Dynamic,1> xSum_;
  T count_;

  boost::mt19937 *pRndGen_;

private:
  boost::uniform_01<> unif_;

  T concentrationLogPdf(const T tau, const T a, const T b) const;
  T sampleConcentration(const T a, const T b, const T TT=10);
};
// -----------------------------------------------------------------------

template<typename T>
vMFpriorFull<T>::vMFpriorFull(const Matrix<T,Dynamic,1>& m0, T t0, T a0, T
    b0, boost::mt19937 *pRndGen)
: Distribution<T>(pRndGen), vmf0_(m0,t0,pRndGen), a0_(a0), b0_(b0), D_(m0.rows()),
xSum_(Matrix<T,Dynamic,1>::Zero(m0.rows())), count_(0.), pRndGen_(pRndGen)
{
//cout<<m0<<endl; cout<<m0.rows()<<endl;cout<<uint32_t(m0.rows())<<endl;
};

template<typename T>
vMFpriorFull<T>::vMFpriorFull(const vMFpriorFull<T>& vmfPriorFull)
: Distribution<T>(vmfPriorFull.pRndGen_), vmf0_(vmfPriorFull.vmf0_),
a0_(vmfPriorFull.a0_), b0_(vmfPriorFull.b0_), D_(vmfPriorFull.D_),
xSum_(vmfPriorFull.xSum_), count_(vmfPriorFull.count_),
pRndGen_(vmfPriorFull.pRndGen_)
{};

template<typename T>
vMFpriorFull<T>::~vMFpriorFull()
{
};

//template<typename T>
//vMFpriorFull<T> vMFpriorFull<T>::posterior(const
//    Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z, uint32_t k)
//{
//  getSufficientStatistics(x,z,k);
//  return posterior();
//};
//
//template<typename T>
//vMFpriorFull<T> vMFpriorFull<T>::posterior() const
//{
//  Matrix<T,Dynamic,1> xi = t0_*m0_ +;
//  const T a = a0_ + count_;
//  const T b = b0_ +
//  return vMFpriorFull(xSum_, count_);
//};

template<typename T>
void vMFpriorFull<T>::resetSufficientStatistics()
{
  xSum_.setZero(D_);
  count_ = 0;
};

template<typename T>
void vMFpriorFull<T>::getSufficientStatistics(const
    Matrix<T,Dynamic,Dynamic> &x, const VectorXu& z, uint32_t k)
{
  this->resetSufficientStatistics();
  // TODO: be carefull here when parallelizing since all are writing to the same
  // location in memory
#pragma omp parallel for
  for (int32_t i=0; i<z.size(); ++i)
  {
    if(z(i) == k)
    {
#pragma omp critical
      {
        xSum_ += x.col(i);
        count_++;
      }
    }
  }
#ifndef NDEBUG
  cout<<" -- updating ss "<<count_<<endl;
  cout<<"xSum="<<xSum_.transpose()<<endl;
  posterior().print();
#endif
//  cout<<"SS: "<<count_<<" "<<xSum_.transpose()<<endl;
};

template<typename T>
vMF<T> vMFpriorFull<T>::sample()
{
  const T tau = sampleConcentration(a0_,b0_);
  const Matrix<T,Dynamic,1> mu = vmf0_.sample();
  return vMF<T>(mu, tau,pRndGen_);
};

template<typename T>
vMF<T> vMFpriorFull<T>::sampleFromPosterior(const vMF<T>& vmf)
{
  vMF<T> vmfPost (vmf);
  vMF<T> vmfPostPrev (vmf);
  T logPost = -FLT_MAX;
  T logPostPrev = logPost;
  // Gibbs sampler for concentration and mean
  for(uint32_t j=0;j<10; ++j)
  {
    // posterior concentration prior parameters
    const T a = a0_ + count_;
    const T b = b0_ + vmfPost.mu().transpose()*xSum_;
//    cout<<"vMFpriorFull::sampleFromPosterior: a="<<a<<" b="<<b<<endl;
    // sample concentration form this posterior
    const T tau = sampleConcentration(a,b);
//    cout<<"vMFpriorFull::sampleFromPosterior: tau="<<tau<<endl;
    // posterior mean (m_N) and concentration (t_N) for vMF
    Matrix<T,Dynamic,1> m_N = vmf0_.tau()*vmf0_.mu() + tau*xSum_;
    T t_N = m_N.norm();
    m_N /= t_N;
//    cout<<"vMFpriorFull::sampleFromPosterior: t_N="<<t_N<<" m_N="<<m_N.transpose()<<endl;
    // sample mean mu from posterior vMF
    Matrix<T,Dynamic,1> mu = vMF<T>(m_N,t_N, pRndGen_).sample();
//    cout<<"vMFpriorFull::sampleFromPosterior: mu="<<mu.transpose()<<endl;
    logPost = (tau*b + tau*mu.transpose()*xSum_ + vmf0_.tau()*vmf0_.mu().transpose()*mu);
    cout<<"@"<<j<<" cost "<<logPost<<" "<<(logPost - logPostPrev)<<endl;
    // return sampled posterior vMF
    vmfPost = vMF<T>(mu, tau, pRndGen_);
//    if((logPost - logPostPrev)<0. ) break;
    logPostPrev = logPost;
    vmfPostPrev = vmfPost;
  }
  return vmfPostPrev;
};

template<typename T>
T vMFpriorFull<T>::concentrationLogPdf(const T tau, const T a, const T b) const
{
  // modified bessel function of the first kind
  // http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/mbessel.html
  const T DD = static_cast<T>(D_);
//  cout<<"vMFpriorFull:: bessel: D="<<DD<<" "<<(DD/2.-1.)<<" tau="<<tau<<endl;
  return a*((DD/2. -1.)*log(tau)
    - (DD/2.)*log(2.*M_PI)
    - logBesselI(DD/2. -1.,tau))
    + tau*b;
};

template<typename T>
T vMFpriorFull<T>::sampleConcentration(const T a, const T b, const T TT)
{
  // slice sampler for concentration paramter tau
  const T w = 0.1;  // width for expansions of search region
  T tau = 0.11;      // arbitrary starting point
  for(int32_t t=0; t<TT; ++t)
  {
    const T yMax = concentrationLogPdf(tau,a,b);
    const T y = log(unif_(*pRndGen_)) + yMax;
//    cout<<"vMFpriorFull::sampleConcentration: yMax="<<yMax<<" y="<<y<<endl;
    T tauMin = tau-w;
    T tauMax = tau+w;
    while (tauMin >=0. && concentrationLogPdf(tauMin,a,b) >= y) tauMin -= w;
    tauMin = max(0.0,tauMin);
    while (concentrationLogPdf(tauMax,a,b) >= y) tauMax += w;
    while(42){
      T tauNew = unif_(*pRndGen_)*(tauMax-tauMin)+tauMin;
//      cout<<"vMFpriorFull::sampleConcentration: "<<tauNew<<" min: "<<tauMin<<" max: "<<tauMax<<" a="<<a<<" b="<<b<<endl;
      if(concentrationLogPdf(tauNew,a,b) >= y)
      {
        tau = tauNew; break;
      }else{
        if (tauNew < tau) tauMin = tauNew; else tauMax = tauNew;
      }
    };
  }
  return tau;
};


#include "random_generator.hpp"
#include <memory>
#include <time.h>
#include <cmath>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/uniform_int.hpp>

RandomGenerator* RandomGenerator::instance = 0;

RandomGenerator* RandomGenerator::getInstance() {
  if (instance == 0) {
      instance = new RandomGenerator();
  }

  return instance;
}

RandomGenerator::RandomGenerator() {
  //pGenerator_ = std::make_shared<
  //  boost::variate_generator<boost::mt19937, boost::normal_distribution<>>
  //  >( boost::mt19937(time(0)), boost::normal_distribution<>() )
  //;

  pGenerator_ = std::make_shared<
    boost::variate_generator<boost::mt19937, boost::uniform_real<>>
    >( boost::mt19937(time(0)), boost::uniform_real<>(0,1) )
  ;

  pIntGenerator_ = std::make_shared<
    boost::variate_generator<boost::mt19937, boost::uniform_int<>>
    >( boost::mt19937(time(0)), boost::uniform_int<>(0, 999999) )
    //>( boost::mt19937(time(0)), boost::uniform_int<>(0, 99) )
  ;
}

int RandomGenerator::getInt() {
  return (*pIntGenerator_)();
}

int RandomGenerator::getMaxInt() {
  return pIntGenerator_->max();
}

double RandomGenerator::getDouble() {
  return (*pGenerator_)();
}

double RandomGenerator::getMaxDouble() {
  return pGenerator_->max();
}