/*
 * 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 <graphblas/graphblas.hpp>
#include <algorithms/bfs.hpp>

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE bfs_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
/// @todo Use a dense matrix type?
namespace
{
    template <typename T>
    std::vector<std::vector<T> > get_tn_answer(T const &INF)
    {
        return std::vector<std::vector<T> >(
            {{  0,   3,   4,   0,   3,   2,   3, INF,   4},
             {  3,   1,   6,   1,   3,   2,   1, INF,   2},
             {  3,   6,   2,   4,   2,   2,   2, INF,   2},
             {  3,   3,   4,   3,   3,   2,   3, INF,   4},
             {  3,   3,   4,   4,   4,   2,   2, INF,   4},
             {  3,   6,   5,   4,   2,   5,   2, INF,   2},
             {  3,   6,   6,   6,   2,   2,   6, INF,   2},
             {INF, INF, INF, INF, INF, INF, INF,   7, INF},
             {  3,   3,   8,   4,   8,   2,   2, INF,   8}});
    }
    /*
     *
     * template:
    template <typename T>
    Matrix<T, DirectedMatrixTag> get_tn_answer(T const &INF)
    {
        std::vector<grb::IndexType> rows = {
        };
        std::vector<grb::IndexType> cols = {
        };
        std::vector<T> vals = {
        };

        Matrix<T, DirectedMatrixTag> temp(9,9, INF);
        buildmatrix(temp, rows.begin(), cols.begin(), vals.begin(), rows.size());
        return temp;
    }
    */
    //template <typename T>
    //grb::Matrix<T> get_tn_answer(T const &INF)
    //{
    //    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,   3,   4,   0,   3,   2,   3,   4,   3,   1,   6,   1,   3,
    //        2,   1,   2,   3,   6,   2,   4,   2,   2,   2,   2,   3,   3,
    //        4,   3,   3,   2,   3,   4,   3,   3,   4,   4,   4,   2,   2,
    //        4,   3,   6,   5,   4,   2,   5,   2,   2,   3,   6,   6,   6,
    //        2,   2,   6,   2,   7,   3,   3,   8,   4,   8,   2,   2,   8
    //    };

    //    grb::Matrix<T> temp(9,9, INF);
    //    grb::buildmatrix(temp, rows.begin(), cols.begin(), vals.begin(), rows.size());
    //    return temp;
    //}

    template <typename T>
    std::vector<std::vector<T> > get_gilbert_answer(T const &INF)
    {
        return std::vector<std::vector<T> >(
            {{  0,  0,  3,  0,  1,  2,  1},
             {  3,  1,  6,  6,  1,  4,  1},
             {INF,INF,  2,INF,INF,  2,INF},
             {  3,  0,  3,  3,  1,  2,  1},
             {INF,INF,  5,INF,  4,  4,INF},
             {INF,INF,  5,INF,INF,  5,INF},
             {  3,  0,  6,  6,  6,  2,  6}});
    }
    //template <typename T>
    //grb::Matrix<T> get_gilbert_answer(T const &INF)
    //{
    //    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, 0,   3, 0,   1,   2,   1,   3,   1,   6,   6,   1,   4,
    //        1,   2,   2,   3, 0,   3,   3,   1,   2,   1,   5,   4,   4,
    //        5,   5,   3, 0,   6,   6,   6,   2,   6
    //    };

    //    grb::Matrix<T> temp(9,9, INF);
    //    grb::buildmatrix(temp, rows.begin(), cols.begin(), vals.begin(), rows.size());
    //    return temp;
    //}
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_test_basic_one_root_source_index)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    grb::Vector<T> parent_list(NUM_NODES);
    algorithms::bfs(G_tn, START_INDEX, parent_list);

    T const INF(std::numeric_limits<T>::max());
    auto G_tn_answer(get_tn_answer(INF));
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (parent_list.hasElement(ix))
        {
            BOOST_CHECK_EQUAL(parent_list.extractElement(ix),
                              G_tn_answer[START_INDEX][ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(INF, G_tn_answer[START_INDEX][ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_test_basic_one_root)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    grb::Vector<T> root(NUM_NODES);
    root.setElement(START_INDEX, 0);

    grb::Vector<T> parent_list(NUM_NODES);
    algorithms::bfs(G_tn, root, parent_list);

    T const INF(std::numeric_limits<T>::max());
    auto G_tn_answer(get_tn_answer(INF));
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (parent_list.hasElement(ix))
        {
            BOOST_CHECK_EQUAL(parent_list.extractElement(ix),
                              G_tn_answer[START_INDEX][ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(INF, G_tn_answer[START_INDEX][ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_batch_test_basic_one_root)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBMatrix root(1, NUM_NODES);
    root.setElement(0, START_INDEX, 0);

    GrBMatrix parent_list(1, NUM_NODES);
    algorithms::bfs_batch(G_tn, root, parent_list);

    T const INF(std::numeric_limits<T>::max());
    auto G_tn_answer(get_tn_answer(INF));
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (parent_list.hasElement(0, ix))
        {
            BOOST_CHECK_EQUAL(parent_list.extractElement(0, ix),
                              G_tn_answer[START_INDEX][ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(INF, G_tn_answer[START_INDEX][ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_batch_test_basic_one_root_integer)
{
    using T = unsigned int;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBMatrix root(1, NUM_NODES);
    root.setElement(0, START_INDEX, 0);  // multiplicative identity

    GrBMatrix parent_list(1, NUM_NODES);
    algorithms::bfs_batch(G_tn, root, parent_list);

    T const INF(std::numeric_limits<T>::max());
    auto G_tn_answer(get_tn_answer(INF));
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (parent_list.hasElement(0, ix))
        {
            BOOST_CHECK_EQUAL(parent_list.extractElement(0, ix),
                              G_tn_answer[START_INDEX][ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(INF, G_tn_answer[START_INDEX][ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_batch_test_basic)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;
    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBMatrix roots(NUM_NODES, NUM_NODES);
    grb::IndexArrayType ii, jj, vv;
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        ii.push_back(ix);
        jj.push_back(ix);
        vv.push_back(1);
    }
    roots.build(ii, jj, vv);

    GrBMatrix parent_lists(NUM_NODES, NUM_NODES);

    algorithms::bfs_batch(G_tn, roots, parent_lists);

    /// @todo Use a dense matrix type?
    T const INF(std::numeric_limits<T>::max());
    auto G_tn_answer(get_tn_answer(INF));

    for (grb::IndexType r = 0; r < NUM_NODES; ++r)
    {
        for (grb::IndexType c = 0; c < NUM_NODES; ++c)
        {
            if (parent_lists.hasElement(r, c))
            {
                BOOST_CHECK_EQUAL(parent_lists.extractElement(r,c),
                                  G_tn_answer[r][c]);
            }
            else
            {
                BOOST_CHECK_EQUAL(INF, G_tn_answer[r][c]);
            }
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_batch_test_gilbert)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    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<T> v(i.size(), 1);

    GrBMatrix G_gilbert(NUM_NODES, NUM_NODES);
    G_gilbert.build(i, j, v);

    GrBMatrix roots(NUM_NODES, NUM_NODES);
    grb::IndexArrayType ii, jj, vv;
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        ii.push_back(ix);
        jj.push_back(ix);
        vv.push_back(1);
    }
    roots.build(ii, jj, vv);

    GrBMatrix parent_lists(NUM_NODES, NUM_NODES);

    algorithms::bfs_batch(G_gilbert, roots, parent_lists);

    /// @todo Replace with a dense matrix type
    T const INF(std::numeric_limits<T>::max());
    auto G_gilbert_answer(get_gilbert_answer(INF));
    for (grb::IndexType r = 0; r < NUM_NODES; ++r)
    {
        for (grb::IndexType c = 0; c < NUM_NODES; ++c)
        {
            if (parent_lists.hasElement(r, c))
            {
                BOOST_CHECK_EQUAL(parent_lists.extractElement(r,c),
                                  G_gilbert_answer[r][c]);
            }
            else
            {
                BOOST_CHECK_EQUAL(INF, G_gilbert_answer[r][c]);
            }
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_level_test_one_root_source)
{
    using T = bool;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), true);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    grb::Vector<grb::IndexType> levels(NUM_NODES);
    algorithms::bfs_level(G_tn, START_INDEX, levels);

    std::vector<grb::IndexType> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};

    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_level_test_one_root)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBMatrix root(1, NUM_NODES);
    root.setElement(0, START_INDEX, 1);  // multiplicative identity

    GrBMatrix levels(1, NUM_NODES);
    algorithms::bfs_level(G_tn, root, levels);

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

    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(0, ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(0, ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_level_test_one_root_integer)
{
    using T = unsigned int;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBMatrix root(1, NUM_NODES);
    root.setElement(0, START_INDEX, 1);  // multiplicative identity

    GrBMatrix levels(1, NUM_NODES);
    algorithms::bfs_level(G_tn, root, levels);

    std::vector<T> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(0, ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(0, ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_level_masked_test_one_root)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;
    using GrBVector = grb::Vector<T>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBVector root(NUM_NODES);
    root.setElement(START_INDEX, 1);  // multiplicative identity

    GrBVector levels(NUM_NODES);
    algorithms::bfs_level_masked(G_tn, root, levels);

    std::vector<T> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_level_masked_test_one_root_integer)
{
    using T = unsigned int;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;
    using GrBVector = grb::Vector<T>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBVector root(NUM_NODES);
    root.setElement(START_INDEX, 1);  // multiplicative identity

    GrBVector levels(NUM_NODES);
    algorithms::bfs_level_masked(G_tn, root, levels);

    std::vector<T> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(batch_bfs_level_masked_test_one_root)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBMatrix root(1, NUM_NODES);
    root.setElement(0, START_INDEX, 1);  // multiplicative identity

    GrBMatrix levels(1, NUM_NODES);
    algorithms::batch_bfs_level_masked(G_tn, root, levels);

    std::vector<T> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(0, ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(0, ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(batch_bfs_level_masked_test_one_root_integer)
{
    using T = unsigned int;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBMatrix root(1, NUM_NODES);
    root.setElement(0, START_INDEX, 1);  // multiplicative identity

    GrBMatrix levels(1, NUM_NODES);
    algorithms::batch_bfs_level_masked(G_tn, root, levels);

    std::vector<T> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(0, ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(0, ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_level_masked_v2_test_one_root)
{
    using T = double;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;
    using GrBVector = grb::Vector<T>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBVector root(NUM_NODES);
    root.setElement(START_INDEX, 1);  // multiplicative identity

    GrBVector levels(NUM_NODES);
    algorithms::bfs_level_masked_v2(G_tn, root, levels);

    std::vector<T> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(bfs_level_masked_v2_test_one_root_integer)
{
    using T = unsigned int;
    using GrBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;
    using GrBVector = grb::Vector<T>;

    grb::IndexType const NUM_NODES(9);
    grb::IndexType const START_INDEX(5);

    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<T> v(i.size(), 1);

    GrBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i, j, v);

    GrBVector root(NUM_NODES);
    root.setElement(START_INDEX, 1);  // multiplicative identity

    GrBVector levels(NUM_NODES);
    algorithms::bfs_level_masked_v2(G_tn, root, levels);

    std::vector<T> answer = {5, 4, 2, 4, 3, 1, 3, 0, 3};
    for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
    {
        if (levels.hasElement(ix))
        {
            BOOST_CHECK_EQUAL(levels.extractElement(ix),
                              answer[ix]);
        }
        else
        {
            BOOST_CHECK_EQUAL(0, answer[ix]);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "inner_node.h"
#include <iostream>
#include <unordered_map>
#include <boost/dynamic_bitset.hpp>
#include "encoding.pb.h"
#include "freq_node.h"
#include "status.h"

InnerNode::InnerNode(std::shared_ptr<FreqNode> l,
                     std::shared_ptr<FreqNode> r)
    : FreqNode(l->get_freq() + r->get_freq()),
      lc{l},
      rc{r} {}

Status InnerNode::find_next_char(const boost::dynamic_bitset<>& encoded_bits,
                                 size_t& bit_index,
                                 char& found) const {
    if (bit_index >= encoded_bits.size()) {
        return Status::kInvalid;
    }else if (encoded_bits[bit_index] == 0) {
        bit_index++;
        lc->find_next_char(encoded_bits, bit_index, found);
        return Status::kOk;
    } else {
        bit_index++;
        rc->find_next_char(encoded_bits, bit_index, found);
        return Status::kOk;
    }
}

void InnerNode::encode_node(boost::dynamic_bitset<> bits,
                            std::unordered_map<char, boost::dynamic_bitset<>>& encodings) const {
    bits.push_back(false);
    lc->encode_node(bits, encodings);

    bits.pop_back();
    bits.push_back(true);
    rc->encode_node(bits, encodings);
}

void InnerNode::to_proto(encode_decode::FreqNodeProto* proto_node) const {
    proto_node->set_type(encode_decode::FreqNodeProto_Type_INNER);
    proto_node->set_freq(this->get_freq());
    encode_decode::FreqNodeProto* proto_lc = proto_node->mutable_lc();
    encode_decode::FreqNodeProto* proto_rc = proto_node->mutable_rc();
    lc->to_proto(proto_lc);
    rc->to_proto(proto_rc);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - dist/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/arithmetic/include/functions/dist.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/real.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/constant/constant.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/splat.hpp>

#include <nt2/include/functions/load.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/include/functions/average.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/include/functions/extract.hpp>

NT2_TEST_CASE_TPL ( dist_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                              cT;
  typedef native<T ,ext_t>                             vT;
  typedef native<cT ,ext_t>                           vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;
  typedef vT r_t;
  double ulpd;
  ulpd=0.0;

  {
    NT2_TEST_EQUAL(nt2::dist(vcT(nt2::Inf<vT>(), nt2::Zero<vT>()), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], nt2::Nan<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vcT(nt2::Zero<vT>(),nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], nt2::Zero<vT>()[0]);
    NT2_TEST_ULP_EQUAL(nt2::dist(vcT(nt2::Zero<vT>(),nt2::One<vT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], nt2::Sqrt_2<vT>()[0], 0.5);
    NT2_TEST_EQUAL(nt2::dist(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], nt2::Zero<vT>()[0]);

    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::Inf<vciT>()), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], nt2::Inf<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::One<vciT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::Zero<vciT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], nt2::Zero<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::Zero<vciT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], nt2::One<vT>()[0]);
    NT2_TEST_ULP_EQUAL(nt2::dist(vciT(nt2::One<vciT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], nt2::Sqrt_2<vT>()[0], 0.5);

    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::Inf<vdT>()),  vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], nt2::Nan<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::One<vdT>()),  vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::Zero<vdT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], nt2::Zero<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::Zero<vdT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::One<vdT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], nt2::Zero<vT>()[0]);

    NT2_TEST_EQUAL(nt2::dist(nt2::Inf<vdT>(),   nt2::Inf<vciT>())[0] , nt2::Inf<vT>()[0] );
    NT2_TEST_EQUAL(nt2::dist(nt2::One<vdT>(),   nt2::Zero<vciT>())[0], nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(nt2::Zero<vdT>(),  nt2::Zero<vciT>())[0], nt2::Zero<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(nt2::Zero<vdT>(),  nt2::One<vciT>())[0] , nt2::One<vT>()[0]);
    NT2_TEST_ULP_EQUAL(nt2::dist(nt2::One<vdT>(),   nt2::One<vciT>())[0] , nt2::Sqrt_2<vT>()[0], 0.5);
  }
  {
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::Inf<vciT>()),   vciT(nt2::Inf<vciT>()))[0],  nt2::Nan<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::One<vciT>()),   vciT(nt2::Zero<vciT>()))[0], nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::Zero<vciT>()),  vciT(nt2::Zero<vciT>()))[0], nt2::Zero<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::Zero<vciT>()),  vciT(nt2::One<vciT>()))[0],  nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vciT(nt2::One<vciT>()),   vciT(nt2::One<vciT>()))[0],  nt2::Zero<vT>()[0]);
  }
  {
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::Inf<vdT>()),   vdT(nt2::Inf<vdT>()))[0],  nt2::Nan<vT>() [0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::One<vdT>()),   vdT(nt2::Zero<vdT>()))[0], nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::Zero<vdT>()),  vdT(nt2::Zero<vdT>()))[0], nt2::Zero<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::Zero<vdT>()),  vdT(nt2::One<vdT>()))[0],  nt2::One<vT>()[0]);
    NT2_TEST_EQUAL(nt2::dist(vdT(nt2::One<vdT>()),   vdT(nt2::One<vdT>()))[0],  nt2::Zero<vT>() [0]);
  }
} // end of test for floating_


// Copyright (c) Sergiu Dotenco 2010
//
// 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)

/**
 * @brief Poisson-process implementation.
 * @file poissonprocess.hpp
 */

#ifndef POISSONPROCESS_HPP
#define POISSONPROCESS_HPP

#include <istream>
#include <ostream>

#include <boost/operators.hpp>
#include <boost/random/exponential_distribution.hpp>

/**
 * @brief Homogeneous Poisson process.
 */
template<class RealType = double>
class HomogeneousPoissonProcess
    : boost::equality_comparable
      <
          HomogeneousPoissonProcess<RealType>,
          HomogeneousPoissonProcess<RealType>
      >
{
    typedef boost::exponential_distribution<RealType> exponential_distribution;
    exponential_distribution exp_;

public:
    typedef typename exponential_distribution::result_type result_type;
    typedef result_type param_type;

    explicit HomogeneousPoissonProcess(result_type lambda)
        : exp_(lambda)
    {
    }

    template<class UniformRandomNumberGenerator>
    result_type operator()(UniformRandomNumberGenerator& u, result_type s)
    {
        return s - exp_(u);
    }

    result_type min() const
    {
        return exp_.min();
    }

    result_type max() const
    {
        return exp_.max();
    }

    result_type lambda() const
    {
        return exp_.lambda();
    }

    void reset()
    {
        exp_.reset();
    }

    bool operator==(const HomogeneousPoissonProcess& other) const
    {
        return exp_ == other.exp_;
    }

    template<class E, class T>
    friend std::basic_ostream<E, T>&
        operator<<(std::basic_ostream<E, T>& out,
                   const HomogeneousPoissonProcess& hpp)
    {
        out << hpp.exp_;
        return out;
    }

    template<class E, class T>
    friend std::basic_istream<E, T>&
        operator>>(std::basic_istream<E, T>& out,
        HomogeneousPoissonProcess& hpp)
    {
        in >> hpp.exp_;
        return in;
    }
};

#endif // POISSONPROCESS_HPP


//
// Created by shankara on 9/13/18.
//

#ifndef PROJECT_UTILS_H
#define PROJECT_UTILS_H

#include <zonotope.h>
#include <Eigen/Dense>
#include <cstdlib>
#include <vector>
#include <iostream>
#include <fstream>

class interval {
  public:
    Eigen::VectorXd lower;
    Eigen::VectorXd upper;
    std::vector<int> posDims;

    interval();
    interval(Eigen::VectorXd, Eigen::VectorXd);

    elina_interval_t** get_elina_interval() const;
    void set_bounds(Eigen::VectorXd, Eigen::VectorXd);
    Eigen::VectorXd get_center() const;
};

interval::interval(Eigen::VectorXd l, Eigen::VectorXd u): lower(l), upper(u) {
  for(int i = 0; i < l.size(); i++) {
    if (upper(i) - lower(i) > 0)
      this->posDims.push_back(i);
  }
}

interval::interval(): lower(Eigen::VectorXd(0)), upper(Eigen::VectorXd(0)) {}

void interval::set_bounds(Eigen::VectorXd l, Eigen::VectorXd u) {
  lower = l;
  upper = u;
}

Eigen::VectorXd interval::get_center() const {
  Eigen::VectorXd ce(lower.size());
  for (int i = 0; i < lower.size(); i++) {
    double l = lower(i);
    double u = upper(i);
    ce(i) = (l + u) / 2.0;
  }
  return ce;
}

interval read_property1(std::string filename, int& dims) {
  std::vector<double> lower;
  std::vector<double> upper;
  std::ifstream in(filename.c_str());
  std::string line;
  while (getline(in, line)) {
    std::istringstream iss(line);
    double l, u;
    iss.get();
    iss >> l;
    iss.get();
    iss.get();
    iss >> u;
    lower.push_back(l);
    upper.push_back(u);
  }

  dims = lower.size();
  Eigen::VectorXd l(dims);
  Eigen::VectorXd u(dims);
  for (int i = 0; i < dims; i++) {
    l(i) = lower[i];
    u(i) = upper[i];
  }

  return interval(l, u);
}

#endif //PROJECT_UTILS_H


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the dlib machine learning tools for
    learning to solve the assignment problem.

    Many tasks in computer vision or natural language processing can be thought of
    as assignment problems.  For example, in a computer vision application where
    you are trying to track objects moving around in video, you likely need to solve
    an association problem every time you get a new video frame.  That is, each new
    frame will contain objects (e.g. people, cars, etc.) and you will want to
    determine which of these objects are actually things you have seen in previous
    frames.

    The assignment problem can be optimally solved using the well known Hungarian
    algorithm.  However, this algorithm requires the user to supply some function
    which measures the "goodness" of an individual association.  In many cases the
    best way to measure this goodness isn't obvious and therefore machine learning
    methods are used.

    The remainder of this example program will show you how to learn a goodness
    function which is optimal, in a certain sense, for use with the Hungarian
    algorithm.  To do this, we will make a simple dataset of example associations
    and use them to train a supervised machine learning method.
*/

#include <iostream>
#include <dlib/svm_threaded.h>

using namespace std;
using namespace dlib;

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

/*
    In an association problem, we will talk about the "Left Hand Set" (LHS) and the
    "Right Hand Set" (RHS).  The task will be to learn to map all elements of LHS to
    unique elements of RHS.  If an element of LHS can't be mapped to a unique element of
    RHS for some reason (e.g. LHS is bigger than RHS) then it can also be mapped to the
    special -1 output, indicating no mapping to RHS.

    So the first step is to define the type of elements in each of these sets.  In the
    code below we will use column vectors in both LHS and RHS.  However, in general,
    they can each contain any type you like.  LHS can even contain a different type
    than RHS.
*/

typedef dlib::matrix<double,0,1> column_vector;

// This type represents a pair of LHS and RHS.  That is, sample_type::first
// contains a left hand set and sample_type::second contains a right hand set.
typedef std::pair<std::vector<column_vector>, std::vector<column_vector> > sample_type;

// This type will contain the association information between LHS and RHS.  That is,
// it will determine which elements of LHS map to which elements of RHS.
typedef std::vector<long> label_type;

// In this example, all our LHS and RHS elements will be 3-dimensional vectors.
const unsigned long num_dims = 3;

void make_data (
    std::vector<sample_type>& samples,
    std::vector<label_type>& labels
);
/*!
    ensures
        - This function creates a training dataset of 5 example associations.
        - #samples.size() == 5
        - #labels.size() == 5
        - for all valid i:
            - #samples[i].first == a left hand set
            - #samples[i].second == a right hand set
            - #labels[i] == a set of integers indicating how to map LHS to RHS.  To be
              precise:
                - #samples[i].first.size() == #labels[i].size()
                - for all valid j:
                    -1 <= #labels[i][j] < #samples[i].second.size()
                    (A value of -1 indicates that #samples[i].first[j] isn't associated with anything.
                    All other values indicate the associating element of #samples[i].second)
                - All elements of #labels[i] which are not equal to -1 are unique.  That is,
                  multiple elements of #samples[i].first can't associate to the same element
                  in #samples[i].second.
!*/

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

struct feature_extractor
{
    /*!
        Recall that our task is to learn the "goodness of assignment" function for
        use with the Hungarian algorithm.  The dlib tools assume this function
        can be written as:
            match_score(l,r) == dot(w, PSI(l,r))
        where l is an element of LHS, r is an element of RHS, w is a parameter vector,
        and PSI() is a user supplied feature extractor.

        This feature_extractor is where we implement PSI().  How you implement this
        is highly problem dependent.
    !*/

    // The type of feature vector returned from get_features().  This must be either
    // a dlib::matrix or a sparse vector.
    typedef column_vector feature_vector_type;

    // The types of elements in the LHS and RHS sets
    typedef column_vector lhs_element;
    typedef column_vector rhs_element;

    unsigned long num_features() const
    {
        // Return the dimensionality of feature vectors produced by get_features()
        return num_dims + 1;
    }

    void get_features (
        const lhs_element& left,
        const rhs_element& right,
        feature_vector_type& feats
    ) const
    /*!
        ensures
            - #feats == PSI(left,right)
              (i.e. This function computes a feature vector which, in some sense,
              captures information useful for deciding if matching left to right
              is "good").
    !*/
    {
        // We will have:
        //   - feats(i) == std::pow(left(i) - right(i), 2.0)
        // Except for the last element of feats which will be equal to 1 and
        // therefore function as a bias term.  Again, how you define this feature
        // extractor is highly problem dependent.
        feats = join_cols(squared(left - right), ones_matrix<double>(1,1));
    }

};

// We need to define serialize() and deserialize() for our feature extractor if we want
// to be able to serialize and deserialize our learned models.  In this case the
// implementation is empty since our feature_extractor doesn't have any state.  But you
// might define more complex feature extractors which have state that needs to be saved.
void serialize   (const feature_extractor& , std::ostream& ) {}
void deserialize (feature_extractor&       , std::istream& ) {}

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

int main()
{
    try
    {
        // Get a small bit of training data.
        std::vector<sample_type> samples;
        std::vector<label_type> labels;
        make_data(samples, labels);

        structural_assignment_trainer<feature_extractor> trainer;
        // This is the common SVM C parameter.  Larger values encourage the
        // trainer to attempt to fit the data exactly but might overfit.
        // In general, you determine this parameter by cross-validation.
        trainer.set_c(10);
        // This trainer can use multiple CPU cores to speed up the training.
        // So set this to the number of available CPU cores.
        trainer.set_num_threads(4);

        // Do the training and save the results in assigner.
        assignment_function<feature_extractor> assigner = trainer.train(samples, labels);

        // Test the assigner on our data.  The output will indicate that it makes the
        // correct associations on all samples.
        cout << "Test the learned assignment function: " << endl;
        for (unsigned long i = 0; i < samples.size(); ++i)
        {
            // Predict the assignments for the LHS and RHS in samples[i].
            std::vector<long> predicted_assignments = assigner(samples[i]);
            cout << "true labels:      " << trans(mat(labels[i]));
            cout << "predicted labels: " << trans(mat(predicted_assignments)) << endl;
        }

        // We can also use this tool to compute the percentage of assignments predicted correctly.
        cout << "training accuracy: " << test_assignment_function(assigner, samples, labels) << endl;

        // Since testing on your training data is a really bad idea, we can also do 5-fold cross validation.
        // Happily, this also indicates that all associations were made correctly.
        randomize_samples(samples, labels);
        cout << "cv accuracy:       " << cross_validate_assignment_trainer(trainer, samples, labels, 5) << endl;

        // Finally, the assigner can be serialized to disk just like most dlib objects.
        ofstream fout("assigner.dat", ios::binary);
        serialize(assigner, fout);
        fout.close();

        // recall from disk
        ifstream fin("assigner.dat", ios::binary);
        deserialize(assigner, fin);
    }
    catch (std::exception& e)
    {
        cout << "EXCEPTION THROWN" << endl;
        cout << e.what() << endl;
    }
}

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

void make_data (
    std::vector<sample_type>& samples,
    std::vector<label_type>& labels
)
{
    // Make four different vectors.  We will use them to make example assignments.
    column_vector A(num_dims), B(num_dims), C(num_dims), D(num_dims);
    A = 1,0,0;
    B = 0,1,0;
    C = 0,0,1;
    D = 0,1,1;

    std::vector<column_vector> lhs;
    std::vector<column_vector> rhs;
    label_type mapping;

    // In all the assignments to follow, we will only say an element of the LHS
    // matches an element of the RHS if the two are equal.  So A matches with A,
    // B with B, etc.  But never A with C, for example.
    // ------------------------

    lhs.resize(3);
    lhs[0] = A;
    lhs[1] = B;
    lhs[2] = C;

    rhs.resize(3);
    rhs[0] = B;
    rhs[1] = A;
    rhs[2] = C;

    mapping.resize(3);
    mapping[0] = 1;  // lhs[0] matches rhs[1]
    mapping[1] = 0;  // lhs[1] matches rhs[0]
    mapping[2] = 2;  // lhs[2] matches rhs[2]

    samples.push_back(make_pair(lhs,rhs));
    labels.push_back(mapping);

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

    lhs[0] = C;
    lhs[1] = A;
    lhs[2] = B;

    rhs[0] = A;
    rhs[1] = B;
    rhs[2] = D;

    mapping[0] = -1;  // The -1 indicates that lhs[0] doesn't match anything in rhs.
    mapping[1] = 0;   // lhs[1] matches rhs[0]
    mapping[2] = 1;   // lhs[2] matches rhs[1]

    samples.push_back(make_pair(lhs,rhs));
    labels.push_back(mapping);

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

    lhs[0] = A;
    lhs[1] = B;
    lhs[2] = C;

    rhs.resize(4);
    rhs[0] = C;
    rhs[1] = B;
    rhs[2] = A;
    rhs[3] = D;

    mapping[0] = 2;
    mapping[1] = 1;
    mapping[2] = 0;

    samples.push_back(make_pair(lhs,rhs));
    labels.push_back(mapping);

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

    lhs.resize(2);
    lhs[0] = B;
    lhs[1] = C;

    rhs.resize(3);
    rhs[0] = C;
    rhs[1] = A;
    rhs[2] = D;

    mapping.resize(2);
    mapping[0] = -1;
    mapping[1] = 0;

    samples.push_back(make_pair(lhs,rhs));
    labels.push_back(mapping);

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

    lhs.resize(3);
    lhs[0] = D;
    lhs[1] = B;
    lhs[2] = C;

    // rhs will be empty.  So none of the items in lhs can match anything.
    rhs.resize(0);

    mapping.resize(3);
    mapping[0] = -1;
    mapping[1] = -1;
    mapping[2] = -1;

    samples.push_back(make_pair(lhs,rhs));
    labels.push_back(mapping);

}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <gtest/gtest.h>
#include <photk/Common.h>
#include <test/Helpers.h>
#include <boost/foreach.hpp>

using namespace vw;
using namespace photk;

TEST(Common, SplitSquareEqualArea) {
  for ( int32 level = 3; level < 20; level++ ) {
    int32 level_size = 0x1 << level;
    int32 k_divisions = level_size;
    if ( k_divisions > 61 )
      k_divisions = 61;
    for ( int32 k = 1; k < k_divisions; k++ ) {
      std::vector<BBox2i> regions =
        split_square_to_equal_area(level_size, k);
      ASSERT_EQ( k, regions.size() ) << "Level: " << level << " size: " << level_size << " k: " << k;
      int32 total_area = 0;
      BOOST_FOREACH( BBox2i const& box, regions )
        total_area += box.width()*box.height();
      EXPECT_EQ( level_size*level_size, total_area );
    }
  }

  // Verify the split_square_to_equal_area produce bboxes that overlap
  // and are contained by the input.
  std::vector<BBox2i> regions = split_square_to_equal_area(256,80);
  BBox2i global_box(0,0,256,256);
  uint64 area = 0;
  for ( size_t i = 0; i < 80; i++ ) {
    EXPECT_TRUE( global_box.contains(regions[i]) );
    area += regions[i].width()*regions[i].height();
    for ( size_t j = i+1; j < 80; j++ )
      EXPECT_FALSE( regions[i].intersects(regions[j]) );
  }
  EXPECT_EQ( 256*256, area );

  // A more aggressive test that I think is failing
  regions = split_square_to_equal_area(2048,2048);
  global_box = BBox2i(0,0,2048,2048);
  area = 0;
  for ( size_t i = 0; i < 2048; i++ ) {
    EXPECT_TRUE( global_box.contains(regions[i]) );
    area += regions[i].width()*regions[i].height();
    for ( size_t j = i+1; j < 2048; j++ )
      EXPECT_FALSE( regions[i].intersects(regions[j]) );
  }
  EXPECT_EQ( 2048*2048, area );
}


#include <array>
#include <boost/numeric/odeint.hpp>
#include "rose499/sfcontrol.hpp"
#include "test/catch.hpp"

using namespace Eigen;
using namespace SimulatorTypes;

SCENARIO( "serret frenet controller applied to polynomial path", "[serretfrenet]" )
{
    namespace odeint = boost::numeric::odeint;
    constexpr ValueType Tfinal = 40.0;
    ValueType T = 0.0;
    ValueType dt = 0.001;

    // Initial Condition
    auto ode45 = odeint::make_controlled( 1.0e-12 , 1.0e-12 , odeint::runge_kutta_fehlberg78<DriveSystem::StateType, ValueType>() );

    // Generate a spline
    Matrix<Spline::ValueType, 2, 2> waypoints;
    waypoints(0, 0) = 1; waypoints(1, 0) = 0;
    waypoints(0, 1) = 10; waypoints(1, 1) = 10;

    // Hopefully the splining works haha
    Spline spline(waypoints, 0);

    GIVEN( "constructed serret frenet controller" )
    {
        DriveSystem robot;
        SerretFrenetController sfcontrol(robot, Vector2T(10, 10), 0.5);
        sfcontrol.path(spline);
        CAPTURE( spline.poly() );

        WHEN( "simulated with initial condition on the path" )
        {
            DriveSystem::StateType x{ 1, 0, 0 };
            do
            {
                // If we fail
                if(odeint::fail == ode45.try_step(std::reference_wrapper<DriveController>(sfcontrol), x, T, dt))
                    continue;

                dt = std::min( dt, static_cast<ValueType>(0.05) );

                // Check if we converged to the path tangentially!
                if( sfcontrol.linearizedState().norm() == Approx(0).margin(1e-6) )
                    break;

            } while( T < Tfinal );

            THEN( "we should converge to the path and stay on it for all time" )
            {
                REQUIRE( sfcontrol.linearizedState().norm() == Approx(0).margin(1e-6) );
                while( T < Tfinal )
                {
                    // If we fail
                    if(odeint::fail == ode45.try_step(std::reference_wrapper<DriveController>(sfcontrol), x, T, dt))
                        continue;

                    dt = std::min( dt, static_cast<ValueType>(0.05) );

                    REQUIRE( sfcontrol.linearizedState().norm() == Approx(0).margin(1e-6) );
                }
            }
        }
    }
}


#include <iostream>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <glog/logging.h>
#include <ctime>
#include <nlopt.h>
#include <unistd.h>
#include <sys/time.h>

// 自定义函数
#include "preprocess/preprocess.h"
#include "utils/utils.h"
#include "reconstruction/finger_reconstruct.h"
#include "edge_detection/edge_detection.h"
#include "map_texture/map_texture.h"

// 错误解
//if((sub == 44 && t == 14) || (sub == 60 && t == 14) || (sub == 153 && t == 4)
//|| (sub == 153 && t == 6) || (sub == 153 && t == 7) || (sub == 153 && t == 8)
//|| (sub == 153 && t == 9) || (sub == 153 && t == 11) || (sub == 153 && t == 12)
//|| (sub == 164 && t == 11) || (sub == 204 && t == 8) || (sub == 16 && t == 8)
//|| (sub == 155 && t == 7))
//continue;

void main_reconstruct()
{
//    test_interp();
    bool en_show = true;
    string dataset_path = "/home/xhb/xhb/DataSet/data/session1/src/";
    int sub = 1;
    int t = 10;
    string img_name = gen_img_name(sub, t);
    //cout << img_name << endl;
    clock_t start, finish;
    long t1, t2;

    // 导入一组图片，对应sub-t.bmp
    start = clock();
    t1 = getCurrentTime();
    vector<cv::Mat> triple_imgs = load_triple_imgs(dataset_path, img_name);
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "读入图片:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";
    cv::Mat img_1 = triple_imgs[0];
    cv::Mat img_2 = triple_imgs[1];
    cv::Mat img_3 = triple_imgs[2];

    if(en_show)
    {
//        cv::imshow("img_1", img_1);
//        cv::imshow("img_2", img_2);
//        cv::imshow("img_3", img_3);
    }

    // 高斯滤波
    start = clock();
    t1 = getCurrentTime();
    cv::GaussianBlur(img_1, img_1, cv::Size(5, 5), 2, 2);
    cv::GaussianBlur(img_2, img_2, cv::Size(5, 5), 2, 2);
    cv::GaussianBlur(img_3, img_3, cv::Size(5, 5), 2, 2);
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "高斯滤波:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";
    if(en_show)
    {
//        cv::imshow("img_1_blur", img_1);
//        cv::imshow("img_2_blur", img_2);
//        cv::imshow("img_3_blur", img_3);
    }

    // 设置相机默认参数
    start = clock();
    t1 = getCurrentTime();
    Finger_Reconstruct fc = Finger_Reconstruct();
    fc.set_cam_param();
//    fc.print_cam_param();
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "设置相机参数:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";

    // 图像校正
    start = clock();
    t1 = getCurrentTime();
    img_1 = fc.img_rectify(img_1, fc.H1);
    img_2 = fc.img_rectify(img_2, fc.H2);
    img_3 = fc.img_rectify(img_3, fc.H3);
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "图像校正:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";
    if(en_show)
    {
//        cv::imshow("img_1_rectified", img_1);
//        cv::imshow("img_2_rectified", img_2);
//        cv::imshow("img_3_rectified", img_3);
    }

    // 图像增强（在对手指作边缘检测前，先进行图像增强，加强边缘，但同时也会降低图像静脉纹理的质量，故仅在边缘检测时使用）
    cv::Mat img_edge_1, img_edge_2, img_edge_3;
    cv::Mat kernel = (cv::Mat_<float>(3, 3) << 0, -1, 0, 0, 7, 0, 0, -1, 0);
    cv::filter2D(img_1, img_edge_1, CV_8UC1, kernel);
    cv::filter2D(img_2, img_edge_2, CV_8UC1, kernel);
    cv::filter2D(img_3, img_edge_3, CV_8UC1, kernel);
    if(en_show)
    {
//        cv::imshow("img_edge_1", img_edge_1);
//        cv::imshow("img_edge_2", img_edge_2);
//        cv::imshow("img_edge_3", img_edge_3);
    }

    // 手指边缘提取
    start = clock();
    t1 = getCurrentTime();
    vector<vector<int>> edge_arr = find_all_edges(img_1, img_2, img_3);
//    vector<vector<int>> edge_arr = find_all_edges(img_edge_1, img_edge_2, img_edge_3);
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "手指边缘提取" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";

    // 显示提取到的手指边缘
//    disp_detected_edges(edge_arr, img_1, img_2, img_3);
    cv::waitKey();

    // 灰度归一化
    start = clock();
    t1 = getCurrentTime();
    vector<cv::Mat> img_list = adjust_brightness(img_1, img_2, img_3, edge_arr, 128);
    img_1 = img_list[0];
    img_2 = img_list[1];
    img_3 = img_list[2];
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "灰度归一化:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";
    if(en_show)
    {
//        cv::imshow("img_1_adjust_brightness", img_1);
//        cv::imshow("img_2_adjust_brightness", img_2);
//        cv::imshow("img_3_adjust_brightness", img_3);
    }

    // 手指三维重建
    start = clock();
    t1 = getCurrentTime();
    fc.reconstruct(edge_arr, 400);
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "手指三维重建:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";

    // 重建出799个截面
//    cout << fc.X_arr.size() << endl;
//    cout << fc.z_arr.size() << endl;

//    vector<vector<float>> temp_X_arr = fc.X_arr;

    // 绘制三维手指模型
//    fc.convert_3d_coor(50);
//    coor_plot_3d::plot_3d_model(fc.X_3D, fc.Y_3D, fc.Z_3D);

    // 纹理映射
//    fc.convert_3d_coor(400);
    start = clock();
    t1 = getCurrentTime();
    map_texture::set_params(fc.coor_c, 400);
    vector<MatrixXf> P_list(3);
    P_list[0] = fc.P1;
    P_list[1] = fc.P2;
    P_list[2] = fc.P3;
//    vector<cv::Mat> img_list_src(3);
//    img_list_src[0] = img_1;
//    img_list_src[1] = img_2;
//    img_list_src[2] = img_3;
    map_texture::map_texture(fc.X_arr, fc.z_arr, img_list, edge_arr, P_list);
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "纹理映射:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";

    // 绘制2维纹理图
//    cv::Mat texture = map_texture::texture_img;
//    cout << texture.size() << endl;
//    cout << map_texture::x_3D.size() << ", " << map_texture::x_3D[0].size() << endl;
//    cv::imshow("map texture", texture);
//    cv::waitKey();

    // 绘制3维指静脉模型
//    vector<vector<unsigned char>> texture_temp = map_texture::texture_3D;
//    coor_plot_3d::plot_3d_finger_vein_model(map_texture::x_3D, map_texture::y_3D, map_texture::z_3D, map_texture::texture_3D);

    start = clock();
    t1 = getCurrentTime();
//    map_texture::projection(180, 180);// 2度采样一个点
    map_texture::projection_2(180, 180);// 2度采样一个点
    finish = clock();
    t2 = getCurrentTime();
    LOG(INFO) << "生成纹理图/深度图:" << (float)(finish - start) / CLOCKS_PER_SEC * 1000 << "ms" << "/" << (t2 - t1) << "ms";

    cv::Mat texture_prj = map_texture::texture_prj;
    cv::Mat depth_prj_img = map_texture::depth_prj_img;
    depth_prj_img.convertTo(depth_prj_img, CV_8UC1, 255.0);
    cv::normalize(depth_prj_img, depth_prj_img, 0, 255, cv::NORM_MINMAX);
//    cout << texture_prj.size() << endl;
    cout << "texture_prj: " << texture_prj.size() << endl;
    cout << "depth_prj_img: " << depth_prj_img.size() << endl;

//    cv::resize(texture_prj, texture_prj, cv::Size(360, 360));
//    cv::resize(depth_prj_img, depth_prj_img, cv::Size(360, 360));
    cv::imshow("texture", texture_prj);
    cv::imshow("depth", depth_prj_img);

    cv::waitKey();

    return;
}

void generate_one_map()
{
    string dataset_path = "/home/xhb/xhb/DataSet/data/session1/src/";

    int sub = 54;
    int t = 13;
    string img_name = gen_img_name(sub, t);

    cout << img_name << endl;

    // 导入一组图片，对应sub-t.bmp
    vector<cv::Mat> triple_imgs = load_triple_imgs(dataset_path, img_name);
    cv::Mat img_1 = triple_imgs[0];
    cv::Mat img_2 = triple_imgs[1];
    cv::Mat img_3 = triple_imgs[2];

    // 高斯滤波
    cv::GaussianBlur(img_1, img_1, cv::Size(5, 5), 2, 2);
    cv::GaussianBlur(img_2, img_2, cv::Size(5, 5), 2, 2);
    cv::GaussianBlur(img_3, img_3, cv::Size(5, 5), 2, 2);

    // 设置相机默认参数
    Finger_Reconstruct fc = Finger_Reconstruct();
    fc.set_cam_param();

    // 图像校正
    img_1 = fc.img_rectify(img_1, fc.H1);
    img_2 = fc.img_rectify(img_2, fc.H2);
    img_3 = fc.img_rectify(img_3, fc.H3);

    // 手指边缘提取
    vector<vector<int>> edge_arr = find_all_edges(img_1, img_2, img_3);

    // 灰度归一化
    vector<cv::Mat> img_list = adjust_brightness(img_1, img_2, img_3, edge_arr, 128);
    img_1 = img_list[0];
    img_2 = img_list[1];
    img_3 = img_list[2];

    // 手指三维重建
    fc.reconstruct(edge_arr, 460);

    // 纹理映射
    map_texture::set_params(fc.coor_c, 520);
    vector<MatrixXf> P_list(3);
    P_list[0] = fc.P1;
    P_list[1] = fc.P2;
    P_list[2] = fc.P3;
    map_texture::map_texture(fc.X_arr, fc.z_arr, img_list, edge_arr, P_list);

    // 生成纹理图/深度图
    map_texture::projection(180, 180);// 2度采样一个点

    cv::Mat texture_prj = map_texture::texture_prj;
    cv::Mat depth_prj_img = map_texture::depth_prj_img;
    cv::resize(texture_prj, texture_prj, cv::Size(360, 360));
    cv::resize(depth_prj_img, depth_prj_img, cv::Size(360, 360));
    depth_prj_img.convertTo(depth_prj_img, CV_8UC1, 255.0);
    cv::normalize(depth_prj_img, depth_prj_img, 0, 255, cv::NORM_MINMAX);
//    cv::imshow("texture", texture_prj);
//    cv::imshow("depth", depth_prj_img);

    cv::imwrite(strcat("./fv_img/", img_name), texture_prj );
    cv::imwrite(strcat("./fv_dep/", img_name), depth_prj_img );
    cout << "生成纹理图:" << strcat("./fv_img/", img_name) << endl;
    cout << "生成深度图:" << strcat("./fv_dep/", img_name) << endl;

    cv::waitKey();

    return;
}

void generate_maps()
{
    string dataset_path = "/home/xhb/xhb/DataSet/data/session2/src/";
    for(int sub=1; sub<=203; sub++)
    {
        for(int t=1; t<=14; t++)
        {
            // 错误解
//            if((sub == 60 && t == 6) || (sub == 123 && t == 1) || (sub == 203 && t == 5))
//                continue;

//            if((sub == 33 && t == 11) || (sub == 139 && t == 2) || (sub == 187 && t == 2))
//                continue;

//            if((sub == 153 && t == 9))
//                continue;

            string img_name = gen_img_name(sub, t);

            cout << img_name << endl;

            // 导入一组图片，对应sub-t.bmp
            vector<cv::Mat> triple_imgs = load_triple_imgs(dataset_path, img_name);
            cv::Mat img_1 = triple_imgs[0];
            cv::Mat img_2 = triple_imgs[1];
            cv::Mat img_3 = triple_imgs[2];

            // 高斯滤波
            cv::GaussianBlur(img_1, img_1, cv::Size(5, 5), 2, 2);
            cv::GaussianBlur(img_2, img_2, cv::Size(5, 5), 2, 2);
            cv::GaussianBlur(img_3, img_3, cv::Size(5, 5), 2, 2);

            // 设置相机默认参数
            Finger_Reconstruct fc = Finger_Reconstruct();
            fc.set_cam_param();

            // 图像校正
            img_1 = fc.img_rectify(img_1, fc.H1);
            img_2 = fc.img_rectify(img_2, fc.H2);
            img_3 = fc.img_rectify(img_3, fc.H3);

            // 手指边缘提取
            vector<vector<int>> edge_arr = find_all_edges(img_1, img_2, img_3);

            // 灰度归一化
            vector<cv::Mat> img_list = adjust_brightness(img_1, img_2, img_3, edge_arr, 128);
            img_1 = img_list[0];
            img_2 = img_list[1];
            img_3 = img_list[2];

            // 手指三维重建
            fc.reconstruct(edge_arr, 400);
            cout << "重建成功" << endl;

            // 纹理映射
//            map_texture::set_params(fc.coor_c, 520);
            map_texture::set_params(fc.coor_c, 400);
            vector<MatrixXf> P_list(3);
            P_list[0] = fc.P1;
            P_list[1] = fc.P2;
            P_list[2] = fc.P3;
            map_texture::map_texture(fc.X_arr, fc.z_arr, img_list, edge_arr, P_list);
            cout << "纹理映射成功" << endl;

            cv::Mat texture = map_texture::texture_img;
            cv::imwrite(strcat("./map_ori/", img_name), texture );
            cout << "生成原始纹理映射图:" << strcat("./map_ori/", img_name) << endl;

            // 生成纹理图/深度图
//            map_texture::projection (180, 180);// 2度采样一个点
            map_texture::projection_2(180, 180);

            cv::Mat texture_prj = map_texture::texture_prj;
            cv::Mat depth_prj_img = map_texture::depth_prj_img;
            cv::resize(texture_prj, texture_prj, cv::Size(360, 360));
            cv::resize(depth_prj_img, depth_prj_img, cv::Size(360, 360));
            depth_prj_img.convertTo(depth_prj_img, CV_8UC1, 255.0);
            cv::normalize(depth_prj_img, depth_prj_img, 0, 255, cv::NORM_MINMAX);
//            cv::imshow("texture", texture_prj);
//            cv::imshow("depth", depth_prj_img);

            cv::imwrite(strcat("./fv_img/", img_name), texture_prj );
            cv::imwrite(strcat("./fv_dep/", img_name), depth_prj_img );
            cout << "生成纹理图:" << strcat("./fv_img/", img_name) << endl;
            cout << "生成深度图:" << strcat("./fv_dep/", img_name) << endl;
        }
    }
}

int main(int argc,char* argv[]) {
    // std::cout << "Hello, World!" << std::endl;

    // 初始化日志打印配置
    init_log(argv);

    // 初始化glut库
    glutInit(&argc, argv);

//    main_reconstruct();

//    generate_one_map();

    generate_maps();

    return 0;
}


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

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

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing refine_rec capabilities

    Performs a Newton-Raphson step to improve precision of reciprocate estimate.
    This function can be used in conjunction with raw_rec or
    fast_rec to add more precision to the estimate if their default
    precision is not enough.

    @see rec

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

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

#endif


#include <fstream>
#include <sstream>
#include <vector>
#include <string>

#include <Eigen/Geometry>
#include <ConciseArgs>

struct Scan {
  int64_t mTimestamp;
  double mThetaInit;
  double mThetaStep;
  std::vector<double> mRanges;
};

struct Pose {
  int64_t mTimestamp;
  Eigen::Isometry3d mTransform;
  Eigen::Vector3d mTrans;
  Eigen::Quaterniond mQuat;
};

std::vector<Pose> readPoses(const std::string& iFileName) {
  std::ifstream ifs(iFileName);
  std::vector<Pose> poses;
  std::string line;
  while (std::getline(ifs,line)) {
    std::istringstream iss(line);
    Pose pose;
    auto matx = Eigen::Isometry3d::Identity();
    iss >> pose.mTimestamp >>
      matx(0,0) >> matx(0,1) >> matx(0,2) >> matx(0,3) >>
      matx(1,0) >> matx(1,1) >> matx(1,2) >> matx(1,3) >>
      matx(2,0) >> matx(2,1) >> matx(2,2) >> matx(2,3);
    pose.mTransform = matx;
    pose.mTrans = matx.translation();
    pose.mQuat = matx.rotation();
    poses.push_back(pose);
  }
  ifs.close();
  return poses;
}

void writePoints(const std::vector<Eigen::Vector3d>& iPoints,
                 const std::string& iFileName) {
  std::ofstream ofs(iFileName);
  for (auto p : iPoints) {
    ofs << p[0] << " " << p[1] << " " << p[2] << std::endl;
  }
  ofs.close();
}

Eigen::Vector3d applyInterpolated(const Eigen::Vector3d& iPoint,
                                  const Eigen::Isometry3d& iPose1,
                                  const Eigen::Isometry3d& iPose2,
                                  const Eigen::Isometry3d& iScanPose,
                                  const double iAlpha) {
  Eigen::Isometry3d scanToWorld1 = iPose1*iScanPose;
  Eigen::Isometry3d scanToWorld2 = iPose2*iScanPose;
  Eigen::Vector3d pos1 = scanToWorld1.translation();
  Eigen::Vector3d pos2 = scanToWorld2.translation();
  Eigen::Quaterniond q1(scanToWorld1.rotation());
  Eigen::Quaterniond q2(scanToWorld2.rotation());
  Eigen::Vector3d pos = pos1*(1-iAlpha) + pos2*iAlpha;
  Eigen::Quaterniond q = q1.slerp(iAlpha, q2);
  return q*iPoint + pos;
}

int main(const int iArgc, const char** iArgv) {
  std::string rootDir;
  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(rootDir, "r", "root_dir", "input root directory");
  opt.parse();

  std::string fileName;

  // read lidar scans
  fileName = rootDir + "/scans.txt";
  std::ifstream ifs(fileName);
  std::string line;
  std::vector<Scan> scans;
  int totalPoints = 0;
  while (std::getline(ifs,line)) {
    std::istringstream iss(line);
    Scan scan;
    iss >> scan.mTimestamp >> scan.mThetaInit >> scan.mThetaStep;
    double val;
    while (iss >> val) {
      scan.mRanges.push_back(val);
    }
    scans.push_back(scan);
    totalPoints += scan.mRanges.size();
  }
  ifs.close();

  // read vo camera poses
  std::vector<Pose> voPoses = readPoses(rootDir + "/fovis_poses.txt");
  std::cout << "read " << voPoses.size() << " vo camera poses" << std::endl;

  // read original camera poses
  std::vector<Pose> camPoses = readPoses(rootDir + "/cam_poses.txt");
  std::cout << "read " << camPoses.size() << " camera poses" << std::endl;

  // read lidar spindle poses
  std::vector<Pose> spindlePoses = readPoses(rootDir + "/spindle_poses.txt");
  std::cout << "read " << spindlePoses.size() << " spindle poses" << std::endl;

  // interpolate camera for every lidar point
  std::cout << "interpolating " << totalPoints << " points..." << std::flush;
  std::vector<Eigen::Vector3d> pointsBefore, pointsAfter;
  pointsAfter.reserve(totalPoints);
  int poseIdx = 0;
  const double kPi = acos(-1);
  const double kScanFrequency = 1.0/40;
  const double kMaxRange = 20;
  for (int i = 0; i < scans.size(); ++i) {
    auto& scan = scans[i];
    int n = scan.mRanges.size();
    double dt = kScanFrequency*(scan.mThetaStep/(2*kPi));
    int64_t time2 = scans[i].mTimestamp;
    int64_t time1 = time2 - int64_t(dt*(n-1)*1e6 + 0.5);
    for (int j = 0; j < n; ++j) {

      // check whether lidar return is valid
      double r = scan.mRanges[j];
      if (r > kMaxRange) continue;

      // find pose for this time
      int64_t t = time1 + int64_t(dt*j*1e6 + 0.5);
      double alpha = (double)j/(n-1);
      if (t < voPoses[poseIdx].mTimestamp) continue;
      while (t > voPoses[poseIdx+1].mTimestamp) {
        ++poseIdx;
        if (poseIdx >= voPoses.size()) break;
      }

      // interpolate between two camera poses
      double theta = scan.mThetaInit + j*scan.mThetaStep;
      Eigen::Vector3d p(r*cos(theta), r*sin(theta), 0);
      Eigen::Vector3d pt;
      pt = applyInterpolated
        (p, camPoses[poseIdx].mTransform, camPoses[poseIdx+1].mTransform,
         spindlePoses[i].mTransform, alpha);
      pointsBefore.push_back(pt);
      pt = applyInterpolated
        (p, voPoses[poseIdx].mTransform, voPoses[poseIdx+1].mTransform,
         spindlePoses[i].mTransform, alpha);
      pointsAfter.push_back(pt);
    }
  }
  std::cout << "done." << std::endl;

  // dump points to files
  writePoints(pointsBefore, rootDir + "/cloud_before.txt");
  writePoints(pointsAfter, rootDir + "/cloud_after.txt");

  return 1;
}


/*
Copyright 2013 Henrik Mühe and Florian Funke

This file is part of CampersCoreBurner.

CampersCoreBurner is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

CampersCoreBurner is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with CampersCoreBurner.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
COMMENT SUMMARY FOR THIS FILE:

This file contains an implementation of an optimized chaining hashset and hashmap.
Depending on the use case, these hashes perform up to 4x better than std::unordered_map.
*/

#pragma once

#include "hash.hpp"
#include "pool.hpp"
#include <boost/utility/string_ref.hpp>
#include <atomic>
#include <cstdint>
#include <iostream>
#include <numeric>
#include <vector>

namespace campers {

/// A fast, chaining hash set
template<class T,class HashFunc=Crc32Hash>
class HashSet {
    /// An entry inside the chain
    struct Entry {
        /// The key
        T word;
        /// The hash value
        uint32_t hashValue;
        /// Chain to collisions
        Entry* next;
        /// Constructor
        Entry () : hashValue(0),next(nullptr) {}
        /// Constructor
        Entry (const T& word,uint32_t hashValue) : word(word),hashValue(hashValue),next(nullptr) {}
    };

    /// An instance of the hash funtion functor
    HashFunc hashFunc;
    /// Hashmap of pointers to actual entries
    std::vector<Entry*> entries;
    /// A custom pool allocator for fast allocation of hash entries
    Pool<Entry> entryAllocator;
    /// The exponent used in this hashmap, to avoid modulus calculation, the hash grows in powers of two, this is the exponent
    uint64_t exp;
    /// This is the mask such that if you compute number&mask you get the index inside the entries table
    uint64_t mask;
    /// Current size in elements of the hashtable
    uint64_t size_=0;

public:
    /// Insert an element if it doesn't exist. Return pointer to element if new, nullptr otherwise
    T* tryInsert(const T& word);
    /// Find an element inside the hash table, return true or false depending on whether it exists
    bool find(const T& word) const;
    /// Find an element inside the hash table, return true or false depending on whether it exists. Hash is known in advance.
    bool find(const T& word, uint32_t hashValue) const;
    /// Pre allocate for size many elements. Can not be called when the hash already contains elements, see grow.
    void hintSize(uint32_t size);
    /// Grow the hash table to twice its current size
    void grow() { grow(2*capacity()); };
    /// Grow the hash table to a specific size
    void grow(uint32_t newSize);
    /// Remove all elements from the hashtable
    void clear() { size_=0; entries.clear(); entries.resize(1<<exp); }
    /// Return the current size of the hashtable
    size_t size() const { return size_; };
    /// Return the capacity of the hash table
    size_t capacity() const { return entries.size(); };
};

/// Grow the hash table to a specific size
template<class T,class HashFunc>
void HashSet<T,HashFunc>::grow(uint32_t newSize) {
    // Grow
    for (exp=1;(uint32_t)(1<<exp)<newSize;++exp);
    std::vector<Entry*> newEntries(1<<exp);
    mask=(1ull<<exp)-1ull;

    // Copy entries over
    for (Entry* entryChain : entries) {
        for  (Entry *e=entryChain, *next; e!=nullptr; e=next) {
            Entry*& entry = newEntries[e->hashValue&mask];
            if (entry==nullptr) { // immediate hit: use slot for e
                entry=e;
                next=e->next;
                e->next=nullptr;
            } else { // slot already in use: prepend e to chain
                next=e->next;
                e->next=entry;
                entry=e;
            }
        }
    }

    // Switch to new entry vector
    entries=std::move(newEntries);
}

/// Insert an element if it doesn't exist. Return pointer to element if new, nullptr otherwise
template<class T,class HashFunc>
T* HashSet<T,HashFunc>::tryInsert(const T& word) {
    uint32_t hashValue=hashFunc(word);
    Entry*& entry=entries[hashValue&mask];
    // Unroll empty slot branch
    if (entry==nullptr) {
        ++size_;
        entry=entryAllocator.allocate();
        new (entry) Entry(word,hashValue);
        return &entry->word;
    } else {
        // Unroll immediate hit branch
        if (entry->hashValue==hashValue&&entry->word==word) return nullptr;

        // Iterate to the end of the chain
        Entry* entryIter;
        for (entryIter=entry;entryIter->next!=nullptr;entryIter=entryIter->next) if (entryIter->next->hashValue==hashValue&&entryIter->next->word==word) return nullptr;

        // Word is not present, insert and return reference for backpatching
        ++size_;
        entryIter->next=entryAllocator.allocate();
        new (entryIter->next) Entry(word,hashValue);
        return &entryIter->next->word;
    }
}

/// Find an element inside the hash table, return true or false depending on whether it exists.
template<class T,class HashFunc>
bool HashSet<T,HashFunc>::find(const T& word) const {
    uint32_t hashValue=(uint32_t)hashFunc(word);
    return find(word, hashValue);
}

/// Find an element inside the hash table, return true or false depending on whether it exists. Hash is known in advance.
template<class T,class HashFunc>
bool HashSet<T,HashFunc>::find(const T& word, uint32_t hashValue) const {
    const Entry* entry=entries[hashValue&mask];
    // Unroll empty slot branch
    if (entry==nullptr) {
        return false;
    } else {
        // Unroll immediate hit branch
        if (entry->hashValue==hashValue&&entry->word==word) return true;

        // Iterate to the end of the chain
        for (;entry->next!=nullptr;entry=entry->next) if (entry->next->hashValue==hashValue&&entry->next->word==word) return true;

        return false;
    }
}

/// Pre allocate for size many elements. Can not be called when the hash already contains elements, see grow.
template<class T,class HashFunc>
void HashSet<T,HashFunc>::hintSize(uint32_t size) {
    for (exp=1;(uint32_t)(1<<exp)<size;++exp);
    entries.resize(1<<exp);
    mask=(1ull<<exp)-1ull;
}

/// A fast, chaining HashMap
template<class T,class Value,uint64_t PartitionCount=1,class HashFunc=Crc32Hash>
class HashMap {
    /// An entry inside the hash map
    struct Entry {
        /// The key
        T word;
        /// The value for this key
        Value value;
        /// The hash value
        uint32_t hashValue;
        /// Pointer to chain of entries for collisions
        Entry* next;

        /// Constructor
        Entry () : hashValue(0),next(nullptr) {}
        /// Constructor
        Entry (const T& word,uint32_t hashValue) : word(word),hashValue(hashValue),next(nullptr) {}
    };

    /// An instance of the hash funtion functor
    HashFunc hashFunc;
    /// The hashmap of pointers to actual entries
    std::vector<Entry*> entries;
    /// The pool allocator. Since this hashmap can be partitioned for concurrent access (but never is), there is one pool allocator per partition
    std::array<Pool<Entry>,PartitionCount> entryAllocator;
    /// The exponent, see HashSet and summary for details
    uint64_t exp;
    /// The mask, see HashSet and summary for details
    uint64_t mask;
    /// The number of elements inside the hash, again partitioned into PartitionCount partitions for potential concurrency (unused)
    std::array<uint64_t,PartitionCount> size_;
    /// The size this hash map was allocated with
    uint64_t allocSize;
    /// The mask used to determine the partition for an element (unused)
    uint64_t partitionMask;

public:
    /// Constructor
    HashMap() { size_.fill(0); } // XXX remove and fix errors
    /// Constructor
    HashMap(uint64_t size) : HashMap() { hintSize(size); }
    /// Inserts an element into the hashtable. Returns the value for that key, regardless if an insertion happened or not. Hash value supplied externally
    Value* tryInsert(const T& word,uint32_t hashValue);
    /// Inserts an element into the hashtable. Returns the value for that key, regardless if an insertion happened or not.
    Value* tryInsert(const T& word) { return tryInsert(word,hashFunc(word)); }
    /// Finds an element inside the hashmap, returns the value if found or nullptr otherwise. Hash value supplied externally
    Value* find(const T& word,uint32_t hashValue);
    /// Finds an element inside the hashmap, returns the value if found or nullptr otherwise
    Value* find(const T& word) { return find(word,hashFunc(word)); }
    /// Alternative find: uses a type that can differ from T, e.g. T=string, T2=string_ref
    template <class T2> Value* altFind(const T2& word);
    /// Pre allocates the hash to be able to contain size elements efficiently. Also, see summary and HashSet
    void hintSize(uint32_t size);
    /// Returns the current number of elements inside the hash
    size_t size() const { return std::accumulate(size_.begin(),size_.end(),0); }
    /// Grows the hash to twice its capacity
    void grow() { grow(2*capacity()); };
    /// Grows the hash to be able to efficiently contain 2* capacity elements
    void grow(uint32_t newSize);
    /// Retrieves the mask used to determine an elements partition (unused)
    uint64_t getPartitionMask() const { return partitionMask; }
    /// Returns the current capacity of the hash
    size_t capacity() const { return entries.size(); };
};

/// Grows the hash to be able to efficiently contain 2* capacity elements
template<class T,class Value,uint64_t PartitionCount,class HashFunc>
void HashMap<T,Value,PartitionCount,HashFunc>::grow(uint32_t newSize) {
    allocSize=newSize;
    // Grow
    for (exp=1;(uint32_t)(1<<exp)<newSize;++exp);
    std::vector<Entry*> newEntries(1<<exp);
    mask=(1ull<<exp)-1ull;

    // Copy entries over
    for (Entry* entryChain : entries) {
        for  (Entry *e=entryChain, *next; e!=nullptr; e=next) {
            Entry*& entry = newEntries[e->hashValue&mask];
            if (entry==nullptr) { // immediate hit: use slot for e
                entry=e;
                next=e->next;
                e->next=nullptr;
            } else { // slot already in use: prepend e to chain
                next=e->next;
                e->next=entry;
                entry=e;
            }
        }
    }

    // Switch to new entry vector
    entries=std::move(newEntries);
}

/// Inserts an element into the hashtable. Returns the value for that key, regardless if an insertion happened or not.
template<class T,class Value,uint64_t PartitionCount,class HashFunc>
Value* HashMap<T,Value,PartitionCount,HashFunc>::tryInsert(const T& word,uint32_t hashValue) {
    uint64_t index=hashValue&partitionMask;
    hashValue>>=8;
    Entry*& entry=entries[hashValue&mask];
    // Unroll empty slot branch
    if (entry==nullptr) {
        ++size_[index];
        entry=entryAllocator[index].allocate();
        new (entry) Entry(word,hashValue);
        return &entry->value;
    } else {
        // Unroll immediate hit branch
        if (entry->hashValue==hashValue&&entry->word==word) return &entry->value;

        // Iterate to the end of the chain
        Entry* entryIter;
        for (entryIter=entry;entryIter->next!=nullptr;entryIter=entryIter->next) if (entryIter->next->hashValue==hashValue&&entryIter->next->word==word) return &entryIter->next->value;

        // Word is not present, insert and return reference for backpatching
        ++size_[index];
        entryIter->next=entryAllocator[index].allocate();
        new (entryIter->next) Entry(word,hashValue);
        return &entryIter->next->value;
    }
}

/// Finds an element inside the hashmap, returns the value if found or nullptr otherwise. Hash value supplied externally
template<class T,class Value,uint64_t PartitionCount,class HashFunc>
Value* HashMap<T,Value,PartitionCount,HashFunc>::find(const T& word,uint32_t hashValue) {
    hashValue>>=8;
    Entry* entry=entries[hashValue&mask];
    // Unroll empty slot branch
    if (entry==nullptr) {
        return nullptr;
    } else {
        // Unroll immediate hit branch
        if (entry->hashValue==hashValue&&entry->word==word) return &entry->value;

        // Iterate to the end of the chain
        for (;entry->next!=nullptr;entry=entry->next) if (entry->next->hashValue==hashValue&&entry->next->word==word) return &entry->next->value;

        return nullptr;
    }
}

/// Alternative find: uses a type that can differ from T, e.g. T=string, T2=string_ref
template<class T,class Value,uint64_t PartitionCount,class HashFunc>
template<class T2>
Value* HashMap<T,Value,PartitionCount,HashFunc>::altFind(const T2& word) {
    uint32_t hashValue=hashFunc(word);
    hashValue>>=8;
    Entry* entry=entries[hashValue&mask];
    // Unroll empty slot branch
    if (entry==nullptr) {
        return nullptr;
    } else {
        // Unroll immediate hit branch
        if (entry->hashValue==hashValue&&entry->word==word) return &entry->value;

        // Iterate to the end of the chain
        for (;entry->next!=nullptr;entry=entry->next) if (entry->next->hashValue==hashValue&&entry->next->word==word) return &entry->next->value;

        return nullptr;
    }
}

/// Pre allocates the hash to be able to contain size elements efficiently. Also, see summary and HashSet
template<class T,class Value,uint64_t PartitionCount,class HashFunc>
void HashMap<T,Value,PartitionCount,HashFunc>::hintSize(uint32_t size) {
    this->allocSize=size;
    for (exp=1;(uint32_t)(1<<exp)<size;++exp);
    entries.resize(1<<exp);
    mask=(1ull<<exp)-1ull;

    if (PartitionCount<=1) { partitionMask=0; return; }
    uint64_t partitionExp;
    for (partitionExp=1;(uint32_t)(1<<partitionExp)<PartitionCount;++partitionExp);
    partitionMask=(1ull<<partitionExp)-1ull;
}

}


//
//=======================================================================
// Copyright 2002 Marc Wintermantel (wintermantel@even-ag.ch)
// ETH Zurich, Center of Structure Technologies (www.imes.ethz.ch/st)
//
// 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)
//=======================================================================
//

#ifndef BOOST_GRAPH_SLOAN_HPP
#define BOOST_GRAPH_SLOAN_HPP

#define WEIGHT1 1               //default weight for the distance in the Sloan algorithm
#define WEIGHT2 2               //default weight for the degree in the Sloan algorithm

#include <boost/config.hpp>
#include <vector>
#include <queue>
#include <algorithm>
#include <limits>
#include <boost/pending/queue.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/properties.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>

////////////////////////////////////////////////////////////
//
//Sloan-Algorithm for graph reordering
//(optimzes profile and wavefront, not primiraly bandwidth
//
////////////////////////////////////////////////////////////

namespace boost {

  /////////////////////////////////////////////////////////////////////////
  // Function that returns the maximum depth of
  // a rooted level strucutre (RLS)
  //
  /////////////////////////////////////////////////////////////////////////
  template<class Distance>
  unsigned RLS_depth(Distance& d)
  {
    unsigned h_s = 0;
    typename Distance::iterator iter;

    for (iter = d.begin(); iter != d.end(); ++iter)
      {
        if(*iter > h_s)
          {
            h_s = *iter;
          }
      }

    return h_s;
  }

  /////////////////////////////////////////////////////////////////////////
  // Function that returns the width of the largest level of
  // a rooted level strucutre (RLS)
  //
  /////////////////////////////////////////////////////////////////////////
  template<class Distance, class my_int>
  unsigned RLS_max_width(Distance& d, my_int depth)
  {

      //Searching for the maximum width of a level
      std::vector<unsigned> dummy_width(depth+1, 0);
      std::vector<unsigned>::iterator my_it;
      typename Distance::iterator iter;
      unsigned w_max = 0;

      for (iter = d.begin(); iter != d.end(); ++iter)
      {
          dummy_width[*iter]++;
      }

      for(my_it = dummy_width.begin(); my_it != dummy_width.end(); ++my_it)
      {
          if(*my_it > w_max) w_max = *my_it;
      }

      return w_max;

  }

  /////////////////////////////////////////////////////////////////////////
  // Function for finding a good starting node for Sloan algorithm
  //
  // This is to find a good starting node. "good" is in the sense
  // of the ordering generated.
  /////////////////////////////////////////////////////////////////////////
  template <class Graph, class ColorMap, class DegreeMap>
  typename graph_traits<Graph>::vertex_descriptor
  sloan_start_end_vertices(Graph& G,
                           typename graph_traits<Graph>::vertex_descriptor &s,
                           ColorMap color,
                           DegreeMap degree)
  {
    typedef typename property_traits<DegreeMap>::value_type Degree;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename std::vector< typename graph_traits<Graph>::vertices_size_type>::iterator vec_iter;
    typedef typename graph_traits<Graph>::vertices_size_type size_type;

    typedef typename property_map<Graph, vertex_index_t>::const_type VertexID;

    s = *(vertices(G).first);
    Vertex e = s;
    Vertex i;
    unsigned my_degree = get(degree, s );
    unsigned dummy, h_i, h_s, w_i, w_e;
    bool new_start = true;
    unsigned maximum_degree = 0;

    //Creating a std-vector for storing the distance from the start vertex in dist
    std::vector<typename graph_traits<Graph>::vertices_size_type> dist(num_vertices(G), 0);

    //Wrap a property_map_iterator around the std::iterator
    boost::iterator_property_map<vec_iter, VertexID, size_type, size_type&> dist_pmap(dist.begin(), get(vertex_index, G));

    //Creating a property_map for the indices of a vertex
    typename property_map<Graph, vertex_index_t>::type index_map = get(vertex_index, G);

    //Creating a priority queue
    typedef indirect_cmp<DegreeMap, std::greater<Degree> > Compare;
    Compare comp(degree);
    std::priority_queue<Vertex, std::vector<Vertex>, Compare> degree_queue(comp);

    //step 1
    //Scan for the vertex with the smallest degree and the maximum degree
    typename graph_traits<Graph>::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui)
    {
      dummy = get(degree, *ui);

      if(dummy < my_degree)
      {
        my_degree = dummy;
        s = *ui;
      }

      if(dummy > maximum_degree)
      {
        maximum_degree = dummy;
      }
    }
    //end 1

    do{
      new_start = false;     //Setting the loop repetition status to false

      //step 2
      //initialize the the disance std-vector with 0
      for(typename std::vector<typename graph_traits<Graph>::vertices_size_type>::iterator iter = dist.begin(); iter != dist.end(); ++iter) *iter = 0;

      //generating the RLS (rooted level structure)
      breadth_first_search
        (G, s, visitor
         (
           make_bfs_visitor(record_distances(dist_pmap, on_tree_edge() ) )
           )
          );

      //end 2

      //step 3
      //calculating the depth of the RLS
      h_s = RLS_depth(dist);

      //step 4
      //pushing one node of each degree in an ascending manner into degree_queue
      std::vector<bool> shrink_trace(maximum_degree, false);
      for (boost::tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui)
      {
        dummy = get(degree, *ui);

        if( (dist[index_map[*ui]] == h_s ) && ( !shrink_trace[ dummy ] ) )
        {
          degree_queue.push(*ui);
          shrink_trace[ dummy ] = true;
        }
      }

      //end 3 & 4

      // step 5
      // Initializing w
      w_e = (std::numeric_limits<unsigned>::max)();
      //end 5

      //step 6
      //Testing for termination
      while( !degree_queue.empty() )
      {
        i = degree_queue.top();       //getting the node with the lowest degree from the degree queue
        degree_queue.pop();           //ereasing the node with the lowest degree from the degree queue

        //generating a RLS
        for(typename std::vector<typename graph_traits<Graph>::vertices_size_type>::iterator iter = dist.begin(); iter != dist.end(); ++iter) *iter = 0;

        breadth_first_search
          (G, i, boost::visitor
           (
             make_bfs_visitor(record_distances(dist_pmap, on_tree_edge() ) )
             )
            );

        //Calculating depth and width of the rooted level
        h_i = RLS_depth(dist);
        w_i = RLS_max_width(dist, h_i);

        //Testing for termination
        if( (h_i > h_s) && (w_i < w_e) )
        {
          h_s = h_i;
          s = i;
          while(!degree_queue.empty()) degree_queue.pop();
          new_start = true;
        }
        else if(w_i < w_e)
        {
          w_e = w_i;
          e = i;
        }
      }
      //end 6

    }while(new_start);

    return e;
  }

  //////////////////////////////////////////////////////////////////////////
  // Sloan algorithm with a given starting Vertex.
  //
  // This algorithm requires user to provide a starting vertex to
  // compute Sloan ordering.
  //////////////////////////////////////////////////////////////////////////
  template <class Graph, class OutputIterator,
            class ColorMap, class DegreeMap,
            class PriorityMap, class Weight>
  OutputIterator
  sloan_ordering(Graph& g,
                 typename graph_traits<Graph>::vertex_descriptor s,
                 typename graph_traits<Graph>::vertex_descriptor e,
                 OutputIterator permutation,
                 ColorMap color,
                 DegreeMap degree,
                 PriorityMap priority,
                 Weight W1,
                 Weight W2)
  {
    //typedef typename property_traits<DegreeMap>::value_type Degree;
    typedef typename property_traits<PriorityMap>::value_type Degree;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename std::vector<typename graph_traits<Graph>::vertices_size_type>::iterator vec_iter;
    typedef typename graph_traits<Graph>::vertices_size_type size_type;

    typedef typename property_map<Graph, vertex_index_t>::const_type VertexID;

    //Creating a std-vector for storing the distance from the end vertex in it
    typename std::vector<typename graph_traits<Graph>::vertices_size_type> dist(num_vertices(g), 0);

    //Wrap a property_map_iterator around the std::iterator
    boost::iterator_property_map<vec_iter, VertexID, size_type, size_type&> dist_pmap(dist.begin(), get(vertex_index, g));

    breadth_first_search
      (g, e, visitor
       (
           make_bfs_visitor(record_distances(dist_pmap, on_tree_edge() ) )
        )
       );

    //Creating a property_map for the indices of a vertex
    typename property_map<Graph, vertex_index_t>::type index_map = get(vertex_index, g);

    //Sets the color and priority to their initial status
    unsigned cdeg;
    typename graph_traits<Graph>::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
    {
        put(color, *ui, Color::white());
        cdeg=get(degree, *ui)+1;
        put(priority, *ui, W1*dist[index_map[*ui]]-W2*cdeg );
    }

    //Priority list
    typedef indirect_cmp<PriorityMap, std::greater<Degree> > Compare;
    Compare comp(priority);
    std::list<Vertex> priority_list;

    //Some more declarations
    typename graph_traits<Graph>::out_edge_iterator ei, ei_end, ei2, ei2_end;
    Vertex u, v, w;

    put(color, s, Color::green());      //Sets the color of the starting vertex to gray
    priority_list.push_front(s);                 //Puts s into the priority_list

    while ( !priority_list.empty() )
    {
      priority_list.sort(comp);         //Orders the elements in the priority list in an ascending manner

      u = priority_list.front();           //Accesses the last element in the priority list
      priority_list.pop_front();               //Removes the last element in the priority list

      if(get(color, u) == Color::green() )
      {
        //for-loop over all out-edges of vertex u
        for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei)
        {
          v = target(*ei, g);

          put( priority, v, get(priority, v) + W2 ); //updates the priority

          if (get(color, v) == Color::white() )      //test if the vertex is inactive
          {
            put(color, v, Color::green() );        //giving the vertex a preactive status
            priority_list.push_front(v);                     //writing the vertex in the priority_queue
          }
        }
      }

      //Here starts step 8
      *permutation++ = u;                      //Puts u to the first position in the permutation-vector
      put(color, u, Color::black() );          //Gives u an inactive status

      //for loop over all the adjacent vertices of u
      for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei) {

        v = target(*ei, g);

        if (get(color, v) == Color::green() ) {      //tests if the vertex is inactive

          put(color, v, Color::red() );        //giving the vertex an active status
          put(priority, v, get(priority, v)+W2);  //updates the priority

          //for loop over alll adjacent vertices of v
          for (boost::tie(ei2, ei2_end) = out_edges(v, g); ei2 != ei2_end; ++ei2) {
            w = target(*ei2, g);

            if(get(color, w) != Color::black() ) {     //tests if vertex is postactive

              put(priority, w, get(priority, w)+W2);  //updates the priority

              if(get(color, w) == Color::white() ){

                put(color, w, Color::green() );   // gives the vertex a preactive status
                priority_list.push_front(w);           // puts the vertex into the priority queue

              } //end if

            } //end if

          } //end for

        } //end if

      } //end for

    } //end while

    return permutation;
  }

  /////////////////////////////////////////////////////////////////////////////////////////
  // Same algorithm as before, but without the weights given (taking default weights
  template <class Graph, class OutputIterator,
            class ColorMap, class DegreeMap,
            class PriorityMap>
  OutputIterator
  sloan_ordering(Graph& g,
                 typename graph_traits<Graph>::vertex_descriptor s,
                 typename graph_traits<Graph>::vertex_descriptor e,
                 OutputIterator permutation,
                 ColorMap color,
                 DegreeMap degree,
                 PriorityMap priority)
  {
    return sloan_ordering(g, s, e, permutation, color, degree, priority, WEIGHT1, WEIGHT2);
  }

  //////////////////////////////////////////////////////////////////////////
  // Sloan algorithm without a given starting Vertex.
  //
  // This algorithm finds a good starting vertex itself to
  // compute Sloan-ordering.
  //////////////////////////////////////////////////////////////////////////

  template < class Graph, class OutputIterator,
             class Color, class Degree,
             class Priority, class Weight>
  inline OutputIterator
  sloan_ordering(Graph& G,
                 OutputIterator permutation,
                 Color color,
                 Degree degree,
                 Priority priority,
                 Weight W1,
                 Weight W2 )
  {
    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;

    Vertex s, e;
    e = sloan_start_end_vertices(G, s, color, degree);

    return sloan_ordering(G, s, e, permutation, color, degree, priority, W1, W2);
  }

  /////////////////////////////////////////////////////////////////////////////////////////
  // Same as before, but without given weights (default weights are taken instead)
  template < class Graph, class OutputIterator,
             class Color, class Degree,
             class Priority >
  inline OutputIterator
  sloan_ordering(Graph& G,
                 OutputIterator permutation,
                 Color color,
                 Degree degree,
                 Priority priority)
  {
    return sloan_ordering(G, permutation, color, degree, priority, WEIGHT1, WEIGHT2);
  }

} // namespace boost

#endif // BOOST_GRAPH_SLOAN_HPP