/*
 * 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
 */

#include <iostream>
#include <limits>

#include <algorithms/sssp.hpp>
#include <graphblas/graphblas.hpp>

using namespace algorithms;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE sssp_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
template <typename T>
grb::Matrix<T> get_tn_answer()
{
    std::vector<grb::IndexType> rows = {
        0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2,
        2, 3, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5,
        5, 5, 6, 6, 6, 6, 6, 6, 6, 6, 7, 8, 8, 8, 8, 8, 8, 8, 8};
    std::vector<grb::IndexType> cols = {
        0, 1, 2, 3, 4, 5, 6, 8, 0, 1, 2, 3, 4, 5, 6, 8, 0, 1, 2, 3, 4, 5, 6,
        8, 0, 1, 2, 3, 4, 5, 6, 8, 0, 1, 2, 3, 4, 5, 6, 8, 0, 1, 2, 3, 4, 5,
        6, 8, 0, 1, 2, 3, 4, 5, 6, 8, 7, 0, 1, 2, 3, 4, 5, 6, 8
    };

    std::vector<T> vals = {
        0,   2,   3,   1,   2,   4,   2,   3,   2, 0,   2,   1,   2,
        3,   1,   3,   3,   2, 0,   2,   1,   1,   1,   1,   1,   1,
        2, 0,   1,   3,   1,   2,   2,   2,   1,   1, 0,   2,   2,
        1,   4,   3,   1,   3,   2, 0,   2,   2,   2,   1,   1,   1,
        2,   2, 0,   2, 0,   3,   3,   1,   2,   1,   2,   2, 0};

    //std::vector<std::vector<T> > G_tn_answer_dense =
    //    {{0, 2, 3, 1, 2, 4, 2, , 3},
    //     {2, 0, 2, 1, 2, 3, 1, , 3},
    //     {3, 2, 0, 2, 1, 1, 1, , 1},
    //     {1, 1, 2, 0, 1, 3, 1, , 2},
    //     {2, 2, 1, 1, 0, 2, 2, , 1},
    //     {4, 3, 1, 3, 2, 0, 2, , 2},
    //     {2, 1, 1, 1, 2, 2, 0, , 2},
    //     { ,  ,  ,  ,  ,  ,  , 0, },
    //     {3, 3, 1, 2, 1, 2, 2, , 0}};
    grb::Matrix<T> temp(9,9);
    temp.build(rows.begin(), cols.begin(), vals.begin(), rows.size());
    return temp;
}

//****************************************************************************
template <typename T>
grb::Matrix<T> get_gilbert_answer()
{
    //std::vector<std::vector<T> > G_gilbert_answer_dense =
    //    {{  0,   1,   2,   1,   2,   3,   2},
    //     {  3,   0,   2,   2,   1,   2,   1},
    //     {   ,    ,   0,    ,    ,   1,    },
    //     {  1,   2,   1,   0,   3,   2,   3},
    //     {   ,    ,   2,    ,   0,   1,    },
    //     {   ,    ,   1,    ,    ,   0,    },
    //     {  2,   3,   1,   1,   1,   2,   0}};
    //return Matrix<T>(G_gilbert_answer_dense, );
    std::vector<grb::IndexType> rows = {
        0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 2, 2, 3, 3, 3, 3, 3, 3, 3,
        4, 4, 4, 5, 5, 6, 6, 6, 6, 6, 6, 6
    };
    std::vector<grb::IndexType> cols = {
        0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 2, 5, 0, 1, 2, 3, 4, 5, 6,
        2, 4, 5, 2, 5, 0, 1, 2, 3, 4, 5, 6
    };
    std::vector<T> vals = {
        0,   1,   2,   1,   2,   3,   2,   3, 0,   2,   2,   1,   2,
        1, 0,   1,   1,   2,   1, 0,   3,   2,   3,   2, 0,   1,
        1, 0,   2,   3,   1,   1,   1,   2, 0
    };
    grb::Matrix<T> temp(7,7);
    temp.build(rows.begin(), cols.begin(), vals.begin(), rows.size());
    return temp;
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_sssp_negative_cycle_detection)
{
    grb::IndexType const NUM_NODES(4);
    grb::IndexType const start_index(0);

    grb::IndexArrayType i = {0, 3, 1, 2};
    grb::IndexArrayType j = {1, 1, 2, 3};
    std::vector<double>       v = {1,-5, 1, 3};
    grb::Matrix<double> G(NUM_NODES, NUM_NODES);
    G.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(false, sssp(G, dist));
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_batch_sssp_negative_cycle_detection)
{
    grb::IndexType const NUM_NODES(4);

    grb::IndexArrayType i = {0, 3, 1, 2};
    grb::IndexArrayType j = {1, 1, 2, 3};
    std::vector<double>       v = {1,-5, 1, 3};
    grb::Matrix<double> G(NUM_NODES, NUM_NODES);
    G.build(i, j, v);

    // Perform for all roots simultaneously
    auto dists =
        grb::scaled_identity<grb::Matrix<double> >(NUM_NODES, 0);

    BOOST_CHECK_EQUAL(false, batch_sssp(G, dists));
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_filtered_sssp_negative_cycle_detection)
{
    grb::IndexType const NUM_NODES(4);
    grb::IndexType const start_index(0);

    grb::IndexArrayType i = {0, 3, 1, 2};
    grb::IndexArrayType j = {1, 1, 2, 3};
    std::vector<double>       v = {1,-5, 1, 2};  // TODO won't work if 0 distance
    grb::Matrix<double> G(NUM_NODES, NUM_NODES);
    G.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(false, filtered_sssp(G, dist));
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_sssp_basic_mincost_one_root)
{
    grb::IndexType const NUM_NODES(4);
    grb::IndexType const start_index(0);

    grb::IndexArrayType i = {0, 0, 1, 2};
    grb::IndexArrayType j = {1, 3, 2, 3};
    std::vector<double>       v = {1, 5, 1, 1};
    grb::Matrix<double> G(NUM_NODES, NUM_NODES);
    G.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(true, sssp(G, dist));
    //grb::print_vector(std::cout, dist, "MINCOST dist");
    BOOST_CHECK_EQUAL(dist.extractElement(0), 0);
    BOOST_CHECK_EQUAL(dist.extractElement(1), 1);
    BOOST_CHECK_EQUAL(dist.extractElement(2), 2);
    BOOST_CHECK_EQUAL(dist.extractElement(3), 3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_sssp_basic_mincost2_one_root)
{
    grb::IndexType const NUM_NODES(4);
    grb::IndexType const start_index(0);

    grb::IndexArrayType i = {0, 0, 1, 2};
    grb::IndexArrayType j = {1, 3, 2, 3};
    std::vector<double>       v = {1, 2, 1, 1};
    grb::Matrix<double> G(NUM_NODES, NUM_NODES);
    G.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(true, sssp(G, dist));
    //grb::print_vector(std::cout, dist, "MINCOST dist");
    BOOST_CHECK_EQUAL(dist.extractElement(0), 0);
    BOOST_CHECK_EQUAL(dist.extractElement(1), 1);
    BOOST_CHECK_EQUAL(dist.extractElement(2), 2);
    BOOST_CHECK_EQUAL(dist.extractElement(3), 2);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_sssp_basic_double_one_root)
{
    grb::IndexType const NUM_NODES(9);
    grb::IndexType const start_index(3);

    grb::IndexArrayType i = {0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3,
                                   4, 4, 4, 5, 6, 6, 6, 8, 8};
    grb::IndexArrayType j = {3, 3, 6, 4, 5, 6, 8, 0, 1, 4, 6,
                                   2, 3, 8, 2, 1, 2, 3, 2, 4};
    std::vector<double>       v(i.size(), 1);
    grb::Matrix<double> G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(true, sssp(G_tn, dist));

    auto G_tn_answer(get_tn_answer<double>());

    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (G_tn_answer.hasElement(start_index, ix))
        {
            BOOST_CHECK_EQUAL(dist.hasElement(ix), true);
            BOOST_CHECK_EQUAL(dist.extractElement(ix),
                              G_tn_answer.extractElement(start_index, ix));
        }
        else
        {
            BOOST_CHECK_EQUAL(dist.hasElement(ix), false);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_batch_sssp_basic_double)
{
    grb::IndexType const NUM_NODES(9);
    grb::IndexArrayType i = {0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3,
                                   4, 4, 4, 5, 6, 6, 6, 8, 8};
    grb::IndexArrayType j = {3, 3, 6, 4, 5, 6, 8, 0, 1, 4, 6,
                                   2, 3, 8, 2, 1, 2, 3, 2, 4};
    std::vector<double>       v(i.size(), 1);
    grb::Matrix<double> G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    // Perform for all roots simultaneously
    auto paths =
        grb::scaled_identity<grb::Matrix<double> >(NUM_NODES, 0);
    BOOST_CHECK_EQUAL(true, batch_sssp(G_tn, paths));

    auto G_tn_answer(get_tn_answer<double>());

    //print_matrix(std::cout, paths, "result");
    //print_matrix(std::cout, G_tn_answer, "correct answer");
    BOOST_CHECK_EQUAL(paths, G_tn_answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_batch_sssp_basic_uint)
{
    grb::IndexType const NUM_NODES(9);
    grb::IndexArrayType i = {0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3,
                                   4, 4, 4, 5, 6, 6, 6, 8, 8};
    grb::IndexArrayType j = {3, 3, 6, 4, 5, 6, 8, 0, 1, 4, 6,
                                   2, 3, 8, 2, 1, 2, 3, 2, 4};
    std::vector<unsigned int> v(i.size(), 1);
    grb::Matrix<unsigned int> G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    // Perform for all roots simultaneously
    auto paths =
        grb::scaled_identity<grb::Matrix<unsigned int> >(NUM_NODES, 0);
    BOOST_CHECK_EQUAL(true, batch_sssp(G_tn, paths));

    auto G_tn_answer(get_tn_answer<unsigned int>());

    BOOST_CHECK_EQUAL(paths, G_tn_answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_batch_sssp_gilbert_double)
{
    grb::IndexType const NUM_NODES(7);
    grb::IndexArrayType i = {0, 0, 1, 1, 2, 3, 3, 4, 5, 6, 6, 6};
    grb::IndexArrayType j = {1, 3, 4, 6, 5, 0, 2, 5, 2, 2, 3, 4};
    std::vector<double>       v(i.size(), 1);
    grb::Matrix<double> G_gilbert(NUM_NODES, NUM_NODES);
    G_gilbert.build(i, j, v);

    // Perform for all roots simultaneously
    auto paths =
        grb::scaled_identity<grb::Matrix<double> >(NUM_NODES, 0);
    BOOST_CHECK_EQUAL(true, batch_sssp(G_gilbert, paths));

    auto G_gilbert_answer(get_gilbert_answer<double>());

    BOOST_CHECK_EQUAL(paths, G_gilbert_answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_batch_sssp_gilbert_uint)
{
    grb::IndexType const NUM_NODES(7);
    grb::IndexArrayType i = {0, 0, 1, 1, 2, 3, 3, 4, 5, 6, 6, 6};
    grb::IndexArrayType j = {1, 3, 4, 6, 5, 0, 2, 5, 2, 2, 3, 4};
    std::vector<unsigned int> v(i.size(), 1);
    grb::Matrix<unsigned int> G_gilbert(NUM_NODES, NUM_NODES);
    G_gilbert.build(i, j, v);

    // Perform for all roots simultaneously
    auto paths =
        grb::scaled_identity<grb::Matrix<unsigned int> >(NUM_NODES, 0);
    BOOST_CHECK_EQUAL(true, batch_sssp(G_gilbert, paths));

    auto G_gilbert_answer(get_gilbert_answer<unsigned int>());

    BOOST_CHECK_EQUAL(paths, G_gilbert_answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_filtered_sssp_basic_mincost_one_root)
{
    grb::IndexType const NUM_NODES(4);
    grb::IndexType const start_index(0);

    grb::IndexArrayType i = {0, 0, 1, 2};
    grb::IndexArrayType j = {1, 3, 2, 3};
    std::vector<double>       v = {1, 5, 1, 1};
    grb::Matrix<double> G(NUM_NODES, NUM_NODES);
    G.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(true, filtered_sssp(G, dist));
    //grb::print_vector(std::cout, dist, "MINCOST dist");
    BOOST_CHECK_EQUAL(dist.extractElement(0), 0);
    BOOST_CHECK_EQUAL(dist.extractElement(1), 1);
    BOOST_CHECK_EQUAL(dist.extractElement(2), 2);
    BOOST_CHECK_EQUAL(dist.extractElement(3), 3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_filtered_sssp_basic_mincost2_one_root)
{
    grb::IndexType const NUM_NODES(4);
    grb::IndexType const start_index(0);

    grb::IndexArrayType i = {0, 0, 1, 2};
    grb::IndexArrayType j = {1, 3, 2, 3};
    std::vector<double>       v = {1, 2, 1, 1};
    grb::Matrix<double> G(NUM_NODES, NUM_NODES);
    G.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(true, filtered_sssp(G, dist));
    //grb::print_vector(std::cout, dist, "MINCOST dist");
    BOOST_CHECK_EQUAL(dist.extractElement(0), 0);
    BOOST_CHECK_EQUAL(dist.extractElement(1), 1);
    BOOST_CHECK_EQUAL(dist.extractElement(2), 2);
    BOOST_CHECK_EQUAL(dist.extractElement(3), 2);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_filtered_sssp_basic_double_one_root)
{
    grb::IndexType const NUM_NODES(9);
    grb::IndexType const start_index(3);

    grb::IndexArrayType i = {0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3,
                                   4, 4, 4, 5, 6, 6, 6, 8, 8};
    grb::IndexArrayType j = {3, 3, 6, 4, 5, 6, 8, 0, 1, 4, 6,
                                   2, 3, 8, 2, 1, 2, 3, 2, 4};
    std::vector<double>       v(i.size(), 1);
    grb::Matrix<double> G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    grb::Vector<double> dist(NUM_NODES);
    dist.setElement(start_index, 0);

    BOOST_CHECK_EQUAL(true, filtered_sssp(G_tn, dist));

    auto G_tn_answer(get_tn_answer<double>());

    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (G_tn_answer.hasElement(start_index, ix))
        {
            BOOST_CHECK_EQUAL(dist.hasElement(ix), true);
            BOOST_CHECK_EQUAL(dist.extractElement(ix),
                              G_tn_answer.extractElement(start_index, ix));
        }
        else
        {
            BOOST_CHECK_EQUAL(dist.hasElement(ix), false);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_filtered_sssp_marcin_uint)
{
    //  - 7 1 - - - - -
    //  7 - - - - 1 1 -
    //  1 - - 1 - - - -
    //  - - 1 - 1 - - -
    //  - - - 1 - 1 - -
    //  - 1 - - 1 - - 1
    //  - 1 - - - - - 1
    //  - - - - - 1 1 -

    // The correct answer for source vertex 0
    //
    //    {0, 5, 1, 2, 3, 4, 6, 5};

    grb::IndexType const SOURCE = 0;
    std::vector<unsigned int> dense_ans = {0, 5, 1, 2, 3, 4, 6, 5};
    grb::Vector<unsigned int> answer(dense_ans);

    grb::IndexType const NUM_NODES(8);
    grb::IndexArrayType i = {0,0,1,1,1,2,2,3,3,4,4,5,5,5,6,6,7,7};
    grb::IndexArrayType j = {1,2,0,5,6,0,3,2,4,3,5,1,4,7,1,7,5,6};
    std::vector<unsigned int> v = {7,1,7,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
    grb::Matrix<unsigned int> G(NUM_NODES, NUM_NODES);
    G.build(i.begin(), j.begin(), v.begin(), i.size());

    grb::Vector<unsigned int> result(NUM_NODES);
    result.setElement(SOURCE, 0);
    filtered_sssp(G, result);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_sssp_delta2_step_gilbert_uint)
{
    grb::IndexType const SOURCE = 3;
    std::vector<unsigned int> dense_ans3 =  {3,   4,   1,   0,   5,   2,   7};
    grb::Vector<unsigned int> answer3(dense_ans3);

    grb::IndexType const NUM_NODES(7);
    grb::IndexArrayType i = {0, 0, 1, 1, 2, 3, 3, 4, 5, 6, 6, 6};
    grb::IndexArrayType j = {1, 3, 4, 6, 5, 0, 2, 5, 2, 2, 3, 4};
    std::vector<unsigned int> v = {1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3};
    grb::Matrix<unsigned int> G_gilbert(NUM_NODES, NUM_NODES);
    G_gilbert.build(i.begin(), j.begin(), v.begin(), i.size());

    grb::Vector<unsigned int> result(NUM_NODES);
    sssp_delta_step(G_gilbert, 2U, SOURCE, result);

    BOOST_CHECK_EQUAL(result, answer3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_sssp_delta_step_marcin_uint)
{
    //  - 7 1 - - - - -
    //  7 - - - - 1 1 -
    //  1 - - 1 - - - -
    //  - - 1 - 1 - - -
    //  - - - 1 - 1 - -
    //  - 1 - - 1 - - 1
    //  - 1 - - - - - 1
    //  - - - - - 1 1 -

    // The correct answer for source vertex 0
    //
    //    {0, 5, 1, 2, 3, 4, 6, 5};

    grb::IndexType const SOURCE = 0;
    std::vector<unsigned int> dense_ans = {0, 5, 1, 2, 3, 4, 6, 5};
    grb::Vector<unsigned int> answer(dense_ans);

    grb::IndexType const NUM_NODES(8);
    grb::IndexArrayType i = {0,0,1,1,1,2,2,3,3,4,4,5,5,5,6,6,7,7};
    grb::IndexArrayType j = {1,2,0,5,6,0,3,2,4,3,5,1,4,7,1,7,5,6};
    std::vector<unsigned int> v = {7,1,7,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
    grb::Matrix<unsigned int> G(NUM_NODES, NUM_NODES);
    G.build(i.begin(), j.begin(), v.begin(), i.size());

    grb::Vector<unsigned int> result2(NUM_NODES);
    sssp_delta_step(G, 2U, SOURCE, result2);
    BOOST_CHECK_EQUAL(result2, answer);
    grb::Vector<unsigned int> result7(NUM_NODES);
    sssp_delta_step(G, 7U, SOURCE, result7);
    BOOST_CHECK_EQUAL(result7, answer);
    grb::Vector<unsigned int> result8(NUM_NODES);
    sssp_delta_step(G, 8U, SOURCE, result8);
    BOOST_CHECK_EQUAL(result8, answer);
    grb::Vector<unsigned int> result9(NUM_NODES);
    sssp_delta_step(G, 9U, SOURCE, result9);
    BOOST_CHECK_EQUAL(result9, answer);
    grb::Vector<unsigned int> result10(NUM_NODES);
    sssp_delta_step(G, 10U, SOURCE, result10);
    BOOST_CHECK_EQUAL(result10, answer);
}

BOOST_AUTO_TEST_SUITE_END()


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file data_equation_test.cpp
/// \brief Basic regression test for data equations.

#include <iostream>
#include <boost/test/minimal.hpp>

#include "mcrl2/atermpp/container_utility.h"
#include "mcrl2/data/data_equation.h"
#include "mcrl2/data/basic_sort.h"
#include "mcrl2/data/variable.h"
#include "mcrl2/data/function_symbol.h"
#include "mcrl2/data/data_equation.h"

using namespace mcrl2;
using namespace mcrl2::data;

void data_equation_test()
{
  basic_sort s("S");
  data::function_symbol c("c", s);
  data::function_symbol f("f", s);
  variable x("x", s);
  variable_list xl = { x };

  data_equation e(xl, c, x, f);
  BOOST_CHECK(e.variables() == xl);
  BOOST_CHECK(e.condition() == c);
  BOOST_CHECK(e.lhs() == x);
  BOOST_CHECK(e.rhs() == f);
}

int test_main(int argc, char** argv)
{
  data_equation_test();

  return EXIT_SUCCESS;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 4> test_int_3_4(const Eigen::Matrix<int, 3, 4> & M)
{
	return M;
}
void export_int_3_4()
{
	boost::python::def("test_int_3_4",test_int_3_4);
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_BINNING_HPP
#define NETKET_BINNING_HPP

#include <Eigen/Dense>
#include <valarray>
#include <vector>

#include "Utils/json_utils.hpp"
#include "Utils/messages.hpp"

#include "onlinestat.hpp"

namespace netket {

template <class T>
class Binning {
  using StatType = OnlineStat<T>;
  using DataType = T;

  // target number of bins
  // the actual number of bins used for the estimates can be larger than this
  int nbins_;

  int nb1_;

  std::vector<StatType> bins1_;
  std::vector<StatType> bins2_;

  int last1_;
  int last2_;

  int nproc_;

 public:
  struct Stats {
    DataType mean;
    DataType sigma;
    DataType taucorr;

    friend void to_json(json &j, const Stats &stats) {
      j["Mean"] = stats.mean;
      j["Sigma"] = stats.sigma;
      j["Taucorr"] = stats.taucorr;
    }
  };

  explicit Binning(int nbins = 16) { Init(nbins); }

  void Init(int nbins) {
    MPI_Comm_size(MPI_COMM_WORLD, &nproc_);

    nbins_ = nbins;

    if (nbins_ % 2 != 0) {
      std::cerr << "#The number of bins should be a multiple of two"
                << std::endl;
    }

    bins1_.resize(nbins);
    bins2_.resize(nbins);

    Reset();
  }

  void Reset() {
    last1_ = 0;
    last2_ = 0;
    nb1_ = 0;

    for (std::size_t i = 0; i < bins1_.size(); i++) {
      bins1_[i].Reset();
    }

    for (std::size_t i = 0; i < bins2_.size(); i++) {
      bins2_[i].Reset();
    }
  }

  void operator<<(const DataType &val) {
    if (last1_ < nbins_) {
      bins1_[last1_] << val;
      last1_++;
    } else {
      nb1_ = bins1_[0].N();

      if (last2_ < nbins_) {
        bins2_[last2_] << val;
        if (bins2_[last2_].N() == nb1_) {
          last2_++;
        }
      } else {
        Merge();
      }
    }
  }

  void Print() {
    for (std::size_t i = 0; i < last1_; i++) {
      InfoMessage() << bins1_[i].Mean() << "  " << bins1_[i].N() << std::endl;
    }
    for (std::size_t i = 0; i < last2_; i++) {
      InfoMessage() << bins2_[i].Mean() << "  " << bins2_[i].N() << std::endl;
    }
  }

  void Merge() {
    for (std::size_t i = 0; i < bins1_.size(); i += 2) {
      bins1_[i] << bins1_[i + 1];
      bins2_[i] << bins2_[i + 1];
    }
    for (std::size_t i = 0; i < bins1_.size() / 2; i++) {
      bins1_[i] = bins1_[2 * i];
    }
    for (std::size_t i = 0; i < bins1_.size() / 2; i++) {
      bins1_[i + bins1_.size() / 2] = bins2_[2 * i];
    }

    last2_ = 0;
    for (std::size_t i = 0; i < bins2_.size(); i++) {
      bins2_[i].Reset();
    }
  }

  // Returns binned statistics
  StatType Binned() const {
    StatType bint;

#ifndef NDEBUG
    int nb = bins1_[0].N();
#endif
    for (int i = 0; i < last1_; i++) {
      bint << bins1_[i].Mean();
      assert(bins1_[i].N() == nb);
    }
    for (int i = 0; i < last2_; i++) {
      bint << bins2_[i].Mean();
      assert(bins2_[i].N() == nb);
    }
    return bint;
  }

  // Returns unbinned statistics
  StatType UnBinned() const {
    StatType bint;

    for (int i = 0; i < last1_; i++) {
      bint << bins1_[i];
    }
    for (int i = 0; i < last2_; i++) {
      bint << bins2_[i];
    }
    return bint;
  }

  // Estimates the auto-correlation time using error of mean
  // computed on blocked values
  DataType TauCorrProc() const {
    const auto unbinned = UnBinned();
    const auto binned = Binned();

    const auto erbinned = binned.ErrorOfMean();
    const auto erunbinned = unbinned.ErrorOfMean();

    return TauCorrOp(erbinned, erunbinned);
  }

  DataType TauCorr() const {
    DataType taucp = TauCorrProc();

    SumOnNodes(taucp);

    return taucp / double(nproc_);
  }

  DataType Mean() {
    const auto binned = Binned();

    DataType mean = binned.Mean();

    SumOnNodes(mean);

    return mean / double(nproc_);
  }

  DataType ErrorOfMean() {
    const auto binned = Binned();

    DataType sigma2 = Sigma2Op(binned.ErrorOfMean());
    SumOnNodes(sigma2);

    return EoMeanOp(sigma2);
  }

  Stats AllStats() const {
    const auto binned = Binned();

    DataType mean = binned.Mean();
    SumOnNodes(mean);
    mean /= double(nproc_);

    DataType sigma2 = Sigma2Op(binned.ErrorOfMean());
    SumOnNodes(sigma2);
    auto eomean = EoMeanOp(sigma2);

    auto tcorr = TauCorr();

    return {mean, eomean, tcorr};
  }

  template<class Map>
  void InsertAllStats(Map &dict) const {
    auto stats = AllStats();
    dict["Mean"] = stats.mean;
    dict["Sigma"] = stats.sigma;
    dict["Taucorr"] = stats.taucorr;
  }

  int NvalProc() {
    int nv = 0;

    for (int i = 0; i < last1_; i++) {
      nv += bins1_[i].N();
    }
    for (int i = 0; i < last2_; i++) {
      nv += bins2_[i].N();
    }
    return nv;
  }

  int N() {
    int Np = NvalProc();
    SumOnNodes(Np);
    return Np;
  }

  double TauCorrOp(double erbinned, double erunbinned) const {
    return 0.5 * (std::pow(erbinned / erunbinned, 2.) - 1.);
  }

  Eigen::VectorXd TauCorrOp(const Eigen::VectorXd &erbinned,
                            const Eigen::VectorXd &erunbinned) const {
    Eigen::VectorXd result(erbinned.size());

    for (int i = 0; i < result.size(); i++) {
      result(i) = 0.5 * (std::pow(erbinned(i) / erunbinned(i), 2.) - 1.);
    }
    return result;
  }

  template <class P>
  std::vector<P> TauCorrOp(const std::vector<P> &erbinned,
                           const std::vector<P> &erunbinned) const {
    std::vector<P> result(erbinned.size());

    for (int i = 0; i < result.size(); i++) {
      result[i] = 0.5 * (std::pow(erbinned[i] / erunbinned[i], 2.) - 1.);
    }
    return result;
  }

  template <class P>
  std::valarray<P> TauCorrOp(const std::valarray<P> &erbinned,
                             const std::valarray<P> &erunbinned) const {
    return 0.5 * (std::pow(erbinned / erunbinned, 2.) - 1.);
  }

  double EoMeanOp(double sigma2) const {
    return std::sqrt(sigma2) / (double(nproc_));
  }

  Eigen::VectorXd EoMeanOp(const Eigen::VectorXd &sigma2) const {
    Eigen::VectorXd result(sigma2.size());

    for (int i = 0; i < result.size(); i++) {
      result(i) = EoMeanOp(sigma2(i));
    }
    return result;
  }

  template <class P>
  std::vector<P> EoMeanOp(const std::vector<P> &sigma2) const {
    std::vector<P> result(sigma2.size());

    for (int i = 0; i < result.size(); i++) {
      result[i] = EoMeanOp(sigma2(i));
    }
    return result;
  }

  template <class P>
  std::valarray<P> EoMeanOp(const std::valarray<P> &sigma2) const {
    return std::sqrt(sigma2) / (double(nproc_));
  }

  double Sigma2Op(double sigma2) const { return sigma2 * sigma2; }

  Eigen::VectorXd Sigma2Op(const Eigen::VectorXd &sigma2) const {
    Eigen::VectorXd result(sigma2.size());

    for (int i = 0; i < result.size(); i++) {
      result(i) = Sigma2Op(sigma2(i));
    }
    return result;
  }

  template <class P>
  std::vector<P> Sigma2Op(const std::vector<P> &sigma2) const {
    std::vector<P> result(sigma2.size());

    for (int i = 0; i < result.size(); i++) {
      result[i] = Sigma2Op(sigma2(i));
    }
    return result;
  }

  template <class P>
  std::valarray<P> Sigma2Op(const std::valarray<P> &sigma2) const {
    return sigma2 * sigma2;
  }
};

}  // namespace netket

#endif


/*
 * (C) Copyright 2009-2020 ECMWF.
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 * In applying this licence, ECMWF does not waive the privileges and immunities
 * granted to it by virtue of its status as an intergovernmental organisation nor
 * does it submit to any jurisdiction.
 */

#include "oops/util/dateFunctions.h"

#include <stdint.h>
#include <limits>
#include <sstream>

#include <boost/lexical_cast.hpp>

#include "eckit/exception/Exceptions.h"
#include "oops/util/Logger.h"

// -----------------------------------------------------------------------------

void failBadFormat(const std::string& str) {
  std::string message = "Badly formatted date: ";
  message.append(str);
  throw eckit::BadValue(message);
}

// -----------------------------------------------------------------------------

int eatChars(std::istream & is, int nchars) {
  // consume nchars characters from the stream and interpret as an integer
  if (nchars < 0) throw eckit::BadParameter("Cannot read a negative number of characters.");
  std::string str((size_t) nchars, '\0');
  is.get(&str[0], nchars+1);  // nchars+1 because istream.get reads (count-1) chars.

  int ret = 0;
  try {
    ret = std::stoi(str);
  }
  catch (...) {
    failBadFormat(str);
  }
  return ret;
}

/// Non-member functions for manipulating date and time

namespace util {
namespace datefunctions {

// -----------------------------------------------------------------------------

uint64_t dateToJulian(const int year, const int month, const int day) {
//  Compute the Julian Day number applying the following formula
//
//  julian_day = ( 1461 * ( y + 4800 + ( m - 14 ) / 12 ) ) / 4 +
//               ( 367 * ( m - 2 - 12 * ( ( m - 14 ) / 12 ) ) ) / 12 -
//                   ( 3 * ( ( y + 4900 + ( m - 14 ) / 12 ) / 100 ) ) / 4 +
//                     d - 32075

  if (!util::datefunctions::validYYYYMMDD(year, month, day)) {
    oops::Log::error() << "year=" << year << " month=" << month << " day=" << day << std::endl;
    throw eckit::BadParameter("Not a valid date");
  }

  int m1 = (month - 14)/12;
  int a  = (1461 * (year + 4800 + m1))/4;
  int b  = (367 * (month - 2 - (12 * m1)))/12;
  int m2 = (year + 4900 + m1)/100;
  int c  = (3 * (m2))/4;
  return static_cast<uint64_t>(a + b - c + day - 32075);
}

// -----------------------------------------------------------------------------

void julianToDate(const uint64_t julian, int & yy, int & mm, int & dd) {
  uint64_t l     = 0;
  uint64_t n     = 0;
  uint64_t i     = 0;
  uint64_t j     = 0;
  uint64_t day   = 0;
  uint64_t month = 0;
  uint64_t year  = 0;

// Modified Julian date

  l = julian + static_cast<uint64_t>(68569);
  n = (4 * l) / 146097;
  l = l - (146097 * n + 3) / 4;
  i = (4000 * (l + 1)) / 1461001;
  l = l - (1461 * i) / 4 + 31;
  j = (80 * l) / 2447;
  day = l - (2447 * j) / 80;
  l = j / 11;
  month = j + 2 - (12 * l);
  year = 100 * (n - 49) + i + l;
  if (year <= static_cast<uint64_t>(std::numeric_limits<int>::max())) {
    dd = static_cast<int>(day);
    mm = static_cast<int>(month);
    yy = static_cast<int>(year);
  } else {
    oops::Log::error() << "year=" << year << std::endl;
    throw eckit::BadParameter("Year out of range");
  }
}

// -----------------------------------------------------------------------------

int hmsToSeconds(const int hour, const int minute, const int second) {
  if (!util::datefunctions::validHhmmss(hour, minute, second)) {
    oops::Log::error() << "hour=" << hour << " minute=" << minute <<
               " second=" << second << std::endl;
    throw eckit::BadParameter("Not a valid time");
  }
  return 3600 * hour + 60 * minute + second;
}

// -----------------------------------------------------------------------------

void secondToHms(const int seconds, int & hh, int & mm, int & ss) {
  const int secondsPerDay = 86400;

  if (seconds >= 0 && seconds <= secondsPerDay) {
    int local_sec = seconds;
    hh = local_sec / 3600;
    local_sec %= 3600;
    mm = local_sec / 60;
    local_sec %= 60;
    ss = local_sec;
  } else {
    oops::Log::error() << "seconds=" << seconds << std::endl;
    throw eckit::BadParameter("seconds out of range");
  }
}

// -----------------------------------------------------------------------------

bool isLeapYear(const int year) {
  return ((year%4 == 0 && year%100 != 0) || year%400 == 0);
}

// -----------------------------------------------------------------------------

bool validHhmmss(const int hour, const int minute, const int second) {
  return hour   >= 0 && hour   <= 23 &&
         minute >= 0 && minute <= 59 &&
         second >= 0 && second <= 59;
}

// -----------------------------------------------------------------------------

bool validYYYYMMDD(const int year, const int month, const int day) {
  bool good;
  good = year   >= 0 && year <= 9999 &&
         month  >= 1 && month  <= 12 &&
         day    >= 1 && day    <= 31;

  if (good) {
    if (month == 9 || month == 4 || month == 6 || month == 11) {
        good = (day <= 30);
    } else if (month != 2) {
        good = (day <= 31);
    } else if (util::datefunctions::isLeapYear(year)) {
        good = (day <= 29);
    } else {
        good = (day <= 28);
    }
  }

  return good;
}

// -----------------------------------------------------------------------------
/*! Set year, month, day, hour, minute and second from an ISO 8601 formatted string
 *  \param str ISO 8601 formatted string
 *  \param year
 *  \param month
 *  \param day
 *  \param hour
 *  \param minute
 *  \param second
 */
void stringToYYYYMMDDhhmmss(const std::string & str,
                            int & year, int & month, int & day,
                            int & hour, int & minute, int & second) {
  std::istringstream datestream(str);

  year = eatChars(datestream, 4);

  bool dashes = (datestream.peek() == '-');
  if (dashes) datestream.get();

  month = eatChars(datestream, 2);
  if (dashes) {
    char c = datestream.get();
    if (c != '-') {failBadFormat(str);}
  }

  day = eatChars(datestream, 2);

  char c = datestream.get();
  if (c != 'T') {failBadFormat(str);}

  hour = eatChars(datestream, 2);

  bool colons = (datestream.peek() == ':');
  if (colons) datestream.get();

  minute = eatChars(datestream, 2);
  if (colons) {
    char c = datestream.get();
    if (c != ':') {failBadFormat(str);}
  }

  second = eatChars(datestream, 2);

  c = datestream.get();
  if (c != 'Z') {failBadFormat(str);}

  datestream.peek();
  if (!datestream.eof()) {failBadFormat(str);}
}

// -----------------------------------------------------------------------------

}  // namespace datefunctions
}  // namespace util


#ifndef algebraic_functions_include
#define algebraic_functions_include

#include <vector>
#include <algorithm>
#include <numeric>
#include <iostream>

#include <boost/config/concept_macros.hpp>

#ifdef __GXX_CONCEPTS__
#  include <concepts>
#  include <bits/iterator_concepts.h>
#endif

#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>

// Pure algebraic functions (mainly for applying concepts)

namespace mtl {

// {Op, Element} must be a Magma
// The result of the operation must be EqualityComparable
// Closed2EqualityComparable requires the comparability of any combination of
// Element and result_type.
// It is thus more limiting than necessary, well slightly.
template <typename Op, typename Element>
#if 0
  _GLIBCXX_WHERE( math::Magma<Op, Element>
	    && std::EqualityComparable< math::Magma<Op, Element>::result_type > )
#endif
  _GLIBCXX_WHERE( math::Closed2EqualityComparable<Op, Element> && math::Magma<Op, Element> )
inline bool equal_results(const Element& v1a, const Element& v1b,
			  const Element& v2a, const Element& v2b, Op op)
{
#if 0
    // If we use EqualityComparable for Element we cannot compare the results
    // directly since Magma only assumes that the results are convertible to Element
    // which does not imply that they are comparable.
    // Assigning to temporaries will the conversion and the temporaries are comparable.

    Element res1= op(v1a, v1b), res2= op(v2a, v2b);
    return res1 == res2;
#endif

    return op(v1a, v1b) == op(v2a, v2b);
}

// Same for AdditiveMagma
template <typename Element>
_GLIBCXX_WHERE ( math::AdditiveMagma<Element>
	   // && math::Closed2EqualityComparable< math::add<Element>, Element > )
	   && std::EqualityComparable< math::AdditiveMagma<Element>::addition_result_type > )
inline bool equal_add_results(const Element& v1a, const Element& v1b,
			      const Element& v2a, const Element& v2b)
{
    return v1a + v1b == v2a + v2b;
}

// {Op, Element} must be a Monoid
// The result of the operation must be EqualityComparable
template <typename Op, typename Element>
  _GLIBCXX_WHERE( math::Closed2EqualityComparable<Op, Element> && math::Monoid<Op, Element> )
inline bool identity_pair(const Element& v1, const Element& v2, Op op)
{
    using math::identity;
    return op(v1, v2) == identity(op, v1) ;
}

// {Op, Element} must be a SemiGroup
template <typename Op, typename Element, typename Exponent>
  _GLIBCXX_WHERE( math::SemiGroup<Op, Element>
            && std::Integral<Exponent> )
Element recursive_multiply_and_square(const Element& a, Exponent n, Op op)
{
    if (n <= 0) throw "In recursive_multiply_and_square: exponent must greater than 0";

    Exponent half= n >> 1;

    // If half is 0 then n must be 1 and the result is a
    if (half == 0)
	return a;

    // compute power of downward rounded exponent and square the result
    Element value= recursive_multiply_and_square(a, half, op);
    value= op(value, value);

    // if odd another multiplication with a is needed
    if (n & 1)
	value= op(value, a);
    return value;
}

// {Op, Element} must be a SemiGroup
template <typename Op, typename Element, typename Exponent>
  _GLIBCXX_WHERE( math::SemiGroup<Op, Element>
            && std::Integral<Exponent> )
inline Element multiply_and_square_horner(const Element& a, Exponent n, Op op)
{
    if (n <= 0) throw "In multiply_and_square_horner: exponent must be greater than 0";

    // Set mask to highest bit
    Exponent mask= 1 << (8 * sizeof(mask) - 1);

    // If this is a negative number right shift can insert 1s instead of 0s -> infinite loop
    // Therefore we take the 2nd-highest bit
    if (mask < 0)
	mask= 1 << (8 * sizeof(mask) - 2);

    // find highest 1 bit
    while(!(bool)(mask & n)) mask>>= 1;

    Element value= a;
    for (mask>>= 1; mask; mask>>= 1) {
	value= op(value, value);
	if (n & mask)
	    value= op(value, a);
    }
    return value;
}

// {Op, Element} must be a Monoid
template <typename Op, typename Element, typename Exponent>
  _GLIBCXX_WHERE( math::Monoid<Op, Element>
            && std::Integral<Exponent> )
inline Element multiply_and_square(const Element& a, Exponent n, Op op)
{
    // Same as the simpler form except that the first multiplication is made before
    // the loop and one squaring is saved this way
    if (n < 0) throw "In multiply_and_square: negative exponent";

    using math::identity;
    Element value= bool(n & 1) ? Element(a) : Element(identity(op, a)), square= a;

    for (n>>= 1; n > 0; n>>= 1) {
	square= op(square, square);
	if (n & 1)
	    value= op(value, square);
    }
    return value;
}

// {Op, Element} must be a Monoid
template <typename Op, typename Element, typename Exponent>
  _GLIBCXX_WHERE( math::Monoid<Op, Element>
            && std::Integral<Exponent> )
inline Element multiply_and_square_simple(const Element& a, Exponent n, Op op)
{
    if (n < 0) throw "In multiply_and_square: negative exponent";

    using math::identity;
    Element value= identity(op, a), square= a;
    for (; n > 0; n>>= 1) {
	if (n & 1)
	    value= op(value, square);
	square= op(square, square);
    }
    return value;
}

template <typename Iter, typename Value, typename Op>
  _GLIBCXX_WHERE( std::ForwardIterator<Iter>
                  && std::Convertible<Value, std::ForwardIterator<Iter>::value_type>
		  && math::Magma<Op, std::ForwardIterator<Iter>::value_type> )
typename std::ForwardIterator<Iter>::value_type
inline accumulate_simple(Iter first, Iter last, Value init, Op op)
{
    typedef typename std::RandomAccessIterator<Iter>::value_type value_type;
    value_type        t0= init;

    // std::cout << "accumulate_simple\n";
    for (; first != last; ++first)
	t0= op(t0, *first);
    return t0;
}

template <typename Iter, typename Value, typename Op>
  _GLIBCXX_WHERE( std::RandomAccessIterator<Iter>
	    && std::Convertible<Value, std::RandomAccessIterator<Iter>::value_type>
	    && math::CommutativeMonoid<Op, std::RandomAccessIterator<Iter>::value_type> )
typename std::RandomAccessIterator<Iter>::value_type
inline accumulate_unrolled(Iter first, Iter last, Value init, Op op)
{
    typedef typename std::RandomAccessIterator<Iter>::value_type value_type;
    typedef typename std::RandomAccessIterator<Iter>::difference_type difference_type;
    value_type        t0= identity(op, init), t1= identity(op, init), t2= identity(op, init), t3= init;
    difference_type size= last - first, bsize= size >> 2 << 2, i;

    for (i= 0; i < bsize; i+= 4) {
	t0= op(t0, first[i]);
	t1= op(t1, first[i+1]);
	t2= op(t2, first[i+2]);
	t3= op(t3, first[i+3]);
    }
    for (; i < size; i++)
	t0= op(t0, first[i]);
    return op(op(t0, t1), op(t2, t3));
}

// Dispatching between simple and unrolled version
template <typename Iter, typename Value, typename Op>
  _GLIBCXX_WHERE( std::ForwardIterator<Iter>
                  && std::Convertible<Value, std::ForwardIterator<Iter>::value_type>
		  && math::Magma<Op, std::ForwardIterator<Iter>::value_type> )
typename std::ForwardIterator<Iter>::value_type
inline accumulate(Iter first, Iter last, Value init, Op op)
{
    std::cout << "Simple accumulate\n";
    return accumulate_simple(first, last, init, op);
}

template <typename Iter, typename Value, typename Op>
  _GLIBCXX_WHERE( std::RandomAccessIterator<Iter>
	          && std::Convertible<Value, std::RandomAccessIterator<Iter>::value_type>
		  && math::CommutativeMonoid<Op, std::RandomAccessIterator<Iter>::value_type> )
typename std::RandomAccessIterator<Iter>::value_type
inline accumulate(Iter first, Iter last, Value init, Op op)
{
    std::cout << "Unrolled accumulate\n";
    return accumulate_unrolled(first, last, init, op);
}

// {Op, Element} must be a PartiallyInvertibleMonoid
// Element and results must be EqualityComparable
// Only
template <typename Op, typename Element>
  _GLIBCXX_WHERE( math::Closed2EqualityComparable<Op, Element>
	    && math::PartiallyInvertibleMonoid<Op, Element> )
inline int algebraic_division(const Element& v1, const Element& v2, Op op)
{
    using math::identity; using math::inverse; using math::is_invertible;

    if (!is_invertible(op, v2))
	throw "In algebraic division: v2 must be invertible!\n";

    // Temporaries to avoid redundant operations
    Element id= identity(op, v1),     // Identity
            iv2= inverse(op, v2),     // Inverse of v2
   	    tmp(v1);                             // Copy of v1, will be lessened until < id

    int counter= 0;
    for (; tmp != id; counter++)
	tmp= op(tmp, iv2);
    return counter;
}

// {Op, Element} must be a PartiallyInvertibleMonoid
// Element must be LessThanComparable
// Under construction w.r.t. semantic requirements, introduction of ordered group needed
template <typename Op, typename Element>
  _GLIBCXX_WHERE( math::Closed2LessThanComparable<Op, Element>
	    && math::PartiallyInvertibleMonoid<Op, Element> )
inline int ordered_algebraic_division(const Element& v1, const Element& v2, Op op)
{
    using math::identity; using math::inverse; using math::is_invertible;

    if (!is_invertible(op, v2))
	throw "In algebraic division: v2 must be invertible!\n";

    // Temporaries to avoid redundant operations
    Element id= identity(op, v1),     // Identity
            iv2= inverse(op, v2),     // Inverse of v2
   	    tmp(v1);                             // Copy of v1, will be lessened until < id

    if (v1 <= id) return 0;
    int counter= 0;
    for (; tmp > id; counter++)
	tmp= op(tmp, iv2);
    // counter only correct if tmp == id
    if (tmp < id)
	counter--;
    return counter;
}

#if 0

// {Iter*, Op} must be a CommutativeMonoid
struct sortedAccumulate_t {
  template <class Iter, class Op, class Comp>
  typename enable_if<glas::is_associative<typename std::iterator_traits<Iter>::value_type, Op>::value
                       && glas::is_commutative<typename std::iterator_traits<Iter>::value_type, Op>::value,
		     typename std::iterator_traits<Iter>::value_type>::type
  operator() (Iter first, Iter last, Op op, Comp comp) {
    std::cout << "sortedAccumulate_t\n";
    typedef typename std::iterator_traits<Iter>::value_type value_type;
    std::vector<value_type> tmp(first, last);
    std::sort(tmp.begin(), tmp.end(), comp);
    return std::accumulate(tmp.begin(), tmp.end(), glas::identity<value_type, Op>()(), op); }
} sortedAccumulate;

// {Iter*, Op} must be a Monoid
struct unsortedAccumulate_t {
  template <class Iter, class Op>
  typename std::iterator_traits<Iter>::value_type
  operator() (Iter first, Iter last, Op op) {
    std::cout << "unsortedAccumulate_t\n";
    typedef typename std::iterator_traits<Iter>::value_type value_type;
    return std::accumulate(first, last, glas::identity<value_type, Op>()(), op); }

  // Only for Compability
  template <class Iter, class Op, class Comp>
  typename std::iterator_traits<Iter>::value_type
  operator() (Iter first, Iter last, Op op, Comp) {
    return operator() (first, last, op); }
} unsortedAccumulate;

// {Iter*, Op} must be a Monoid
template <class Iter, class Op, class Comp>
inline typename std::iterator_traits<Iter>::value_type
trySortedAccumulate(Iter first, Iter last, Op op, Comp comp) {
  typedef typename std::iterator_traits<Iter>::value_type value_type;
  typename if_type<glas::is_associative<value_type, Op>::value && glas::is_commutative<value_type, Op>::value,
                   sortedAccumulate_t, unsortedAccumulate_t>::type  accumulate;
  // alternatively checking for structure
  //   typename if_type<glas::is_commutative_monoid<value_type, Op>::value,
  //                    sortedAccumulate_t, unsortedAccumulate_t>::type  accumulate;
  return accumulate(first, last, op, comp);
}

#endif // 0

} // namespace mtl

#endif // algebraic_functions_include


// floop16 generated by makeloops.py Thu Jun 30 16:44:56 2011

#include <blitz/vector2.h>
#include <blitz/array.h>
#include <random/uniform.h>
#include <blitz/benchext.h>

#ifdef BZ_HAVE_VALARRAY
 #define BENCHMARK_VALARRAY
#endif

#ifdef BENCHMARK_VALARRAY
#include <valarray>
#endif

BZ_NAMESPACE(blitz)
extern void sink();
BZ_NAMESPACE_END

BZ_USING_NAMESPACE(blitz)
BZ_USING_NAMESPACE(std)

#if defined(BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES)
 #define floop16_f77 floop16_f77_
 #define floop16_f77overhead floop16_f77overhead_
 #define floop16_f90 floop16_f90_
 #define floop16_f90overhead floop16_f90overhead_
#elif defined(BZ_FORTRAN_SYMBOLS_WITH_DOUBLE_TRAILING_UNDERSCORES)
 #define floop16_f77 floop16_f77__
 #define floop16_f77overhead floop16_f77overhead__
 #define floop16_f90 floop16_f90__
 #define floop16_f90overhead floop16_f90overhead__
#elif defined(BZ_FORTRAN_SYMBOLS_CAPS)
 #define floop16_f77 FLOOP16_F77
 #define floop16_f77overhead FLOOP16_F77OVERHEAD
 #define floop16_f90 FLOOP16_F90
 #define floop16_f90overhead FLOOP16_F90OVERHEAD
#endif

extern "C" {
  void floop16_f77(const int& N, float* y, float* x, float* a, float* b, float* c, const float& u);
  void floop16_f77overhead(const int& N, float* y, float* x, float* a, float* b, float* c, const float& u);
  void floop16_f90(const int& N, float* y, float* x, float* a, float* b, float* c, const float& u);
  void floop16_f90overhead(const int& N, float* y, float* x, float* a, float* b, float* c, const float& u);

}

void VectorVersion(BenchmarkExt<int>& bench, float u);
void ArrayVersion(BenchmarkExt<int>& bench, float u);
void ArrayVersion_unaligned(BenchmarkExt<int>& bench, float u);
void ArrayVersion_misaligned(BenchmarkExt<int>& bench, float u);
void ArrayVersion_index(BenchmarkExt<int>& bench, float u);
void doTinyVectorVersion(BenchmarkExt<int>& bench, float u);
void F77Version(BenchmarkExt<int>& bench, float u);
#ifdef FORTRAN_90
void F90Version(BenchmarkExt<int>& bench, float u);
#endif
#ifdef BENCHMARK_VALARRAY
void ValarrayVersion(BenchmarkExt<int>& bench, float u);
#endif

const int numSizes = 80;
const bool runvector=false; // no point as long as Vector is Array<1>

int main()
{
    int numBenchmarks = 5;
    if (runvector) numBenchmarks++;
#ifdef BENCHMARK_VALARRAY
    numBenchmarks++;
#endif
#ifdef FORTRAN_90
    numBenchmarks++;
#endif

    BenchmarkExt<int> bench("floop16: $x = $a+$b+$c; $y = $x+$c+u", numBenchmarks);

    bench.setNumParameters(numSizes);

    Array<int,1> parameters(numSizes);
    Array<long,1> iters(numSizes);
    Array<double,1> flops(numSizes);

    parameters=pow(pow(2.,0.25),tensor::i)+tensor::i;
    flops = 4 * parameters;
    iters = 100000000L / flops;
    iters = where(iters<2, 2, iters);
    cout << iters << endl;

    bench.setParameterVector(parameters);
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);
    bench.setDependentVariable("flops");
    bench.beginBenchmarking();

    float u = 0.39123982498157938742;

    ArrayVersion(bench, u);
    ArrayVersion_unaligned(bench, u);
    ArrayVersion_misaligned(bench, u);
    ArrayVersion_index(bench, u);
    //doTinyVectorVersion(bench, u);
    F77Version(bench, u);
#ifdef FORTRAN_90
    F90Version(bench, u);
#endif
#ifdef BENCHMARK_VALARRAY
    ValarrayVersion(bench, u);
#endif

    if(runvector)
      VectorVersion(bench, u);

    bench.endBenchmarking();

    bench.saveMatlabGraph("floop16.m");
    return 0;
}

template<class T>
void initializeRandomDouble(T* data, int numElements, int stride = 1)
{
    ranlib::Uniform<T> rnd;

    for (int i=0; i < numElements; ++i)
        data[size_t(i*stride)] = rnd.random();
}

template<class T>
void initializeRandomDouble(valarray<T>& data, int numElements, int stride = 1)
{
    ranlib::Uniform<T> rnd;

    for (int i=0; i < numElements; ++i)
        data[size_t(i*stride)] = rnd.random();
}

void VectorVersion(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("Vector<T>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Vector<float> y(N);
        initializeRandomDouble(y.data(), N);
        Vector<float> x(N);
        initializeRandomDouble(x.data(), N);
        Vector<float> a(N);
        initializeRandomDouble(a.data(), N);
        Vector<float> b(N);
        initializeRandomDouble(b.data(), N);
        Vector<float> c(N);
        initializeRandomDouble(c.data(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a+b+c; y = x+c+u;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("Array<T,1>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Array<float,1> y(N);
        initializeRandomDouble(y.dataFirst(), N);
        Array<float,1> x(N);
        initializeRandomDouble(x.dataFirst(), N);
        Array<float,1> a(N);
        initializeRandomDouble(a.dataFirst(), N);
        Array<float,1> b(N);
        initializeRandomDouble(b.dataFirst(), N);
        Array<float,1> c(N);
        initializeRandomDouble(c.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a+b+c; y = x+c+u;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_index(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("Array<T,1> (indexexpr.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Array<float,1> y(N);
        initializeRandomDouble(y.dataFirst(), N);
        Array<float,1> x(N);
        initializeRandomDouble(x.dataFirst(), N);
        Array<float,1> a(N);
        initializeRandomDouble(a.dataFirst(), N);
        Array<float,1> b(N);
        initializeRandomDouble(b.dataFirst(), N);
        Array<float,1> c(N);
        initializeRandomDouble(c.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a(tensor::i)+b(tensor::i)+c(tensor::i); y = x(tensor::i)+c(tensor::i)+u;;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_unaligned(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("Array<T,1> (unal.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

    Array<float,1> yfill(N+1);
    Array<float,1> y(yfill(Range(1,N)));
    initializeRandomDouble(y.dataFirst(), N);

    Array<float,1> xfill(N+1);
    Array<float,1> x(xfill(Range(1,N)));
    initializeRandomDouble(x.dataFirst(), N);

    Array<float,1> afill(N+1);
    Array<float,1> a(afill(Range(1,N)));
    initializeRandomDouble(a.dataFirst(), N);

    Array<float,1> bfill(N+1);
    Array<float,1> b(bfill(Range(1,N)));
    initializeRandomDouble(b.dataFirst(), N);

    Array<float,1> cfill(N+1);
    Array<float,1> c(cfill(Range(1,N)));
    initializeRandomDouble(c.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a+b+c; y = x+c+u;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_misaligned(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("Array<T,1> (misal.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

    Array<float,1> yfill(N+5);
    Array<float,1> y(yfill(Range(0,N+0-1)));
    initializeRandomDouble(y.dataFirst(), N);

    Array<float,1> xfill(N+5);
    Array<float,1> x(xfill(Range(1,N+1-1)));
    initializeRandomDouble(x.dataFirst(), N);

    Array<float,1> afill(N+5);
    Array<float,1> a(afill(Range(2,N+2-1)));
    initializeRandomDouble(a.dataFirst(), N);

    Array<float,1> bfill(N+5);
    Array<float,1> b(bfill(Range(3,N+3-1)));
    initializeRandomDouble(b.dataFirst(), N);

    Array<float,1> cfill(N+5);
    Array<float,1> c(cfill(Range(4,N+4-1)));
    initializeRandomDouble(c.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a+b+c; y = x+c+u;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

#ifdef BENCHMARK_VALARRAY
void ValarrayVersion(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("valarray<T>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        long iters = bench.getIterations();

        valarray<float> y(N);
        initializeRandomDouble(y, N);
        valarray<float> x(N);
        initializeRandomDouble(x, N);
        valarray<float> a(N);
        initializeRandomDouble(a, N);
        valarray<float> b(N);
        initializeRandomDouble(b, N);
        valarray<float> c(N);
        initializeRandomDouble(c, N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a+b+c; y = x+c+u;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
	  sink();
	}
        bench.stopOverhead();
    }

    bench.endImplementation();
}
#endif

void F77Version(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("Fortran 77");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        int iters = bench.getIterations();

        float* y = new float[N];
        initializeRandomDouble(y, N);
        float* x = new float[N];
        initializeRandomDouble(x, N);
        float* a = new float[N];
        initializeRandomDouble(a, N);
        float* b = new float[N];
        initializeRandomDouble(b, N);
        float* c = new float[N];
        initializeRandomDouble(c, N);

        bench.start();
        for (int iter=0; iter < iters; ++iter)
            floop16_f77(N, y, x, a, b, c, u);
        bench.stop();

        bench.startOverhead();
        for (int iter=0; iter < iters; ++iter)
            floop16_f77overhead(N, y, x, a, b, c, u);

        bench.stopOverhead();

        delete [] y;
        delete [] x;
        delete [] a;
        delete [] b;
        delete [] c;

    }

    bench.endImplementation();
}

#ifdef FORTRAN_90
void F90Version(BenchmarkExt<int>& bench, float u)
{
    bench.beginImplementation("Fortran 90");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        int iters = bench.getIterations();

        float* y = new float[N];
        initializeRandomDouble(y, N);
        float* x = new float[N];
        initializeRandomDouble(x, N);
        float* a = new float[N];
        initializeRandomDouble(a, N);
        float* b = new float[N];
        initializeRandomDouble(b, N);
        float* c = new float[N];
        initializeRandomDouble(c, N);

        bench.start();
        for (int iter=0; iter < iters; ++iter)
            floop16_f90(N, y, x, a, b, c, u);
        bench.stop();

        bench.startOverhead();
        for (int iter=0; iter < iters; ++iter)
            floop16_f90overhead(N, y, x, a, b, c, u);

        bench.stopOverhead();
        delete [] y;
        delete [] x;
        delete [] a;
        delete [] b;
        delete [] c;

    }

    bench.endImplementation();
}
#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file DiscreteFactorGraph.cpp
 *  @date Feb 14, 2011
 *  @author Duy-Nguyen Ta
 *  @author Frank Dellaert
 */

//#define ENABLE_TIMING
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <boost/make_shared.hpp>

namespace gtsam {

	// Explicitly instantiate so we don't have to include everywhere
	template class FactorGraph<DiscreteFactor> ;
	template class EliminationTree<DiscreteFactor> ;

	/* ************************************************************************* */
	DiscreteFactorGraph::DiscreteFactorGraph() {
	}

	/* ************************************************************************* */
	DiscreteFactorGraph::DiscreteFactorGraph(
			const BayesNet<DiscreteConditional>& bayesNet) :
			FactorGraph<DiscreteFactor>(bayesNet) {
	}

	/* ************************************************************************* */
	FastSet<Index> DiscreteFactorGraph::keys() const {
		FastSet<Index> keys;
		BOOST_FOREACH(const sharedFactor& factor, *this)
		if (factor) keys.insert(factor->begin(), factor->end());
		return keys;
	}

	/* ************************************************************************* */
	DecisionTreeFactor DiscreteFactorGraph::product() const {
		DecisionTreeFactor result;
		BOOST_FOREACH(const sharedFactor& factor, *this)
			if (factor) result = (*factor) * result;
		return result;
	}

	/* ************************************************************************* */
	double DiscreteFactorGraph::operator()(
			const DiscreteFactor::Values &values) const {
		double product = 1.0;
		BOOST_FOREACH( const sharedFactor& factor, factors_ )
			product *= (*factor)(values);
		return product;
	}

	/* ************************************************************************* */
	void DiscreteFactorGraph::print(const std::string& s,
			const IndexFormatter& formatter) const {
		std::cout << s << std::endl;
		std::cout << "size: " << size() << std::endl;
		for (size_t i = 0; i < factors_.size(); i++) {
			std::stringstream ss;
			ss << "factor " << i << ": ";
			if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
		}
	}

	/* ************************************************************************* */
	std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
	EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, size_t num) {

		// PRODUCT: multiply all factors
		tic(1, "product");
		DecisionTreeFactor product;
		BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors){
			product = (*factor) * product;
		}

		toc(1, "product");

		// sum out frontals, this is the factor on the separator
		tic(2, "sum");
		DecisionTreeFactor::shared_ptr sum = product.sum(num);
		toc(2, "sum");

		// now divide product/sum to get conditional
		tic(3, "divide");
		DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum));
		toc(3, "divide");
		tictoc_finishedIteration();

		return std::make_pair(cond, sum);
	}

/* ************************************************************************* */
} // namespace


#include "base/py.h"
#include <cmath>
#include <boost/python.hpp>
#include "math/py_math.h"
#include "render/py_render.h"

using namespace boost::python;

BOOST_PYTHON_MODULE(negine_core)
{
    init_py_math();
    init_py_render();
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_INCLUDE_CONSTANTS_EPS_RELATED_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_INCLUDE_CONSTANTS_EPS_RELATED_HPP_INCLUDED

#include <boost/simd/constant/include/constants/eps.hpp>
#include <boost/simd/constant/include/constants/halfeps.hpp>
#include <boost/simd/constant/include/constants/sqrteps.hpp>
#include <boost/simd/constant/include/constants/mlogeps2.hpp>
#include <boost/simd/constant/include/constants/threeeps.hpp>
#include <boost/simd/constant/include/constants/fourthrooteps.hpp>
#include <boost/simd/constant/include/constants/thirdrooteps.hpp>
#include <boost/simd/constant/include/constants/smallestposval.hpp>
#include <boost/simd/constant/include/constants/sqrtsmallestposval.hpp>
#include <boost/simd/constant/include/constants/mindenormal.hpp>

#endif


//
// Created by haohanwang on 2/24/16.
//

#include "AdaMultiLasso.hpp"

#include <Eigen/Dense>
#include <stdexcept>
#include <unordered_map>

using namespace std;
using namespace Eigen;

AdaMultiLasso::AdaMultiLasso() {
    lambda1 = default_lambda1;
    lambda2 = default_lambda2;
    mu = default_mu;
    initTrainingFlag = false;
    logisticFlag = false;
}

AdaMultiLasso::AdaMultiLasso(const unordered_map<string, string>& opts) {
    try {
        lambda1 = stod(opts.at("lambda"));
    } catch (std::out_of_range& oor) {
        lambda1 = default_lambda1;
    }
    try {
        lambda2 = stod(opts.at("gamma"));
    } catch (std::out_of_range& oor) {
        lambda2 = default_lambda2;
    }
    try {
        mu = stod(opts.at("mu"));
    } catch (std::out_of_range& oor) {
        mu = default_mu;
    }
    initTrainingFlag = false;
    logisticFlag = false;
}

void AdaMultiLasso::setAttributeMatrix(const string& str, MatrixXf* Z) {
    if (str == "snpsFeatures1" || str == "snpsFeature1") {
        setSnpsFeatures1(*Z);
    } else if (str == "snpsFeatures2" || str == "snpsFeature2") {
        setSnpsFeatures2(*Z);
    } else if (str == "snpsFeatures" || str == "snpsFeature") {
        setSnpsFeatures(*Z);
    } else {
//        throw runtime_error("AdaMultiLasso models have no attribute with name" + str);
        std::clog << "AdaMultiLasso models have no attribute with name " << str << endl;
    }
}

void AdaMultiLasso::setLambda1(float d) {
    lambda1 = d;
}

void AdaMultiLasso::setLambda2(float d) {
    lambda2 = d;
}

void AdaMultiLasso::setSnpsFeatures1(MatrixXf xd) {
    snpsFeatures1 = xd;
}

void AdaMultiLasso::setSnpsFeatures2(MatrixXf xd) {
    snpsFeatures2 = xd;
}

void AdaMultiLasso::setSnpsFeatures(MatrixXf xd) {
    setSnpsFeatures1(xd);
    setSnpsFeatures2(xd);
}

MatrixXf AdaMultiLasso::getSnpsFeatures1() {
    return snpsFeatures1;
}

MatrixXf AdaMultiLasso::getSnpsFeatures2() {
    return snpsFeatures2;
}

VectorXf AdaMultiLasso::getW() {
    return w;
}

VectorXf AdaMultiLasso::getV() {
    return v;
}

void AdaMultiLasso::updateW(VectorXf xd) {
    w = xd;
}

void AdaMultiLasso::updateV(VectorXf xd) {
    v = xd;
}

VectorXf AdaMultiLasso::gradient_w() {
    long c = snpsFeatures1.cols();
    long k = beta.cols();
    VectorXf grad = VectorXf::Zero(c);
    for (long j=0;j<c;j++){
        grad(j) += (-k*snpsFeatures1.col(j).array()/theta.array()).sum();
        grad(j) += (snpsFeatures1.col(j).transpose()*(beta.array().abs().matrix())).array().sum();
    }
    return grad;
}

VectorXf AdaMultiLasso::gradient_v() {
    long c = snpsFeatures2.cols();
    long k = beta.cols();
    VectorXf grad = VectorXf::Zero(c);
    for (long j=0;j<c;j++){
        grad(j) += (-k*snpsFeatures1.col(j).array()/theta.array()).sum();
        grad(j) += (snpsFeatures1.col(j).transpose()*(beta.array().abs().matrix())).array().sum();  // float check this shortcut
    }
    return grad;
}

void AdaMultiLasso::updateTheta() {
    long c = snpsFeatures1.rows();
    theta = VectorXf::Zero(c);
    for (long j=0; j<c; j++){
        theta(j) = snpsFeatures1.row(j)*w;;
    }
}

void AdaMultiLasso::updateRho() {
    long c = snpsFeatures2.rows();
    rho = VectorXf::Zero(c);
    for (long j=0; j<c; j++){
        rho(j) = snpsFeatures2.row(j)*v;
    }
}

void AdaMultiLasso::updateTheta_Rho() {
    this->updateTheta();
    this->updateRho();
}

void AdaMultiLasso::initTheta() {
    w = VectorXf::Ones(snpsFeatures1.cols());
    long r = snpsFeatures1.rows();
    theta = VectorXf::Zero(r);
    for (long j=0; j<r; j++){
        theta(j) = snpsFeatures1.row(j)*w;
    }
}

void AdaMultiLasso::initRho() {
    v = VectorXf::Ones(snpsFeatures2.cols());
    long r = snpsFeatures2.rows();
    rho = VectorXf::Zero(r);
    for (long j=0; j<r; j++){
        rho(j) = snpsFeatures2.row(j)*v;
    }
}

VectorXf AdaMultiLasso::getTheta() {
    return theta;
}

VectorXf AdaMultiLasso::getRho() {
    return rho;
}

MatrixXf AdaMultiLasso::getBeta() {
    long r = beta.rows()/taskNum;
    long c = taskNum;
    MatrixXf tmp = MatrixXf::Zero(r, c);
    for (long i=0;i<r;i++){
        for (long j=0;j<c;j++){
            tmp(i,j) = beta(i*taskNum+j, 0);
        }
    }
    return tmp;
}

MatrixXf AdaMultiLasso::getFormattedBeta() {
    return beta;
}

void AdaMultiLasso::setX(MatrixXf xd) {
    X = xd;
}

void AdaMultiLasso::setY(MatrixXf yd) {
    y = yd;
}

void AdaMultiLasso::setXY(MatrixXf m, MatrixXf n) {
    X = m;
    y = n;
}

void AdaMultiLasso::initBeta() {
    long c = X.cols();
    long d = y.cols();
    beta = MatrixXf::Random(c, d);
}

float AdaMultiLasso::cost() {
    initTraining();
    if (logisticFlag){
        return 0.5*(y - (X * beta).unaryExpr(&sigmoid)).squaredNorm() + penalty_cost();
    }
    else{
        return 0.5*(y - X * beta).squaredNorm() + penalty_cost();
    }

}

float AdaMultiLasso::penalty_cost() {
    MatrixXf rb = getBeta();
    long c = rb.rows();
    float result = 0;
    VectorXf theta = getTheta()*lambda1;
    VectorXf rho = getRho()*lambda2;
    for (long i=0;i<c;i++){
        result += theta(i)*rb.row(i).lpNorm<1>() + rho(i)*rb.row(i).norm();
    }
    return result;
}

void AdaMultiLasso::assertReadyToRun() {
    // X and Y must be compatible
	if (!((X.rows() > 0) && (X.rows() == y.rows())
		&& (X.cols() > 0) && (y.cols() > 0))) {
		throw runtime_error("X and Y matrices of size (" + to_string(X.rows()) + "," + to_string(X.cols()) + "), and (" +
            to_string(y.rows()) + "," + to_string(y.cols()) + ") are not compatible.");
	}
    // SNPs features must have p rows (the number of markers)
	if (!((snpsFeatures1.rows() > 0) && (snpsFeatures1.cols() > 0)
		&& (snpsFeatures2.rows() > 0) && (snpsFeatures2.cols() > 0)
        && (snpsFeatures1.rows() == X.cols()) && (snpsFeatures2.rows() == X.cols()))) {
		throw runtime_error("SNP Feature matrices of size (" + to_string(snpsFeatures1.rows()) +
            ", " + to_string(snpsFeatures1.cols()) + "), and (" + to_string(snpsFeatures2.rows()) +
            ", " + to_string(snpsFeatures2.cols()) + ") are not compatible with X of size (" +
            to_string(X.rows()) + ", " + to_string(X.cols()) + ").");
	}
    checkLogisticRegression();
}

void AdaMultiLasso::initTraining() {
    if (!initTrainingFlag){
        initTrainingFlag = true;
        initTheta();
        initRho();
        long n = X.rows();
        long c = X.cols();
        taskNum = y.cols();
        MatrixXf tmpX = MatrixXf::Zero(n*taskNum, c*taskNum);
        MatrixXf tmpY = MatrixXf::Zero(n*taskNum, 1);
        VectorXf tmpT = VectorXf::Zero(c*taskNum);
        VectorXf tmpR = VectorXf::Zero(c*taskNum);
        MatrixXf tmpS1 = MatrixXf::Zero(c*taskNum, snpsFeatures1.cols());
        MatrixXf tmpS2 = MatrixXf::Zero(c*taskNum, snpsFeatures2.cols());
        for (long j=0;j<taskNum;j++){
            for (long k=0;k<c;k++){
                tmpT(k*taskNum+j) = theta(k);
                tmpR(k*taskNum+j) = rho(k);
                tmpS1.row(k*taskNum+j) = snpsFeatures1.row(k);
                tmpS2.row(k*taskNum+j) = snpsFeatures2.row(k);
            }
        }
        for (long i=0;i<n;i++){
            for (long j=0;j<taskNum;j++){
                for (long k=0;k<c;k++){
                    tmpX(i*taskNum+j, k*taskNum+j) = X(i, k);
                }
                tmpY(i*taskNum+j, 0) = y(i, j);
            }
        }
        X = tmpX;
        y = tmpY;
        theta = tmpT;
        rho = tmpR;
        snpsFeatures1 = tmpS1;
        snpsFeatures2 = tmpS2;
        initBeta();
        initC();
        L = ((X.transpose()*X).eigenvalues()).real().maxCoeff() + C.norm()/mu;
    }
}

void AdaMultiLasso::initC() {
    long c = X.cols();
    C = MatrixXf::Zero(c * taskNum, c);
    for (long i = 0; i < c; i++) {
        for (long j = 0; j < taskNum; j++) {
            C(i*j+j, i) = rho(i)*lambda2;
        }
    }
}

MatrixXf AdaMultiLasso::proximal_derivative() {
    long r = beta.rows();
    long c = beta.cols();
    MatrixXf A = MatrixXf::Zero(r*taskNum, c);
    MatrixXf tmp = C*beta;
    for (long i=0;i<r*taskNum;i++){
        A.row(i) = Math::getInstance().L2Thresholding(tmp.row(i)/mu);
    }
    if (logisticFlag){
        return X.transpose()*((X*beta).unaryExpr(&sigmoid)-y) + C.transpose()*A;
    }
    else{
        return X.transpose()*(X*beta-y) + C.transpose()*A;
    }
}

MatrixXf AdaMultiLasso::proximal_operator(MatrixXf in, float lr) {
    MatrixXf sign = ((in.array()>0).matrix()).cast<float>();
    sign += -1.0*((in.array()<0).matrix()).cast<float>();
    in = ((in.array().abs()-lr*lambda1*theta.array()).max(0)).matrix();
    return (in.array()*sign.array()).matrix();
}

float AdaMultiLasso::getL() {
    return L;
}

VectorXf AdaMultiLasso::projection(VectorXf in) {
    VectorXf a = in;
    sort(a.data(), a.data()+a.size());
    long l = a.size();
    float I = 0;
    float S = 0;
    for (long i=0;i<l;i++){
        float ss = 0;
        for (long j=i;j<l;j++) {
            ss += a(j);
        }
        if (ss - 1 < a(i)*(l-i)){
            S = ss - 1;
            I = l-i;
            break;
        }
    }
    float t = S/I;
    VectorXf r = ((in.array() - t).max(0)).matrix();
    return r;
}


#include <boost/test/unit_test.hpp>
#include <shz/math/intersection.hpp>

using namespace shz::math;

BOOST_AUTO_TEST_CASE(ray_planeIntersection)
{
	vector<shz::math::f32, 3> o;
	o.data[0] = 0;
	o.data[1] = 1;
	o.data[2] = 0;
	vector<shz::math::f32, 3> d;
	d.data[0] = 0;
	d.data[1] = 0;
	d.data[2] = 1;
	ray<shz::math::f32> test_ray(o, d);
	plane<shz::math::f32> test_plane(0, 1, 0, 0);

	shz::math::intersector<ray<shz::math::f32>, plane<shz::math::f32>> check;
	BOOST_CHECK(!check(test_ray, test_plane));
	BOOST_CHECK(check.state == IntersectState::OUTSIDE);

	test_ray.origin.data[0] = 0;
	test_ray.origin.data[1] = 0;
	test_ray.origin.data[2] = 0;
	test_ray.direction.data[0] = 0;
	test_ray.direction.data[1] = 0;
	test_ray.direction.data[2] = 1;
	BOOST_CHECK(check(test_ray, test_plane));
	BOOST_CHECK(check.state == IntersectState::INSIDE); // Parallel, degenerated solution

	test_ray.origin.data[0] = 0;
	test_ray.origin.data[1] = 1;
	test_ray.origin.data[2] = 0;
	test_ray.direction.data[0] = 0;
	test_ray.direction.data[1] = -1;
	test_ray.direction.data[2] = 0;
	BOOST_CHECK(check(test_ray, test_plane));
	BOOST_CHECK(check.state == IntersectState::PARTIAL_INSIDE);
	BOOST_CHECK_CLOSE(check.length, 1.0, 0.00001);
}

BOOST_AUTO_TEST_CASE(ray_bboxIntersection)
{
	vector<shz::math::f32, 3> o, d;
	o.data[0] = 0;
	o.data[1] = 2;
	o.data[2] = 0;
	d.data[0] = 0;
	d.data[1] = 1;
	d.data[2] = 0;
	ray<shz::math::f32> test_ray(o, d);
	bbox<shz::math::f32> test_bbox(-1, -1, -1, 1, 1, 1);

	shz::math::intersector<ray<shz::math::f32>, bbox<shz::math::f32>> check;
	BOOST_CHECK(!check(test_ray, test_bbox));
	BOOST_CHECK(check.state == IntersectState::OUTSIDE);

	o.data[0] = 0;
	o.data[1] = -2;
	o.data[2] = 0;
	test_ray.origin = o;
	test_ray.direction = d;
	BOOST_CHECK(check(test_ray, test_bbox));
	BOOST_CHECK(check.state == IntersectState::PARTIAL_INSIDE);
	BOOST_CHECK_CLOSE(check.length, 1.f, 0.0001f);

	o.data[0] = 0;
	o.data[1] = 0;
	o.data[2] = 0;
	test_ray.origin = o;
	BOOST_CHECK(check(test_ray, test_bbox));
	BOOST_CHECK(check.state == IntersectState::INSIDE);
}

BOOST_AUTO_TEST_CASE(ray_sphereIntersection)
{
	ray<shz::math::f32> test_ray;
	sphere<shz::math::f32> test_sphere;
	test_ray.origin.data[0] = 0;
	test_ray.origin.data[1] = 0;
	test_ray.origin.data[2] = 0;
	test_ray.direction.data[0] = 0;
	test_ray.direction.data[1] = 1;
	test_ray.direction.data[2] = 0;
	test_ray.length = 10;
	test_sphere.center.data[0] = 0;
	test_sphere.center.data[1] = 0;
	test_sphere.center.data[2] = 0;
	test_sphere.radius = 1;

	shz::math::intersector<ray<shz::math::f32>, sphere<shz::math::f32>> check;
	BOOST_CHECK(check(test_ray, test_sphere));
	BOOST_CHECK(check.state == IntersectState::INSIDE);

	test_sphere.center.data[0] = 0;
	test_sphere.center.data[1] = 5;
	test_sphere.center.data[2] = 0;
	test_sphere.radius = 1;
	BOOST_CHECK(check(test_ray, test_sphere));

	shz::math::intersector<ray<shz::math::f32>, sphere<shz::math::f32>, shz::math::partial_resolve_intersector> partial_check;
	test_sphere.center.data[0] = 0;
	test_sphere.center.data[1] = 5;
	test_sphere.center.data[2] = 0;
	test_sphere.radius = 1;
	BOOST_CHECK(partial_check(test_ray, test_sphere));
}


#ifndef CPCA_HPP
#define CPCA_HPP

#include <Eigen/Core>
#include <vector>

class CPCA {
public:
  CPCA(Eigen::Index const nComponents = 2, bool const standardize = true);

  void initialize();
  Eigen::MatrixXf
  fitTransform(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
               bool const autoAlphaSelection = true, float const alpha = 0.0f,
               float const eta = 1e-3f, float const convergenceRatio = 1e-2f,
               unsigned int const maxIter = 10, bool const keepReports = false);
  void fit(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
           bool const autoAlphaSelection = true, float const alpha = 0.0f,
           float const eta = 1e-3f, float const convergenceRatio = 1e-2f,
           unsigned int maxIter = 10, bool const keepReports = false);
  void fitWithManualAlpha(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
                          float const alpha);
  void fitWithBestAlpha(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
                        float const initAlpha = 0.0f, float const eta = 1e-3f,
                        float const convergenceRatio = 1e-2f,
                        unsigned int const maxIter = 10,
                        bool const keepReports = false);
  void updateComponents(float const alpha);
  Eigen::MatrixXf transform(Eigen::MatrixXf const &X);
  float bestAlpha(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
                  float const initAlpha = 0.0f, float const eta = 1e-3f,
                  float const convergenceRatio = 1e-2f,
                  unsigned int const maxIter = 10,
                  bool const keepReports = false);
  std::vector<float> logspace(float const start, float const end,
                              unsigned int const num, float const base = 10.0f);
  Eigen::MatrixXf getComponents() { return components_; }
  Eigen::VectorXf getComponent(Eigen::Index const index) {
    return components_.col(index);
  }
  Eigen::RowVectorXf getEigenvalues() { return eigenvalues_; }
  float getEigenvalue(Eigen::Index const index) {
    return eigenvalues_(index);
  }
  Eigen::MatrixXf getLoadings() { return loadings_; }
  Eigen::VectorXf getLoading(Eigen::Index const index) {
    return loadings_.col(index);
  }
  Eigen::MatrixXf getCurrentFg() { return fg_; }
  Eigen::MatrixXf getDiffCov(float const alpha) {
    return fgCov_ - alpha * bgCov_;
  }
  float getBestAlpha() { return bestAlpha_; }
  std::vector<float> getReports() { return reports_; }

private:
  Eigen::Index nComponents_;
  bool standardize_;
  Eigen::MatrixXf components_;
  Eigen::RowVectorXf eigenvalues_;
  Eigen::MatrixXf loadings_;
  Eigen::MatrixXf fg_;
  Eigen::MatrixXf bg_;
  Eigen::MatrixXf fgCov_;
  Eigen::MatrixXf bgCov_;
  Eigen::MatrixXf affinityMat_;
  std::vector<float> reports_;
  float bestAlpha_;

  std::vector<float> findSpectalAlphas(unsigned int const nAlphasToReturn,
                                       unsigned int const nAlphas,
                                       float const maxLogAlpha);
  Eigen::MatrixXf createAffinityMatrix(Eigen::MatrixXf const &X,
                                       unsigned int const nAlphas,
                                       float const maxLogAlpha);
};

#endif // CPCA_HPP


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/meta/float.hpp>
#include <boost/simd/sdk/meta/double.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_valmax Valmax
 *
 * \par Description
 * Constant Valmax, maximum value of a type.
 * \arg int8    127, uint8    255,
 * \arg int16 32767, uint16 65535,
 * \arg int32 2147483647, uint32 4294967295,
 * \arg int64 9223372036854775807, uint64 18446744073709551615,\arg float \f$\infty\f$, double \f$\infty\f$,
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/valmax.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::valmax_(A0)>::type
 *     Valmax();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Valmax
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Valmax of functor Valmax
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    struct Valmax : ext::pure_constant_<Valmax>
    {
      typedef double default_type;
      template<class Target, class Dummy=void>
      struct  apply
            : meta::int_c < typename Target::type
                          , typename Target::
                            type( typename Target::type(1)
                                  << (sizeof(typename Target::type)*CHAR_BIT-1)
                                )
                          >
      {};
    };

    template<class T, class Dummy>
    struct  Valmax::apply< boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<0x7F7FFFFF> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<0x7FEFFFFFFFFFFFFFULL> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::uint8_<T>,Dummy>
          : meta::int_c<boost::simd::uint8_t,0xFF> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::uint16_<T>,Dummy>
          : meta::int_c<boost::simd::uint16_t,0xFFFF> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::uint32_<T>,Dummy>
          : meta::int_c<boost::simd::uint32_t,0xFFFFFFFFUL> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::uint64_<T>,Dummy>
          : meta::int_c<boost::simd::uint64_t,0xFFFFFFFFFFFFFFFFULL> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::int8_<T>,Dummy>
          : meta::int_c<boost::simd::int8_t,127> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::int16_<T>,Dummy>
          : meta::int_c<boost::simd::int16_t,32767> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::int32_<T>,Dummy>
          : meta::int_c<boost::simd::int32_t,2147483647> {};

    template<class T, class Dummy>
    struct  Valmax::apply<boost::dispatch::meta::int64_<T>,Dummy>
          : meta::int_c<boost::simd::int64_t,9223372036854775807ULL> {};
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Valmax, Valmax)
} }

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

#endif


//
// Created by kerin on 2019-09-30.
//

#ifndef BGEN_PROG_RHE_REG_COMPONENT_HPP
#define BGEN_PROG_RHE_REG_COMPONENT_HPP

#include "genotype_matrix.hpp"
#include "file_utils.hpp"
#include "parameters.hpp"
#include "eigen_utils.hpp"
#include "mpi_utils.hpp"
#include "data.hpp"
#include "eigen_utils.hpp"

#include "tools/eigen3.3/Dense"

#include <boost/iostreams/filtering_stream.hpp>

#include <random>

class RHEreg_Component {
public:
	Eigen::MatrixXd _XXtWz;
	double ytXXty;
	long n_covar;
	long n_samples;
	long n_draws;
	long n_jacknife_local;
	long rm_jacknife_block;
	double n_var_local;
	parameters params;

	// Identifiers
	std::string label;
	bool is_gxe;
	bool is_active;
	bool is_finalised;
	std::string group;
	std::string effect_type;
	long env_var_index;

	// Storage for jacknife blocks
	std::vector<Eigen::MatrixXd> _XXtzs;
	std::vector<double> n_vars_local;
	std::vector<double> ytXXtys;

	Eigen::MatrixXd C, CtC_inv;
	Eigen::MatrixXd zz;
	Eigen::VectorXd Y;
	Eigen::VectorXd env_var;
	RHEreg_Component(const parameters& myparams,
	                 const Eigen::VectorXd& myY,
	                 const Eigen::MatrixXd& myWzz,
	                 const Eigen::MatrixXd& myC,
	                 const Eigen::MatrixXd& myCtC_inv,
	                 const long& myNJacknifeLocal);

	RHEreg_Component(const parameters& myparams,
	                 const Eigen::VectorXd& myY,
	                 const Eigen::MatrixXd& myC,
	                 const Eigen::MatrixXd& myCtC_inv,
	                 Eigen::MatrixXd XXtWz) : params(myparams), Y(myY),
		C(myC), CtC_inv(myCtC_inv),
		_XXtWz(XXtWz){
		// For aggregating components after genotypes have been streamed
		// ie no need for any of the zz data fields.
		n_jacknife_local = 1;
		n_draws = XXtWz.cols();
		n_samples = XXtWz.rows();
		n_covar = C.cols();

		ytXXty = 0;
		n_var_local = 0;
		label = "";
		is_active = true;
		is_gxe = false;
		is_finalised = false;
		rm_jacknife_block = -1;

		if(params.debug) {
			std::string ram = mpiUtils::currentUsageRAM();
			std::cout << "(" << ram << ")" << std::endl;
		}
	}

	void set_env_var(const Eigen::Ref<const Eigen::VectorXd>& my_env_var);

	void change_env_var(const Eigen::Ref<const Eigen::VectorXd>& my_env_var);

	void set_inactive();

	void add_to_trace_estimator(Eigen::Ref<Eigen::MatrixXd> X,
	                            long jacknife_index = 0);

	void finalise();

	Eigen::MatrixXd getXXtz() const;

	double get_bb_trace() const;

	double get_n_var_local() const;

	double operator*(const RHEreg_Component& other) const;

	Eigen::MatrixXd project_out_covars(Eigen::Ref<Eigen::MatrixXd> rhs) const;
};

void get_GxE_collapsed_component(const std::vector<RHEreg_Component> &vec_of_components,
                                 RHEreg_Component &new_comp,
                                 const Eigen::Ref<const Eigen::MatrixXd> &E,
                                 const Eigen::Ref<const Eigen::VectorXd> &env_weights,
                                 const std::vector<Eigen::MatrixXd> &ytEXXtEy,
                                 const bool& copy_jacknife_partitions = false);

void get_GxE_collapsed_system(const std::vector<RHEreg_Component> &vec_of_components,
                              std::vector<RHEreg_Component> &new_components,
                              const Eigen::Ref<const Eigen::MatrixXd> &E,
                              const Eigen::Ref<const Eigen::VectorXd> &env_weights,
                              const std::vector<Eigen::MatrixXd> &ytEXXtEy,
                              const bool& copy_jacknife_partitions = false);

#endif


#include <chrono>
#include <functional>
#include <iostream>
#include <random>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/simple_dtw.hpp>

// We're using Vector4d to make sure alignment is handled properly for builds
// using -march=native and fixed-size-vectorizable Eigen types.
using PointVector = common_robotics_utilities::math::VectorVector4d;
using PointVectorDTW = common_robotics_utilities::simple_dtw::SimpleDTW<
    Eigen::Vector4d, Eigen::Vector4d, PointVector, PointVector>;

PointVector SampleRandomPointVector(
    const size_t num_points, std::mt19937_64& prng,
    std::uniform_real_distribution<double>& distribution)
{
  std::cout << "Generating " << num_points << " random points" << std::endl;
  PointVector random_points(num_points);
  for (size_t idx = 0; idx < num_points; idx++)
  {
    const double x = distribution(prng);
    const double y = distribution(prng);
    const double z = distribution(prng);
    random_points.at(idx) = Eigen::Vector4d(x, y, z, 1.0);
  }
  return random_points;
}

int main(int, char**)
{
  // Set up PRNG and sampling distribution.
  const int64_t seed = 42;
  std::mt19937_64 prng(seed);
  std::uniform_real_distribution<double> dist(0.0, 10.0);

  // Set up test vectors.
  const PointVector test_vector1 = SampleRandomPointVector(12500, prng, dist);
  const PointVector test_vector2 = SampleRandomPointVector(15000, prng, dist);
  const PointVector test_vector3 = SampleRandomPointVector(17500, prng, dist);
  const PointVector test_vector4 = SampleRandomPointVector(20000, prng, dist);

  // Set up the distance function.
  const std::function<double(const Eigen::Vector4d&, const Eigen::Vector4d&)>
      distance_fn = [] (const Eigen::Vector4d& v1, const Eigen::Vector4d& v2)
  {
    return (v1 - v2).norm();
  };

  // Run a number of DTW queries without reusing the DTW matrix memory.
  const auto uncached_start_time = std::chrono::steady_clock::now();

  {
    const auto point_vector_dtw_distance_fn
        = [&] (const PointVector& vec1, const PointVector& vec2)
    {
      return common_robotics_utilities::simple_dtw::EvaluateWarpingCost<
          Eigen::Vector4d, Eigen::Vector4d, PointVector, PointVector>(
              vec1, vec2, distance_fn);
    };

    const auto vector1_self_distance =
        point_vector_dtw_distance_fn(test_vector1, test_vector1);
    std::cout << "vector1 self-distance = " << vector1_self_distance
              << std::endl;

    const auto vector2_self_distance =
        point_vector_dtw_distance_fn(test_vector2, test_vector2);
    std::cout << "vector2 self-distance = " << vector2_self_distance
              << std::endl;

    const auto vector3_self_distance =
        point_vector_dtw_distance_fn(test_vector3, test_vector3);
    std::cout << "vector3 self-distance = " << vector3_self_distance
              << std::endl;

    const auto vector4_self_distance =
        point_vector_dtw_distance_fn(test_vector4, test_vector4);
    std::cout << "vector4 self-distance = " << vector4_self_distance
              << std::endl;

    const auto vector1_vector2_distance =
        point_vector_dtw_distance_fn(test_vector1, test_vector2);
    std::cout << "vector1-vector2 distance = " << vector1_vector2_distance
              << std::endl;

    const auto vector2_vector3_distance =
        point_vector_dtw_distance_fn(test_vector2, test_vector3);
    std::cout << "vector2-vector3 distance = " << vector2_vector3_distance
              << std::endl;

    const auto vector3_vector4_distance =
        point_vector_dtw_distance_fn(test_vector3, test_vector4);
    std::cout << "vector3-vector4 distance = " << vector3_vector4_distance
              << std::endl;

    const auto vector4_vector1_distance =
        point_vector_dtw_distance_fn(test_vector4, test_vector1);
    std::cout << "vector4-vector1 distance = " << vector4_vector1_distance
              << std::endl;
  }

  const auto uncached_end_time = std::chrono::steady_clock::now();
  const double uncached_elapsed = std::chrono::duration<double>(
      uncached_end_time - uncached_start_time).count();

  std::cout << "DTW queries runtime: " << uncached_elapsed
            << " seconds (without memory reuse)" << std::endl;

  // Run the same set of DTW queries reusing the DTW matrix memory.
  const auto cached_start_time = std::chrono::steady_clock::now();

  {
    // Create a DTW evaluator with the known maximum sequence size.
    PointVectorDTW dtw_evaluator(2000, 2000);

    const auto vector1_self_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector1, test_vector1, distance_fn);
    std::cout << "vector1 self-distance = " << vector1_self_distance
              << std::endl;

    const auto vector2_self_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector2, test_vector2, distance_fn);
    std::cout << "vector2 self-distance = " << vector2_self_distance
              << std::endl;

    const auto vector3_self_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector3, test_vector3, distance_fn);
    std::cout << "vector3 self-distance = " << vector3_self_distance
              << std::endl;

    const auto vector4_self_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector4, test_vector4, distance_fn);
    std::cout << "vector4 self-distance = " << vector4_self_distance
              << std::endl;

    const auto vector1_vector2_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector1, test_vector2, distance_fn);
    std::cout << "vector1-vector2 distance = " << vector1_vector2_distance
              << std::endl;

    const auto vector2_vector3_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector2, test_vector3, distance_fn);
    std::cout << "vector2-vector3 distance = " << vector2_vector3_distance
              << std::endl;

    const auto vector3_vector4_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector3, test_vector4, distance_fn);
    std::cout << "vector3-vector4 distance = " << vector3_vector4_distance
              << std::endl;

    const auto vector4_vector1_distance = dtw_evaluator.EvaluateWarpingCost(
        test_vector4, test_vector1, distance_fn);
    std::cout << "vector4-vector1 distance = " << vector4_vector1_distance
              << std::endl;
  }

  const auto cached_end_time = std::chrono::steady_clock::now();
  const double cached_elapsed = std::chrono::duration<double>(
      cached_end_time - cached_start_time).count();

  std::cout << "DTW queries runtime: " << cached_elapsed
            << " seconds (with memory reuse)" << std::endl;

  return 0;
}