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

#define GRAPHBLAS_LOGGING_LEVEL 0

#include <graphblas/graphblas.hpp>

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE mxv_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

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

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

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

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

    std::vector<double> ans3_dense = {13, 0, 7};
    std::vector<double> ans4_dense = { 3, 0, 1};

    static std::vector<double> v4_ones = {1, 1, 1, 1};
    static std::vector<double> v3_ones = {1, 1, 1};
}

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

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);

    grb::Vector<double, grb::SparseTag> w4(4);
    grb::Vector<double, grb::SparseTag> w3(3);

    // incompatible input dimensions
    BOOST_CHECK_THROW(
        (grb::mxv(w3,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(),
                  mB,
                  u3)),
        grb::DimensionException);

    // incompatible output matrix dimensions
    BOOST_CHECK_THROW(
        (grb::mxv(w4,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(),
                  mA,
                  u3)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);
    grb::Vector<double> u3(u3_dense, 0.);
    grb::Vector<double> u4(u4_dense, 0.);
    grb::Vector<double> result(3);
    grb::Vector<double> ansA(ans3_dense, 0.);
    grb::Vector<double> ansB(ans4_dense, 0.);

    grb::mxv(result,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             mA,
             u3);
    BOOST_CHECK_EQUAL(result, ansA);

    grb::mxv(result,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             mB,
             u4);
    BOOST_CHECK_EQUAL(result, ansB);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_stored_zero_result)
{
    // Build some matrices.
    std::vector<std::vector<int> > A = {{1, 1, 0, 0},
                                        {1, 2, 2, 0},
                                        {0, 2, 3, 3},
                                        {0, 0, 3, 4}};
    std::vector<int> u4_dense =  { 1,-1, 0, 0};
    grb::Matrix<int, grb::DirectedMatrixTag> mA(A, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> result(4);

    // use a different sentinel value so that stored zeros are preserved.
    int const NIL(666);
    std::vector<int> ans = {  0,  -1, -2, NIL};
    grb::Vector<int> answer(ans, NIL);

    grb::mxv(result,
             grb::NoMask(),
             grb::NoAccumulate(), // Second<int>(),
             grb::ArithmeticSemiring<int>(),
             mA,
             u);
    BOOST_CHECK_EQUAL(result, answer);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m3x4_dense, 0.);
    grb::Vector<double> u3(u3_dense, 0.);
    grb::Vector<double> u4(u4_dense, 0.);
    grb::Vector<double> result3(3);
    grb::Vector<double> result4(4);

    std::vector<double> ansAT_dense = {12, 1, 9};
    std::vector<double> ansBT_dense = {11, 7, 1, 2};

    grb::Vector<double> ansA(ansAT_dense, 0.);
    grb::Vector<double> ansB(ansBT_dense, 0.);

    grb::mxv(result3,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             transpose(mA),
             u3);
    BOOST_CHECK_EQUAL(result3, ansA);

    BOOST_CHECK_THROW(
        (grb::mxv(result4,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(),
                  transpose(mB),
                  u4)),
        grb::DimensionException);

    grb::mxv(result4,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             transpose(mB),
             u3);
    BOOST_CHECK_EQUAL(result4, ansB);
}

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

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(3);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::mxv(w3,
                  m4,
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(), mA, u3,
                  grb::REPLACE)),
        grb::DimensionException);
}

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.);
    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 m3,
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 m3,
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 1);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_masked_replace_reg_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask

    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> mask_dense =  { 1, 1, 1, 0};
    std::vector<int> ones_dense =  { 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(mask_dense, 0);
    grb::Vector<int> result(ones_dense, 0);

    BOOST_CHECK_EQUAL(mask.nvals(), 3);  // [ 1 1 1 - ]
    mask.setElement(3, 0);
    BOOST_CHECK_EQUAL(mask.nvals(), 4);  // [ 1 1 1 0 ]

    int const NIL(666);
    std::vector<int> ans = { 0, 1, 1,  NIL};
    grb::Vector<int> answer(ans, NIL);

    grb::mxv(result,
             mask,
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), A, u,
             grb::REPLACE);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_masked_replace_a_transpose)
{
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0, 1},
                                              {1, 2, 2, 0, 1},
                                              {0, 2, 3, 3, 1},
                                              {0, 0, 3, 4, 1}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> mask_dense =  { 1, 0, 1, 0, 1};
    std::vector<int> ones_dense =  { 1, 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(mask_dense, 0);
    grb::Vector<int> result(ones_dense, 0);

    int const NIL(666);
    std::vector<int> ans = { 0, NIL, 1,  NIL, 1};
    grb::Vector<int> answer(ans, NIL);

    grb::mxv(result,
             mask,
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             transpose(A),
             u,
             grb::REPLACE);

    BOOST_CHECK_EQUAL(result, answer);
}

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

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(v3_ones, 0.);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::mxv(w3,
                  m4,
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(),
                  mA,
                  u3)),
        grb::DimensionException);
}

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 m3,
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3);

        BOOST_CHECK_EQUAL(result.nvals(), 3);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 m3,
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense); // [1 1 0]
    BOOST_CHECK_EQUAL(m3.nvals(), 3);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 m3,
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3);

        BOOST_CHECK_EQUAL(result.nvals(), 3);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 m3,
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {12, 1, 1};
    grb::Vector<double> answer(ans, 0.);

    grb::mxv(result,
             m3,
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), transpose(mA), u3);

    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

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

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(3);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::mxv(w3,
                  grb::complement(m4),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(),
                  mA,
                  u3,
                  grb::REPLACE)),
        grb::DimensionException);
}
//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_scmp_masked_replace_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);

    std::vector<double> scmp_mask_dense = {0., 0., 1.};
    grb::Vector<double, grb::SparseTag> m3(scmp_mask_dense, 0.);
    BOOST_CHECK_EQUAL(m3.nvals(), 1);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 grb::complement(m3),
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::mxv(result,
                 grb::complement(m3),
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(),
                 mA,
                 u3,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 1);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_scmp_masked_replace_reg_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> scmp_mask_dense =  { 0, 0, 0, 1 };
    std::vector<int> ones_dense =  { 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(scmp_mask_dense); //, 0);
    grb::Vector<int> result(ones_dense, 0);

    BOOST_CHECK_EQUAL(mask.nvals(), 4);  // [ 0 0 0 1 ]

    int const NIL(666);
    std::vector<int> ans = { 0, 1, 1,  NIL};
    grb::Vector<int> answer(ans, NIL);

    grb::mxv(result,
             grb::complement(mask),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             A,
             u,
             grb::REPLACE);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_scmp_masked_replace_a_transpose)
{
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0, 1},
                                              {1, 2, 2, 0, 1},
                                              {0, 2, 3, 3, 1},
                                              {0, 0, 3, 4, 1}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> mask_dense =  { 0, 1, 0, 1, 0};
    std::vector<int> ones_dense =  { 1, 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(mask_dense, 0);
    grb::Vector<int> result(ones_dense, 0);

    int const NIL(666);
    std::vector<int> ans = { 0, NIL, 1,  NIL, 1};
    grb::Vector<int> answer(ans, NIL);

    grb::mxv(result,
             grb::complement(mask),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             transpose(A),
             u,
             grb::REPLACE);

    BOOST_CHECK_EQUAL(result, answer);
}

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

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(v3_ones, 0.);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::mxv(w3,
                  grb::complement(m4),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(),
                  mA,
                  u3)),
        grb::DimensionException);
}
//****************************************************************************
BOOST_AUTO_TEST_CASE(test_mxv_scmp_masked_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {1, 1, 7}; // if accum is NULL then {1, 0, 7};
    grb::Vector<double> answer(ans, 0.);

    grb::mxv(result,
             grb::complement(m3),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             mA,
             u3);

    BOOST_CHECK_EQUAL(result.nvals(), 3); //2);
    BOOST_CHECK_EQUAL(result, answer);
}

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense); // [1 1 0]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 3);

    std::vector<double> ans = {1, 1, 7}; // if accum is NULL then {1, 0, 7};
    grb::Vector<double> answer(ans, 0.);

    grb::mxv(result,
             grb::complement(m3),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             mA,
             u3);

    BOOST_CHECK_EQUAL(result.nvals(), 3); //2);
    BOOST_CHECK_EQUAL(result, answer);
}

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

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {1, 1, 9};
    grb::Vector<double> answer(ans, 0.);

    grb::mxv(result,
             grb::complement(m3),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             transpose(mA),
             u3);

    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
// Matrix-vector multiply with mask and accumulator but no replace
//****************************************************************************
BOOST_AUTO_TEST_CASE(mxv_reg)
{
    std::vector<std::vector<double>> mat = {{8, 1, 6},
                                            {3, 0, 7},
                                            {0, 0, 2}};

    std::vector<uint32_t> mask = {1, 1, 1};
    grb::Vector<uint32_t, grb::SparseTag> m(mask, 0);

    grb::Matrix<double, grb::DirectedMatrixTag> A(mat, 0.);

    std::vector<double> vec1 = {-1, 2, 1};
    std::vector<double> vec2 = {0, 1, 0};
    std::vector<double> vec3 = {2, 1, 0};
    std::vector<double> vec4 = {0, 1, 1};

    grb::Vector<double, grb::SparseTag> v1(vec1, 0.);
    grb::Vector<double, grb::SparseTag> v2(vec2, 0.);
    grb::Vector<double, grb::SparseTag> v3(vec3, 0.);
    grb::Vector<double, grb::SparseTag> v4(vec4, 0.);

    std::vector<double> a1_dense = {0, 4, 2};
    std::vector<double> a2_dense = {1, 0, 0};
    std::vector<double> a3_dense = {17, 6, 0};
    std::vector<double> a4_dense = {7, 7, 2};

    grb::Vector<double, grb::SparseTag> ans1(a1_dense);
    grb::Vector<double, grb::SparseTag> ans2(a2_dense, 0.);
    grb::Vector<double, grb::SparseTag> ans3(a3_dense, 0.);
    grb::Vector<double, grb::SparseTag> ans4(a4_dense, 0.);

    grb::Vector<double, grb::SparseTag> result(3);

    grb::mxv(result,
             m,                                    // mask
             grb::Second<double>(),                // accum
             grb::ArithmeticSemiring<double>(),    // semiring
             A,
             v1);
    BOOST_CHECK_EQUAL(result, ans1);

    result.clear();
    grb::mxv(result,
             m,                                    // mask
             grb::Second<double>(),                // accum
             grb::ArithmeticSemiring<double>(),    // semiring
             A,
             v2);
    BOOST_CHECK_EQUAL(result, ans2);

    result.clear();
    grb::mxv(result,
             m,                                    // mask
             grb::Second<double>(),                // accum
             grb::ArithmeticSemiring<double>(),    // semiring
             A,
             v3);
    BOOST_CHECK_EQUAL(result, ans3);

    result.clear();
    grb::mxv(result,
             m,                                    // mask
             grb::Second<double>(),                // accum
             grb::ArithmeticSemiring<double>(),    // semiring
             A,
             v4);
    BOOST_CHECK_EQUAL(result, ans4);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("gen_randu_1")
  {
  const uword n_rows = 100;
  const uword n_cols = 101;

  mat A(n_rows,n_cols, fill::randu);

  mat B(n_rows,n_cols); B.randu();

  mat C; C.randu(n_rows,n_cols);

  REQUIRE( (accu(A)/A.n_elem) == Approx(0.5).epsilon(0.01) );
  REQUIRE( (accu(B)/A.n_elem) == Approx(0.5).epsilon(0.01) );
  REQUIRE( (accu(C)/A.n_elem) == Approx(0.5).epsilon(0.01) );

  REQUIRE( (mean(vectorise(A))) == Approx(0.5).epsilon(0.01) );
  }

TEST_CASE("gen_randu_2")
  {
  mat A(50,60,fill::zeros);

  A(span(1,48),span(1,58)).randu();

  REQUIRE( accu(A.head_cols(1)) == Approx(0.0) );
  REQUIRE( accu(A.head_rows(1)) == Approx(0.0) );

  REQUIRE( accu(A.tail_cols(1)) == Approx(0.0) );
  REQUIRE( accu(A.tail_rows(1)) == Approx(0.0) );

  REQUIRE( mean(vectorise(A(span(1,48),span(1,58)))) == Approx(double(0.5)).epsilon(0.01) );
  }


#ifndef ROBOT_BODY_FILTER_URDF_EIGEN_HPP
#define ROBOT_BODY_FILTER_URDF_EIGEN_HPP

#include <Eigen/Geometry>
#include <urdf_model/pose.h>

namespace robot_body_filter
{

/**
 * \brief Helper function to convert URDF pose to Eigen transform.
 *
 * \param pose The pose to convert.
 * \return The corresponding Eigen transform.
 */
inline Eigen::Isometry3d urdfPose2EigenTransform(const urdf::Pose &pose) {
  return Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) *
      Eigen::Quaterniond(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
  // who the ### decided that W is put first in Eigen::Quaternion constructor???
}

}

#endif //ROBOT_BODY_FILTER_URDF_EIGEN_HPP


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "massmatrix_intrinsic.h"
#include "edge_lengths.h"
#include "normalize_row_sums.h"
#include "sparse.h"
#include "doublearea.h"
#include "repmat.h"
#include <Eigen/Geometry>
#include <iostream>
#include <cassert>

template <typename Derivedl, typename DerivedF, typename Scalar>
IGL_INLINE void igl::massmatrix_intrinsic(
  const Eigen::MatrixBase<Derivedl> & l,
  const Eigen::MatrixBase<DerivedF> & F,
  const MassMatrixType type,
  Eigen::SparseMatrix<Scalar>& M)
{
  const int n = F.maxCoeff()+1;
  return massmatrix_intrinsic(l,F,type,n,M);
}

template <typename Derivedl, typename DerivedF, typename Scalar>
IGL_INLINE void igl::massmatrix_intrinsic(
  const Eigen::MatrixBase<Derivedl> & l,
  const Eigen::MatrixBase<DerivedF> & F,
  const MassMatrixType type,
  const int n,
  Eigen::SparseMatrix<Scalar>& M)
{
  using namespace Eigen;
  using namespace std;
  MassMatrixType eff_type = type;
  const int m = F.rows();
  const int simplex_size = F.cols();
  // Use voronoi of for triangles by default, otherwise barycentric
  if(type == MASSMATRIX_TYPE_DEFAULT)
  {
    eff_type = (simplex_size == 3?MASSMATRIX_TYPE_VORONOI:MASSMATRIX_TYPE_BARYCENTRIC);
  }
  assert(F.cols() == 3 && "only triangles supported");
  Matrix<Scalar,Dynamic,1> dblA;
  doublearea(l,0.,dblA);
  Matrix<int,Dynamic,1> MI;
  Matrix<int,Dynamic,1> MJ;
  Matrix<Scalar,Dynamic,1> MV;

  switch(eff_type)
  {
    case MASSMATRIX_TYPE_BARYCENTRIC:
      // diagonal entries for each face corner
      MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
      MI.block(0*m,0,m,1) = F.col(0);
      MI.block(1*m,0,m,1) = F.col(1);
      MI.block(2*m,0,m,1) = F.col(2);
      MJ = MI;
      repmat(dblA,3,1,MV);
      MV.array() /= 6.0;
      break;
    case MASSMATRIX_TYPE_VORONOI:
      {
        // diagonal entries for each face corner
        // http://www.alecjacobson.com/weblog/?p=874
        MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
        MI.block(0*m,0,m,1) = F.col(0);
        MI.block(1*m,0,m,1) = F.col(1);
        MI.block(2*m,0,m,1) = F.col(2);
        MJ = MI;

        // Holy shit this needs to be cleaned up and optimized
        Matrix<Scalar,Dynamic,3> cosines(m,3);
        cosines.col(0) =
          (l.col(2).array().pow(2)+l.col(1).array().pow(2)-l.col(0).array().pow(2))/(l.col(1).array()*l.col(2).array()*2.0);
        cosines.col(1) =
          (l.col(0).array().pow(2)+l.col(2).array().pow(2)-l.col(1).array().pow(2))/(l.col(2).array()*l.col(0).array()*2.0);
        cosines.col(2) =
          (l.col(1).array().pow(2)+l.col(0).array().pow(2)-l.col(2).array().pow(2))/(l.col(0).array()*l.col(1).array()*2.0);
        Matrix<Scalar,Dynamic,3> barycentric = cosines.array() * l.array();
        normalize_row_sums(barycentric,barycentric);
        Matrix<Scalar,Dynamic,3> partial = barycentric;
        partial.col(0).array() *= dblA.array() * 0.5;
        partial.col(1).array() *= dblA.array() * 0.5;
        partial.col(2).array() *= dblA.array() * 0.5;
        Matrix<Scalar,Dynamic,3> quads(partial.rows(),partial.cols());
        quads.col(0) = (partial.col(1)+partial.col(2))*0.5;
        quads.col(1) = (partial.col(2)+partial.col(0))*0.5;
        quads.col(2) = (partial.col(0)+partial.col(1))*0.5;

        quads.col(0) = (cosines.col(0).array()<0).select( 0.25*dblA,quads.col(0));
        quads.col(1) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(1));
        quads.col(2) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(2));

        quads.col(0) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(0));
        quads.col(1) = (cosines.col(1).array()<0).select(0.25*dblA,quads.col(1));
        quads.col(2) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(2));

        quads.col(0) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(0));
        quads.col(1) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(1));
        quads.col(2) = (cosines.col(2).array()<0).select( 0.25*dblA,quads.col(2));

        MV.block(0*m,0,m,1) = quads.col(0);
        MV.block(1*m,0,m,1) = quads.col(1);
        MV.block(2*m,0,m,1) = quads.col(2);

        break;
      }
    case MASSMATRIX_TYPE_FULL:
      assert(false && "Implementation incomplete");
      break;
    default:
      assert(false && "Unknown Mass matrix eff_type");
  }
  sparse(MI,MJ,MV,n,n,M);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::massmatrix_intrinsic<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 4, 0, -1, 4>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 4, 0, -1, 4> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix_intrinsic<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix_intrinsic<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix_intrinsic<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
#endif


#ifndef _shogun_helpers_hxx_
#define _shogun_helpers_hxx_

#include <algorithm>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <iostream>
#include <shogun/features/DenseFeatures.h>
#include <shogun/features/DenseSubSamplesFeatures.h>
#include <shogun/labels/MulticlassLabels.h>
#include <shogun/lib/SGMatrix.h>
#include <shogun/lib/SGVector.h>

namespace sg = shogun;
namespace np = boost::python::numpy;
namespace bp = boost::python;

using namespace shogun;

typedef DenseFeatures<double> Features;
typedef DenseSubSamplesFeatures<double> SubFeatures;
typedef std::shared_ptr<DenseSubSamplesFeatures<double>> SubFeaturesPtr;
typedef std::shared_ptr<DenseFeatures<double>> FeaturesPtr;
typedef std::shared_ptr<MulticlassLabels> MulticlassLabelsPtr;

// Get the median of an unordered set of numbers of arbitrary
// type without modifying the underlying dataset.
template <typename T> T median(T *begin, T *end);

template <typename T>
inline int categorize_sample(SGVector<T> &f, int const &idx_first,
                             int const &idx_second, double const &thr) {

    if (std::max(f.get_element(idx_first), f.get_element(idx_second)) <
        thr) {
      return 0;
    } else if ((std::min(f.get_element(idx_first), f.get_element(idx_second)) <
                thr) &&
               (std::max(f.get_element(idx_first), f.get_element(idx_second)) >=
                thr)) {
      return 1;
    } else {
      return 2;
    }
}
class CategorizedFeatures {
private:
  double threshold;

public:
  std::vector<std::vector<int>> indices;
  int n_cats = 3;
  int n_dims = 0;
  FeaturesPtr feats;
  std::vector<FeaturesPtr> feats_by_cat;

  CategorizedFeatures() {}

  double get_threshold() { return threshold; }
  template <typename T>
  int get_cat(SGVector<T> &f, int const &idx_first,
              int const &idx_second) const {
    return categorize_sample(f, idx_first, idx_second, threshold);
  }
  CategorizedFeatures(FeaturesPtr feats_, int const &idx_first,
                      int const &idx_second) {

    // Compute median of features on given idx
    auto mat = feats_->get_feature_matrix();
    // mat.display_matrix();
    auto first = mat.get_row_vector(idx_first);
    auto second = mat.get_row_vector(idx_second);
    auto first_vec =
        std::vector<double>(first.vector, first.vector + first.size());
    auto second_vec =
        std::vector<double>(first.vector, first.vector + first.size());
    first_vec.insert(first_vec.end(), second_vec.begin(), second_vec.end());

    n_dims = mat.get_column(0).size();

    threshold = median<double>(&first_vec.front(),
                               &first_vec.front() + first_vec.size());
    // std::cout << "median: " << threshold << std::endl;

    // store indices for each category
    indices = std::vector<std::vector<int>>(3);
    for (int i = 0; i < feats_->get_num_vectors(); ++i) {
      auto this_feat = feats_->get_feature_vector(i);
      auto this_cat = get_cat(this_feat, idx_first, idx_second);
      indices[this_cat].push_back(i);
    }

    for (int i = 0; i < indices.size(); ++i) {
      // std::cout << "got " << indices[i].size() << " samples in cat. " << i
      //           << std::endl;
      auto mat_this_cat =
          SGMatrix<double>(feats_->get_num_features(), indices[i].size());
      for (int j = 0; j < indices[i].size(); ++j) {
        SGVector<double> vec = mat.get_column(indices[i][j]);
        // vec.display_vector();
        mat_this_cat.set_column(j, vec);
      }
      // mat_this_cat.display_matrix();

      auto feats_this_cat =
          std::make_shared<DenseFeatures<double>>(mat_this_cat);
      feats_by_cat.push_back(feats_this_cat);
    }

    feats = feats_;
  }

  FeaturesPtr get(int const &cat) { return feats_by_cat[cat]; }

  // this doesn't work: https://github.com/shogun-toolbox/shogun/issues/4795
  SubFeaturesPtr get_subsamples(int const &cat) {
    auto vec = SGVector<int>(indices[cat].begin(), indices[cat].end());
    return std::make_shared<SubFeatures>(feats, vec);
  }

  // Generate MulticlassLabels according to category
  MulticlassLabelsPtr filter_labels(MulticlassLabelsPtr labels,
                                    int const &cat) const {

    labels->remove_all_subsets();
    auto idx = SGVector<int>(indices[cat].begin(), indices[cat].end());
    labels->add_subset(idx);
    return labels;
  }

  // For each category, display feature matrix
  void display_feats() {
    for (int i = 0; i < indices.size(); ++i) {
      // std::cout << "category: " << i << ", n_samples:" << indices[i].size()
      //           << std::endl;
      get(i)->get_feature_matrix().display_matrix();
    }
  }
};

typedef std::shared_ptr<CategorizedFeatures> CategorizedFeaturesPtr;

template <typename T> T median(T *begin, T *end) {
  std::vector<T> data(begin, end);
  std::nth_element(data.begin(), data.begin() + data.size() / 2, data.end());
  return data[data.size() / 2];
}

template <typename T> std::vector<T> get_sorted_uniques(T *v, int const &len) {

  std::vector<T> uniques(v, v + len);
  std::sort(uniques.begin(), uniques.end());
  auto last = std::unique(uniques.begin(), uniques.end());
  uniques.erase(last, uniques.end());
  return uniques;
}

template <typename Tx>
MulticlassLabelsPtr np_to_shogun_labels(np::ndarray const &X) {

  auto X_ = X.astype(np::dtype::get_builtin<Tx>());
  // auto X_copy = X_.copy();
  // Tx *data = reinterpret_cast<Tx *>(X_copy.get_data());

  Tx *data = reinterpret_cast<Tx *>(X_.get_data());

  auto labels = std::make_shared<MulticlassLabels>(X.shape(0));

  for (int i = 0; i < labels->get_num_labels(); ++i) {
    if (data[i] <= 0)
      labels->set_int_label(i, 0);
    else
      labels->set_int_label(i, 1);
  }
  return labels;
}

template <typename Tx> FeaturesPtr np_to_shogun_feats(np::ndarray const &X) {
  Tx *data = reinterpret_cast<Tx *>(X.get_data());
  auto mat = SGMatrix<Tx>(X.shape(1), X.shape(0));
  // std::cout << X.shape(0) << std::endl;
  // std::cout << X.shape(1) << std::endl;
  for (int i = 0; i < X.shape(0); ++i) {
    auto vec = SGVector<Tx>(X.shape(1));
    for (int j = 0; j < X.shape(1); ++j) {
      vec.set_element(data[i * X.shape(1) + j], j);
    }
    // vec.display_vector();
    mat.set_column(i, vec);
  }
  // mat.display_matrix();
  auto out = std::make_shared<DenseFeatures<double>>(mat);
  return out;
}

inline SGVector<double> make_balanced_weight_vector(MulticlassLabelsPtr Y,
                                                    SGVector<double> &weights) {

  auto Yvec = Y->get_labels();
  // Yvec.display_vector();
  std::shared_ptr<double[]> label_counts(new double[2]);

  label_counts[0] = 0;
  label_counts[1] = 0;
  for (int i = 0; i < Yvec.vlen; ++i) {
    if (Yvec[i] == 0)
      label_counts[0] += 1;
    if (Yvec[i] == 1)
      label_counts[1] += 1;
  }

  weights.zero();
  for (int i = 0; i < Yvec.vlen; ++i) {
    if (Yvec[i] == 0)
      weights[i] = Yvec.vlen / (2 * label_counts[0]);
    if (Yvec[i] == 1)
      weights[i] = Yvec.vlen / (2 * label_counts[1]);
  }

  return weights;
}

// from https://gist.github.com/marcinwol/b8df949bf8009cf856a3

template <class T>
inline boost::python::list std_vector_to_list(std::vector<T> vector) {
  typename std::vector<T>::iterator iter;
  boost::python::list list;
  for (iter = vector.begin(); iter != vector.end(); ++iter) {
    list.append(*iter);
  }
  return list;
}

template <class T>
inline boost::python::list
std_2d_vector_to_list(std::vector<std::vector<T>> vector) {
  boost::python::list list;
  for (int i = 0; i < vector.size(); ++i) {
    auto l = std_vector_to_list(vector[i]);
    list.append(l);
  }
  return list;
}

template <typename T>
inline std::vector<T> list_to_std_vector(const bp::object &iterable) {
  return std::vector<T>(boost::python::stl_input_iterator<T>(iterable),
                        boost::python::stl_input_iterator<T>());
}

#endif


/**
 * Copyright (c) 2013 Jonan Cruz-Martin
 *
 * Distributed under the terms of the MIT license, see the accompanying
 * file COPYING or http://opensource.org/licenses/MIT.
 *
 * @file
 * @brief Implement calculation of the circumcenter of a cell.
 */

#ifndef ALGORHITHMS_CELL_CIRCUMCENTER_HPP
#define ALGORHITHMS_CELL_CIRCUMCENTER_HPP

#include <boost/python.hpp>
using namespace boost::python;

#include "../points/cartesian.hpp"
#include "../points/cylindrical.hpp"
#include "../points/polar.hpp"
#include "../points/spherical.hpp"

#include "../cells/linear.hpp"
#include "../cells/triangular.hpp"
#include "../cells/quadrilateral.hpp"
#include "../cells/tetrahedral.hpp"

////////////
// Linear //
////////////

/**
 * Compute the circumcenter of a linear cartesian 1D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian1D LinearCartesian1D_Cell_circumcenter(LinearCartesian1D_Cell cell);

/**
 * Compute the circumcenter of a linear cartesian 2D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian2D LinearCartesian2D_Cell_circumcenter(LinearCartesian2D_Cell cell);

/**
 * Compute the circumcenter of a linear cartesian 3D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian3D LinearCartesian3D_Cell_circumcenter(LinearCartesian3D_Cell cell);

/**
 * Compute the circumcenter of a linear cylindrical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCylindrical3D LinearCylindrical3D_Cell_circumcenter(LinearCylindrical3D_Cell cell);

/**
 * Compute the circumcenter of a linear polar cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointPolar2D LinearPolar2D_Cell_circumcenter(LinearPolar2D_Cell cell);

/**
 * Compute the circumcenter of a linear spherical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointSpherical3D LinearSpherical3D_Cell_circumcenter(LinearSpherical3D_Cell cell);

////////////////
// Triangular //
////////////////

/**
 * Compute the circumcenter of a triangular cartesian 2D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian2D TriangularCartesian2D_Cell_circumcenter(TriangularCartesian2D_Cell cell);

/**
 * Compute the circumcenter of a triangular cartesian 3D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian3D TriangularCartesian3D_Cell_circumcenter(TriangularCartesian3D_Cell cell);

/**
 * Compute the circumcenter of a triangular cylindrical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCylindrical3D TriangularCylindrical3D_Cell_circumcenter(TriangularCylindrical3D_Cell cell);

/**
 * Compute the circumcenter of a triangular polar cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointPolar2D TriangularPolar2D_Cell_circumcenter(TriangularPolar2D_Cell cell);

/**
 * Compute the circumcenter of a triangular spherical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointSpherical3D TriangularSpherical3D_Cell_circumcenter(TriangularSpherical3D_Cell cell);

///////////////////
// Quadrilateral //
///////////////////

/**
 * Compute the circumcenter of a quadrilateral cartesian 2D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian2D QuadrilateralCartesian2D_Cell_circumcenter(QuadrilateralCartesian2D_Cell cell);

/**
 * Compute the circumcenter of a quadrilateral cartesian 3D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian3D QuadrilateralCartesian3D_Cell_circumcenter(QuadrilateralCartesian3D_Cell cell);

/**
 * Compute the circumcenter of a quadrilateral cylindrical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCylindrical3D QuadrilateralCylindrical3D_Cell_circumcenter(QuadrilateralCylindrical3D_Cell cell);

/**
 * Compute the circumcenter of a quadrilateral polar cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointPolar2D QuadrilateralPolar2D_Cell_circumcenter(QuadrilateralPolar2D_Cell cell);

/**
 * Compute the circumcenter of a quadrilateral spherical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointSpherical3D QuadrilateralSpherical3D_Cell_circumcenter(QuadrilateralSpherical3D_Cell cell);

/////////////////
// Tetrahedral //
/////////////////

/**
 * Compute the circumcenter of a tetrahedral cartesian 3D cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCartesian3D TetrahedralCartesian3D_Cell_circumcenter(TetrahedralCartesian3D_Cell cell);

/**
 * Compute the circumcenter of a tetrahedral cylindrical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointCylindrical3D TetrahedralCylindrical3D_Cell_circumcenter(TetrahedralCylindrical3D_Cell cell);

/**
 * Compute the circumcenter of a tetrahedral spherical cell.
 *
 * @param cell The cell whose circumcenter will be computed.
 * @return Point that represents the circumcenter of the cell.
 *
 * @since 0.1.0
 */
PointSpherical3D TetrahedralSpherical3D_Cell_circumcenter(TetrahedralSpherical3D_Cell cell);

#endif /* end of include guard: ALGORHITHMS_CELL_CIRCUMCENTER_HPP */


/*
Copyright (c) 2020 Daniel Stahlke

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

// This demonstrates all sorts of data types that can be plotted using send1d().  It is not
// meant as a first tutorial; for that see example-misc.cc or the project wiki.

#include <vector>
#include <complex>
#include <cmath>
#include <array>
#include <valarray>

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/irange.hpp>
#include <boost/bind.hpp>

#ifdef USE_ARMA
#include <armadillo>
#endif

#ifdef USE_EIGEN
#include <Eigen/Dense>
#endif

#ifdef USE_BLITZ
#include <blitz/array.h>
#endif

#include "gnuplot-iostream.h"

#ifndef M_PI
#    define M_PI 3.14159265358979323846
#endif

static constexpr int num_steps = 100;

double get_x(int step, double shift) {
    double theta = 2.0*M_PI*step/(num_steps-1);
    return std::cos(theta) * (1 + 0.3*std::cos(3.0*theta+2.0*M_PI*shift));
}

double get_y(int step, double shift) {
    double theta = 2.0*M_PI*step/(num_steps-1);
    return std::sin(theta) * (1 + 0.3*std::cos(3.0*theta+2.0*M_PI*shift));
}

double get_z(int step, double shift) {
    double theta = 2.0*M_PI*step/(num_steps-1);
    return 0.3*std::sin(3.0*theta+2.0*M_PI*shift);
}

// This doesn't have to be a template.  It's just a template to show that such things are
// possible.
template <typename T>
struct MyTriple {
    MyTriple() : x(0), y(0), z(0) { }
    MyTriple(T _x, T _y, T _z) : x(_x), y(_y), z(_z) { }

    T x, y, z;
};

// Tells gnuplot-iostream how to print objects of class MyTriple.
namespace gnuplotio {
    template<typename T>
    struct BinfmtSender<MyTriple<T>> {
        static void send(std::ostream &stream) {
            BinfmtSender<T>::send(stream);
            BinfmtSender<T>::send(stream);
            BinfmtSender<T>::send(stream);
        }
    };

    template <typename T>
    struct BinarySender<MyTriple<T>> {
        static void send(std::ostream &stream, const MyTriple<T> &v) {
            BinarySender<T>::send(stream, v.x);
            BinarySender<T>::send(stream, v.y);
            BinarySender<T>::send(stream, v.z);
        }
    };

    // We don't use text mode in this demo.  This is just here to show how it would go.
    template<typename T>
    struct TextSender<MyTriple<T>> {
        static void send(std::ostream &stream, const MyTriple<T> &v) {
            TextSender<T>::send(stream, v.x);
            stream << " ";
            TextSender<T>::send(stream, v.y);
            stream << " ";
            TextSender<T>::send(stream, v.z);
        }
    };
} // namespace gnuplotio

int main() {
    Gnuplot gp;
    // for debugging, prints to console
    //Gnuplot gp(stdout);

    // To send data as text rather than binary (slower but more compatible):
    // FIXME function doesn't exist
    //gp.transportBinary(false);
    // To use temporary files rather then sending data through gnuplot's stdin:
    //gp.useTmpFile(true);

    int num_examples = 15;
#ifdef USE_ARMA
    num_examples += 4;
#endif
#ifdef USE_EIGEN
    num_examples += 4;
#endif
#ifdef USE_BLITZ
    num_examples += 3;
#endif

    double shift = 0;

    gp << "set zrange [-1:1]\n";

    auto plots = gp.splotGroup();

    {
        std::vector<std::pair<std::pair<double, double>, double>> pts;
        for(int i=0; i<num_steps; i++) {
            pts.emplace_back(
                    std::pair(get_x(i, shift), get_y(i, shift)),
                    get_z(i, shift));
        }
        plots.add_plot1d(pts, "with lines title 'vector of nested std::pair'");
    }

    shift += 1.0/num_examples;

    {
        // complex is treated as if it were a pair
        std::vector<std::pair<std::complex<double>, double>> pts;
        for(int i=0; i<num_steps; i++) {
            pts.emplace_back(
                    std::complex<double>(get_x(i, shift), get_y(i, shift)),
                    get_z(i, shift));
        }
        plots.add_plot1d(pts, "with lines title 'vector of pair of cplx and double'");
    }

    shift += 1.0/num_examples;

    {
        std::vector<std::tuple<double, double, double>> pts;
        for(int i=0; i<num_steps; i++) {
            pts.emplace_back(
                    get_x(i, shift), get_y(i, shift), get_z(i, shift));
        }
        plots.add_plot1d(pts, "with lines title 'vector of std::tuple'");
    }

    shift += 1.0/num_examples;

    {
        std::vector<double> x_pts, y_pts, z_pts;
        for(int i=0; i<num_steps; i++) {
            x_pts.push_back(get_x(i, shift));
            y_pts.push_back(get_y(i, shift));
            z_pts.push_back(get_z(i, shift));
        }
        plots.add_plot1d(std::tuple(x_pts, y_pts, z_pts),
                "with lines title 'std::tuple of vector'");
    }

    shift += 1.0/num_examples;

    {
        std::vector<std::array<double, 3>> pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            pts[i][0] = get_x(i, shift);
            pts[i][1] = get_y(i, shift);
            pts[i][2] = get_z(i, shift);
        }
        plots.add_plot1d(pts, "with lines title 'vector of std::array'");
    }

    shift += 1.0/num_examples;

    {
        std::vector<std::vector<double>> pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            pts[i].push_back(get_x(i, shift));
            pts[i].push_back(get_y(i, shift));
            pts[i].push_back(get_z(i, shift));
        }
        plots.add_plot1d(pts, "with lines title 'vector of vector'");
    }

    shift += 1.0/num_examples;

    {
        std::vector<std::vector<double>> pts(3);
        for(int i=0; i<num_steps; i++) {
            pts[0].push_back(get_x(i, shift));
            pts[1].push_back(get_y(i, shift));
            pts[2].push_back(get_z(i, shift));
        }
        plots.add_plot1d_colmajor(pts, "with lines title 'vector of vector (colmajor)'");
    }

    shift += 1.0/num_examples;

    {
        std::vector<MyTriple<double>> pts;
        for(int i=0; i<num_steps; i++) {
            pts.emplace_back(get_x(i, shift), get_y(i, shift), get_z(i, shift));
        }
        plots.add_plot1d(pts, "with lines title 'vector of MyTriple'");
    }

    shift += 1.0/num_examples;

    {
        // Note: C style arrays seem to work, but are a bit fragile since they easily decay to
        // pointers, causing them to forget their lengths.  It is highly recommended that you
        // use std::array instead.  These have the same size and efficiency of C style
        // arrays, but act like STL containers.
        double pts[num_steps][3];
        for(int i=0; i<num_steps; i++) {
            pts[i][0] = get_x(i, shift);
            pts[i][1] = get_y(i, shift);
            pts[i][2] = get_z(i, shift);
        }
        plots.add_plot1d(pts, "with lines title 'double[N][3]'");
    }

    shift += 1.0/num_examples;

    {
        // Note: C style arrays seem to work, but are a bit fragile since they easily decay to
        // pointers, causing them to forget their lengths.  It is highly recommended that you
        // use std::array instead.  These have the same size and efficiency of C style
        // arrays, but act like STL containers.
        double pts[3][num_steps];
        for(int i=0; i<num_steps; i++) {
            pts[0][i] = get_x(i, shift);
            pts[1][i] = get_y(i, shift);
            pts[2][i] = get_z(i, shift);
        }
        plots.add_plot1d_colmajor(pts, "with lines title 'double[N][3] (colmajor)'");
    }

    shift += 1.0/num_examples;

    {
        // Note: C style arrays seem to work, but are a bit fragile since they easily decay to
        // pointers, causing them to forget their lengths.  It is highly recommended that you
        // use std::array instead.  These have the same size and efficiency of C style
        // arrays, but act like STL containers.
        double x_pts[num_steps];
        double y_pts[num_steps];
        double z_pts[num_steps];
        for(int i=0; i<num_steps; i++) {
            x_pts[i] = get_x(i, shift);
            y_pts[i] = get_y(i, shift);
            z_pts[i] = get_z(i, shift);
        }
        plots.add_plot1d(std::forward_as_tuple(x_pts, y_pts, z_pts),
                "with lines title 'std::tuple of double[N]'");
    }

    shift += 1.0/num_examples;

    {
        std::valarray<std::tuple<double, double, double>> pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            pts[i] = std::tuple(
                    get_x(i, shift), get_y(i, shift), get_z(i, shift));
        }
        plots.add_plot1d(pts, "with lines title 'valarray of std::tuple'");
    }

#ifdef USE_ARMA
    shift += 1.0/num_examples;

    {
        arma::mat pts(num_steps, 3);
        for(int i=0; i<num_steps; i++) {
            pts(i, 0) = get_x(i, shift);
            pts(i, 1) = get_y(i, shift);
            pts(i, 2) = get_z(i, shift);
        }
        plots.add_plot1d(pts, "with lines title 'armadillo N*3'");
    }

    shift += 1.0/num_examples;

    {
        arma::mat pts(3, num_steps);
        for(int i=0; i<num_steps; i++) {
            pts(0, i) = get_x(i, shift);
            pts(1, i) = get_y(i, shift);
            pts(2, i) = get_z(i, shift);
        }
        plots.add_plot1d_colmajor(pts, "with lines title 'armadillo 3*N (colmajor)'");
    }

    shift += 1.0/num_examples;

    {
        arma::Row<double> x_pts(num_steps);
        arma::Col<double> y_pts(num_steps);
        arma::Col<double> z_pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            x_pts(i) = get_x(i, shift);
            y_pts(i) = get_y(i, shift);
            z_pts(i) = get_z(i, shift);
        }
        plots.add_plot1d(std::tuple(x_pts, y_pts, z_pts),
                "with lines title 'tuple of arma Row,Col,Col'");
    }

    shift += 1.0/num_examples;

    {
        arma::field<std::tuple<double,double,double>> pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            pts(i) = std::tuple(
                get_x(i, shift),
                get_y(i, shift),
                get_z(i, shift)
            );
        }
        plots.add_plot1d(pts, "with lines title 'armadillo field of tuple'");
    }
#endif // USE_ARMA

#ifdef USE_EIGEN
    shift += 1.0/num_examples;

    {
        Eigen::MatrixXf pts(num_steps, 3);
        for(int i=0; i<num_steps; i++) {
            pts(i, 0) = get_x(i, shift);
            pts(i, 1) = get_y(i, shift);
            pts(i, 2) = get_z(i, shift);
        }
        plots.add_plot1d(pts, "with lines title 'eigen N*3'");
    }

    shift += 1.0/num_examples;

    {
        Eigen::MatrixXf pts(3, num_steps);
        for(int i=0; i<num_steps; i++) {
            pts(0, i) = get_x(i, shift);
            pts(1, i) = get_y(i, shift);
            pts(2, i) = get_z(i, shift);
        }
        plots.add_plot1d_colmajor(pts, "with lines title 'eigen 3*N (colmajor)'");
    }

    shift += 1.0/num_examples;

    {
        Eigen::VectorXf x_pts(num_steps);
        Eigen::VectorXf y_pts(num_steps);
        Eigen::VectorXf z_pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            x_pts(i) = get_x(i, shift);
            y_pts(i) = get_y(i, shift);
            z_pts(i) = get_z(i, shift);
        }
        plots.add_plot1d(std::tuple(x_pts, y_pts, z_pts),
                "with lines title 'tuple of eigen Col,Col,Col'");
    }

    shift += 1.0/num_examples;

    {
        Eigen::RowVectorXf x_pts(num_steps);
        Eigen::RowVectorXf y_pts(num_steps);
        Eigen::RowVectorXf z_pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            x_pts(i) = get_x(i, shift);
            y_pts(i) = get_y(i, shift);
            z_pts(i) = get_z(i, shift);
        }
        plots.add_plot1d(std::tuple(x_pts, y_pts, z_pts),
                "with lines title 'tuple of eigen Row,Row,Row'");
    }
#endif // USE_EIGEN

#ifdef USE_BLITZ
    shift += 1.0/num_examples;

    {
        blitz::Array<blitz::TinyVector<double, 3>, 1> pts(num_steps);
        for(int i=0; i<num_steps; i++) {
            pts(i)[0] = get_x(i, shift);
            pts(i)[1] = get_y(i, shift);
            pts(i)[2] = get_z(i, shift);
        }
        plots.add_plot1d(pts, "with lines title 'blitz::Array<blitz::TinyVector<double, 3>, 1>'");
    }

    shift += 1.0/num_examples;

    {
        blitz::Array<double, 2> pts(num_steps, 3);
        for(int i=0; i<num_steps; i++) {
            pts(i, 0) = get_x(i, shift);
            pts(i, 1) = get_y(i, shift);
            pts(i, 2) = get_z(i, shift);
        }
        plots.add_plot1d(pts, "with lines title 'blitz<double>(N*3)'");
    }

    shift += 1.0/num_examples;

    {
        blitz::Array<double, 2> pts(3, num_steps);
        for(int i=0; i<num_steps; i++) {
            pts(0, i) = get_x(i, shift);
            pts(1, i) = get_y(i, shift);
            pts(2, i) = get_z(i, shift);
        }
        plots.add_plot1d_colmajor(pts, "with lines title 'blitz<double>(3*N) (colmajor)'");
    }
#endif // USE_BLITZ

    shift += 1.0/num_examples;

    {
        std::function<std::tuple<double,double,double>(int)> f = [&shift](int i) {
            return std::tuple(get_x(i, shift), get_y(i, shift), get_z(i, shift)); };

        auto pts = boost::irange(0, num_steps) | boost::adaptors::transformed(f);

        plots.add_plot1d(pts, "with lines title 'boost transform to tuple'");
    }

    shift += 1.0/num_examples;

    {
        auto steps = boost::irange(0, num_steps);

        plots.add_plot1d(std::tuple(
                steps | boost::adaptors::transformed(boost::bind(get_x, _1, shift)),
                steps | boost::adaptors::transformed(boost::bind(get_y, _1, shift)),
                steps | boost::adaptors::transformed(boost::bind(get_z, _1, shift))
            ), "with lines title 'tuple of boost transform'");
    }

    shift += 1.0/num_examples;

    {
        // Note: C style arrays seem to work, but are a bit fragile since they easily decay to
        // pointers, causing them to forget their lengths.  It is highly recommended that you
        // use std::array instead.  These have the same size and efficiency of C style
        // arrays, but act like STL containers.
        double x_pts[num_steps];
        double y_pts[num_steps];
        double z_pts[num_steps];
        for(int i=0; i<num_steps; i++) {
            x_pts[i] = get_x(i, shift);
            y_pts[i] = get_y(i, shift);
            z_pts[i] = get_z(i, shift);
        }
        // Note: std::tuple doesn't work here since it makes the arrays decay to pointers,
        // and as a result they forget their lengths.
        plots.add_plot1d(std::tie(x_pts, y_pts, z_pts),
                "with lines title 'std::tie of double[N]'");
    }

    gp << plots;

    shift += 1.0/num_examples;
    //std::cout << shift << std::endl;
    assert(std::fabs(shift - 1.0) < 1e-12);

#ifdef _WIN32
    // For Windows, prompt for a keystroke before the Gnuplot object goes out of scope so that
    // the gnuplot window doesn't get closed.
    std::cout << "Press enter to exit." << std::endl;
    std::cin.get();
#endif

    return 0;
}


/** \file string_utils.hpp
 * collection of utilities working with strings
 */

#ifndef STRING_UTILS_HPP
#define STRING_UTILS_HPP

#include <string>
#include <boost/unordered_map.hpp>

#include "utils/exceptions.hpp"
#include "utils/vector2d.hpp"

#define WHITESPACES " \t"

typedef boost::unordered_map<std::string, unsigned> StringCount;

//! remove all characters not in 'skip_to' from the beginning of s
inline void skip_to(std::string& s, const std::string& skip_to)
{
  const size_t pos = s.find_first_of(skip_to);
  if(pos == std::string::npos) s.clear(); else s.erase(0,pos);
}

//! remove all characters in 'to_skip' from the beginning of s
inline void skip_all(std::string& s, const std::string& to_skip)
{
  const size_t pos = s.find_first_not_of(to_skip);
  if(pos == std::string::npos) s.clear(); else s.erase(0,pos);
}

//! consume an integer from the beginning of s and return it
long read_single_number(std::string& s)
{
  long result = std::stoi(s.c_str());
  // remove result from the number
  skip_all(s, "+-");
  skip_all(s, "0123456789");
  return result;
}

//! remove leading & trailing chars (whitespaces by default) from str
std::string trim(const std::string& str, const std::string& to_remove = WHITESPACES)
{
    const size_t first = str.find_first_not_of(to_remove);
    if(first == std::string::npos) return "";
    const size_t last = str.find_last_not_of(to_remove);
    return str.substr(first, last - first + 1);
}

//! returns the Hamming distance between the maximal prefixes of equal length
unsigned hamming_distance(const std::string& s1, const std::string& s2)
{
  unsigned len = std::min(s1.length(), s2.length()) + 1;
  unsigned result = 0;
  while(len--) result += (s1[len] != s2[len]);
  return result;
}
//! returns the Hamming distance between the maximal prefixes of equal length (char* version)
/** Note: no checks are performed, use at own risk
 */
unsigned hamming_distance(const char* s1, const char* s2, unsigned length)
{
  unsigned result = 0;
  while(length--) result += (s1[length] != s2[length]);
  return result;
}

std::vector<unsigned> get_hamming_distances(const std::string& s1, const std::string& s2, const size_t lower_index, const size_t upper_index)
{
  std::vector<unsigned> result(upper_index - lower_index);
  for(unsigned index = lower_index; index < upper_index; ++index){
    const unsigned length = std::min(s1.length() - index, s2.length());
    result[index - lower_index] = hamming_distance(s1.c_str() + index, s2.c_str(), length);
  }
  return result;
}

enum Op : unsigned char {NOOP, DEL_LEFT, DEL_RIGHT, CHANGE};
typedef std::vector2d<Op> Levenstein_Op_Table;

//! modified Levenstein distance of an alignment of two sequences
/** this Levenstein distance is modified to absorb any suffix of s1 or s2 at no cost, as it will be used to
 * find alignments of sub-sequences and we don't want to punish the fact that one of the sequences may be shorter.
 * Note that the seeming advantage of short sequences is countered by using the AVERAGE distance for scoring.
 *
 * NOTE: this is a modification of https://en.wikibooks.org/wiki/Algorithm_Implementation/Strings/Levenshtein_distance#C.2B.2B
 */
std::vector<unsigned> modified_levenstein_distance(const std::string& s1,
                                                   const std::string& s2,
                                                   const size_t lower_index,
                                                   const size_t upper_index,
                                                   Levenstein_Op_Table* op_table = NULL)
{
  assert(upper_index >= lower_index);
  assert(upper_index <= s1.length());
  const ssize_t len1 = s1.length() - lower_index;
  const ssize_t len2 = s2.length();
  const ssize_t window = upper_index - lower_index;
  DEBUG5(std::cout << "computing shifted Levenstein distances for strings of length "<<len1<<" and "<<len2<<" - op table @"<<op_table<<std::endl);
  // d[x,y] = the cost for the substrings s1[x:len1] and s2[y:len2]
  std::vector2d<unsigned> d(len1 + 1, len2 + 1);
  if(op_table) op_table->resize(len1 + 1, len2 + 1);

  // step1: init; allow eating any suffix of s1 OR s2 for free
  for(unsigned i = 0; i <= len1; ++i) d[{i, len2}] = 0;
  for(unsigned j = 1; j <= len2; ++j) d[{len1, j}] = 0;

  // step2: compute inner cells
  for(ssize_t i = len1-1; i >= 0; --i){
    for(ssize_t j = len2-1; j >= 0; --j){
      const unsigned del1_cost = d[{i+1, j}] + 1;
      const unsigned del2_cost = d[{i, j+1}] + 1;
      const unsigned change_cost = d[{i+1, j+1}] + (s1[i + lower_index] == s2[j] ? 0 : 1);
      if(del1_cost < del2_cost){
        if(change_cost < del1_cost){
          d[{i, j}] = change_cost;
          if(op_table) (*op_table)[{i, j}] = CHANGE;
        } else {
          d[{i, j}] = del1_cost;
          if(op_table) (*op_table)[{i, j}] = DEL_LEFT;
        }
      } else {
        if(change_cost < del2_cost){
          d[{i, j}] = change_cost;
          if(op_table) (*op_table)[{i, j}] = CHANGE;
        } else {
          d[{i, j}] = del2_cost;
          if(op_table) (*op_table)[{i, j}] = DEL_RIGHT;
        }
      }
    }
  }

  std::vector<unsigned> result(window);
  for(unsigned i = 0; i < window; ++i) result[i] = d[{i, 0}];

  return result;
}

//! a simple char consensus; This can be used if some transitions are more likely than others
char char_consensus(const char& x, const char& y)
{
  return x;
}

//! retrace an operations table for a Levenstein-distance computation, producting the implied consensus
std::string levenstein_consensus(const std::string& s1, const std::string& s2, const unsigned s2_offset, const Levenstein_Op_Table& op_table)
{
  std::string result;
  const size_t len1 = op_table.size().first - 1;
  const size_t len2 = op_table.size().second - 1;
  size_t i = s2_offset, j = 0;
  Op last_op;

  do{
    last_op = op_table[{i, j}];
    switch(last_op){
      case DEL_LEFT:
        ++i;
        break;
      case DEL_RIGHT:
        ++j;
        break;
      case CHANGE:
        result += char_consensus(s1[i], s2[j + s2_offset]);
        ++i;
        ++j;
        break;
      default:
        break;
    }
  } while(last_op != NOOP);

  if(i == len1) result += s2.substr(j);
  if(j == len2) result += s1.substr(i);
  return result;
}

//! merge two sequences seq1 and seq2 with overlap "overlap" according to their order
/** This functions segfaults for reasons that are beyond me...
 * For now, use the non-segfaulting "merge_strings()" below
 */
std::string merge_strings_segfault(const std::string& seq1,
                            const std::string& seq2,
                            const unsigned overlap)
{
  assert(overlap <= seq1.length());
  const unsigned seq1_len = seq1.length() - overlap;
  const unsigned seq2_len = seq2.length();
  const unsigned new_len = seq1_len + seq2_len;
  char* const new_seq = (char*)malloc(new_len + 1);
  char* const seq1_start = new_seq;
  char* const seq2_start = new_seq + seq1_len;
  std::cout << "got "<<new_len+1<<" bytes @"<<(long)new_seq<<std::endl;

  std::cout << "writing "<<(long)(seq2_start - seq1_start)<<" chars of "<<seq1<<" to "<<(long)seq1_start<<std::endl;
  memcpy(seq1_start, seq1.c_str(), seq1_len);

  std::cout << "writing "<<seq2_len<<" chars of "<<seq2<<" to "<<(long)seq2_start<<std::endl;
  memcpy(seq2_start, seq2.c_str(), seq2_len);
  new_seq[new_len+1] = 0;

  std::string result(new_seq);
  std::cout << "result string is "<< result <<" with c_str @"<<(long)(result.c_str())<<", freeing pointer @"<<(long)new_seq<<std::endl;
  free(new_seq);
  std::cout << "done!"<<std::endl;
  return result;
}

//! merge two sequences seq1 and seq2 with overlap "overlap" according to their order
/** For example: merge_strings("abcde", "1234", 3) = "ab1234"
 */
std::string merge_strings(const std::string& seq1,
                            const std::string& seq2,
                            const unsigned overlap)
{

  std::string result = seq1.substr(0, seq1.length() - overlap) + seq2;
  return result;
}

#endif


#include "stdafx.h"
#include "qmath.h"
#include "config.h"

// each test module could contain no more then one 'main' file with init function defined
// alternatively you could define init function yourself
#include <boost/test/unit_test.hpp>

using namespace math;

namespace {

BOOST_AUTO_TEST_CASE(VEC2)
{
	vec2f a;

    BOOST_CHECK(
			a.x == 0.0 &&
			a.y == 0.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_UNINIT)
{
	vec2f a(vec2f::uninitialized);
	// nothing to check here
}

BOOST_AUTO_TEST_CASE(VEC2_FROMVALUES)
{
	vec2f a(1.0, 2.0);

    BOOST_CHECK(
			a.x == 1.0 &&
			a.y == 2.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_FROMVALUE)
{
	vec2f a(1.0);

    BOOST_CHECK(
			a.x == 1.0 &&
			a.y == 1.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_COPY_CONSTRUCTOR)
{
	vec2f a(1.0, 2.0);
	vec2f b(a);

    BOOST_CHECK(
			b.x == 1.0 &&
			b.y == 2.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_CAST_VEC2U)
{
	vec2u32 a(1, 2);
	vec2f b(a);

    BOOST_CHECK(
			b.x == 1.0 &&
			b.y == 2.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_CAST_VEC3)
{
	vec3f a(1.0, 2.0, 3.0);
	vec2f b(a);

    BOOST_CHECK(
			b.x == 1.0 &&
			b.y == 2.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_CAST_VEC4)
{
	vec4f a(1.0, 2.0, 3.0, 4.0);
	vec2f b(a);

    BOOST_CHECK(
			b.x == 1.0 &&
			b.y == 2.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_SET)
{
	vec2f a(1.0, 2.0);
	a.set(3.0 ,4.0);

    BOOST_CHECK(
			a.x == 3.0 &&
			a.y == 4.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_GET_LENGTH)
{
	vec2f a(1.0, 2.0);
	float len = length(a);

    BOOST_CHECK(
			equals(len, 2.2360680f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_GET_LENGTH_SQ)
{
	vec2f a(1.0, 2.0);
	float len = length_sq(a);

    BOOST_CHECK(
			equals(len, 5.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_SET_LENGTH)
{
	vec2f a(3.0, 4.0);
	a.set_length(5);

    BOOST_CHECK(
			a.x == 3.0 &&
			a.y == 4.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_NORMALIZE)
{
	vec2f a(3.0, 4.0);
	vec2f n = a.normalize();

    BOOST_CHECK(
			equals(n.x, 0.6f) &&
			equals(n.y, 0.8f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_ACCES_OPERATOR)
{
	vec2f a(1.0, 2.0);

    BOOST_CHECK(
			a[0] == 1.0 &&
			a[1] == 2.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_CONST_ACCES_OPERATOR)
{
	vec2f a(1.0, 2.0);
	const float x = a[0];
	const float y = a[1];

	BOOST_CHECK(
			x == 1.0 &&
			y == 2.0
		);
}

BOOST_AUTO_TEST_CASE(VEC2_EQUAL_OPERATOR)
{
	vec2f a(1.0, 2.0);
	vec2f b(1.0, 2.0);

    BOOST_CHECK(
			a == b
		);
}

BOOST_AUTO_TEST_CASE(VEC2_NOT_EQUAL_OPERATOR)
{
	vec2f a(1.0, 2.0);
	vec2f b(2.0, 3.0);

    BOOST_CHECK(
			a != b
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MINUS)
{
	vec2f a(1.0, 2.0);
	vec2f b = -a;

    BOOST_CHECK(
			equals(b.x, -1.0f) &&
			equals(b.y, -2.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_PLUS_SCALAR)
{
	vec2f a(1.0, 2.0);
	vec2f c = a + 2.0f;

    BOOST_CHECK(
			equals(c.x, 3.0f) &&
			equals(c.y, 4.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_PLUS_VEC)
{
	vec2f a(1.0, 2.0);
	vec2f b(2.0, 3.0);
	vec2f c = a + b;

    BOOST_CHECK(
			equals(c.x, 3.0f) &&
			equals(c.y, 5.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MINUS_SCALAR)
{
	vec2f a(1.0, 2.0);
	vec2f b = a - 2.0f;

    BOOST_CHECK(
			equals(b.x, -1.0f) &&
			equals(b.y, 0.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MINUS_VEC)
{
	vec2f a(1.0, 2.0);
	vec2f b(2.0, 3.0);
	vec2f c = b - a;

    BOOST_CHECK(
			equals(c.x, 1.0f) &&
			equals(c.y, 1.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MULTIPLY_SCALAR)
{
	vec2f a(1.0, 2.0);
	vec2f c = a * 2.0f;

    BOOST_CHECK(
			equals(c.x, 2.0f) &&
			equals(c.y, 4.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MULTIPLY_VEC)
{
	vec2f a(1.0, 2.0);
	vec2f b(2.0, 3.0);
	vec2f c = a * b;

    BOOST_CHECK(
			equals(c.x, 2.0f) &&
			equals(c.y, 6.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_DIVIDE_SCALAR)
{
	vec2f a(1.0, 2.0);
	vec2f c = a / 2.0f;

    BOOST_CHECK(
			equals(c.x, 0.5f) &&
			equals(c.y, 1.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_EQUALS)
{
	vec2f a(1.0, 2.0);
	vec2f b;

	b = a;

    BOOST_CHECK(
			equals(b.x, 1.0f) &&
			equals(b.y, 2.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_EQUALS_VEC2U)
{
	vec2u32 a(1, 2);
	vec2f c(a);

    BOOST_CHECK(
			equals(c.x, 1.f) &&
			equals(c.y, 2.f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_EQUALS_VEC3)
{
	vec3f b(2.0, 3.0, 4.0);
	vec2f c;

	c = b;

	BOOST_CHECK(
			equals(c.x, 2.0f) &&
			equals(c.y, 3.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_EQUALS_VEC4)
{
	vec4f b(2.0, 3.0, 4.0, 5.0f);
	vec2f c;

	c = b;

	BOOST_CHECK(
			equals(c.x, 2.0f) &&
			equals(c.y, 3.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_PLUS_EQUALS_SCALAR)
{
	vec2f a(1.0f, 2.0f);

	a += 2.0f;

	BOOST_CHECK(
			equals(a.x, 3.0f) &&
			equals(a.y, 4.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_PLUS_EQUALS_VEC)
{
	vec2f a(1.0f, 2.0f);
	vec2f b(1.0f, 2.0f);

	a += b;

	BOOST_CHECK(
			equals(a.x, 2.0f) &&
			equals(a.y, 4.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MINUS_EQUALS_SCALAR)
{
	vec2f a(1.0f, 2.0f);

	a -= 2.0f;

	BOOST_CHECK(
			equals(a.x, -1.0f) &&
			equals(a.y, 0.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MINUS_EQUALS_VEC)
{
	vec2f a(1.0f, 2.0f);
	vec2f b(1.0f, 2.0f);

	a -= b;

	BOOST_CHECK(
			equals(a.x, 0.0f) &&
			equals(a.y, 0.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MULTIPLY_EQUALS_SCALAR)
{
	vec2f a(1.0f, 2.0f);

	a *= 2.0f;

	BOOST_CHECK(
			equals(a.x, 2.0f) &&
			equals(a.y, 4.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_MULTIPLY_EQUALS_VEC)
{
	vec2f a(1.0f, 2.0f);
	vec2f b(1.0f, 2.0f);

	a *= b;

	BOOST_CHECK(
			equals(a.x, 1.0f) &&
			equals(a.y, 4.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_DIVIDE_EQUALS_SCALAR)
{
	vec2f a(1.0f, 2.0f);

	a /= 2.0f;

	BOOST_CHECK(
			equals(a.x, 0.5f) &&
			equals(a.y, 1.0f)
		);
}

BOOST_AUTO_TEST_CASE(VEC2_OPERATOR_DIVIDE_EQUALS_VEC)
{
	vec2f a(1.0f, 2.0f);
	vec2f b(1.0f, 2.0f);

	a /= b;

	BOOST_CHECK(
			equals(a.x, 1.0f) &&
			equals(a.y, 1.0f)
		);
}

BOOST_AUTO_TEST_CASE(SCALAR_OPERATOR_MULTIPLY_VEC2)
{
	vec2f a(1.0f, 2.0f);
	vec2f b;

	b = 2.0f * a;

	BOOST_CHECK(
			equals(b.x, 2.0f) &&
			equals(b.y, 4.0f)
		);
}

}


#include "./../../header/analyzer.h"
#include "./../../header/tarjan.h"
#include "./../../header/nuutila.h"
#include "./../../header/pearce.h"

//STL
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iterator>
#include <fstream>
//Boost
#include <boost/config.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list_io.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/copy.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost::system;
namespace filesys = boost::filesystem;
#ifndef USING_BOOST
#define USING_BOOST
#endif
/**
 * @brief This function gives the current date and time for the experiments and the logs
 *
 * @return std::string
 */
std::string Analyzer::currentDateTime()
{
	time_t now = time(0);
	struct tm tstruct;
	char buf[80];
	tstruct = *localtime(&now);
	// Visit http://en.cppreference.com/w/cpp/chrono/c/strftime
	// for more information about date/time format
	strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct);

	return buf;
}
/**
 * @brief gets all of the graph files from the directory and returns them in a vector
 *
 * @param dirPath
 * @return std::vector<std::string>
 */
std::vector<std::string> Analyzer::getInputList(std::string &dirPath)
{
	std::vector<std::string> inputGraphs;
	try
	{
		if (filesys::exists(dirPath) && filesys::is_directory(dirPath))
		{
			filesys::recursive_directory_iterator iter(dirPath);
			filesys::recursive_directory_iterator end;

			// Iterate till end
			while (iter != end)
			{
				if (!filesys::is_directory(iter->path().string()))
				{
					inputGraphs.push_back(iter->path().string());
				}

				error_code ec;
				// Increment the iterator to point to next entry in recursive iteration
				iter.increment(ec);
				if (ec)
				{
					std::cerr << "Error While Accessing : " << iter->path().string() << " :: " << ec.message() << '\n';
				}
			}
		}
	}
	catch (std::system_error &e)
	{
		std::cerr << "Exception :: " << e.what();
	}
	return inputGraphs;
}

std::string Analyzer::benchmark_comparison(bool write_to_file, bool isCsv, std::string &inputDirectory)
{
	Visualize v;
	std::string csv_name = "None";
	boost::filesystem::path full_path(boost::filesystem::current_path());
	std::string dateTime = currentDateTime();
	std::string file_location_csv ;
	std::string file_location ;
	if(write_to_file){
		file_location = full_path.string() + "/logs/experiment-" + dateTime + ".log";
		std::ofstream logFile;
		logFile.open(file_location, std::ios::app);
		logFile << "This log is created with the " + inputDirectory + " directory" << std::endl;
		logFile.close();
		if(isCsv){
			file_location_csv = full_path.string() + "/data/experiment_" + dateTime + ".csv";
			csv_name = "experiment_" + dateTime + ".csv";
		}
	}
	if (write_to_file)
	{
		v.writeTableBanner( file_location);
	}
	v.printTableBanner();

	UtilityStructs::StorageItems r_t;

	for (std::vector<std::string>::iterator it = graphList.begin(); it != graphList.end(); it++)
	{
		std::vector<UtilityStructs::StorageItems> results;
		Tarjan t(*it);
		r_t = t.ApplySCC(false);
		int vertex = r_t.vertexCount;
		int edges = r_t.edgeCount;
		int id = it - graphList.begin();
		results.push_back(r_t);
		Nuutila n(*it);
		r_t = n.ApplySCC_Original();
		results.push_back(r_t);
		r_t = n.ApplySCC_v1();
		results.push_back(r_t);
		r_t = n.ApplySCC_v2();
		results.push_back(r_t);
		Pearce p(*it);
		r_t = p.Pea_Find_SCC1();
		results.push_back(r_t);
		r_t = p.Pea_Find_SCC2();
		results.push_back(r_t);
		r_t = p.Pea_Find_SCC3();
		results.push_back(r_t);
		if (write_to_file)
		{
			if (isCsv)
			{
				v.writeExperimentRow_CSV( id, vertex, edges, results, file_location_csv);
			}
			v.writeExperimentRow( id, vertex, edges, results, file_location);
		}

		v.printExperimentRow( id, vertex, edges, results);

	}
	if(isCsv){
		v.printLine("The CSV file is saved into " + file_location_csv);
	}
	if(write_to_file){
		v.printLine("The Log file is saved into " + file_location);
	}
	return csv_name;
}

void Analyzer::solve_with_tarjan(std::string &inputFile){
	Visualize v;
	Tarjan t(inputFile);
	UtilityStructs::StorageItems s;
	v.printProgramBottom();
	v.printLine( "Tarjan implementation is processing the graph");
	v.printProgramBottom();
	s = t.ApplySCC(true);
	v.printProgramBottom();
}

void Analyzer::solve_with_pearce(std::string &inputFile){
	Visualize v;
	Pearce p(inputFile);
	UtilityStructs::StorageItems s;
	v.printLine( "Pearce SCC implementation mark 1 is processing the graph");
	v.printProgramBottom();
	s = p.Pea_Find_SCC1();
	p.print_result_max(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("Pearce SCC implementation mark 2 is processing the graph");
	v.printProgramBottom();
	s = p.Pea_Find_SCC2();
	p.print_result_min(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("Pearce SCC implementation mark 3 is processing the graph");
	v.printProgramBottom();
	s = p.Pea_Find_SCC3();
	p.print_result_min(s);
	v.printProgramBottom();
}

void Analyzer::solve_with_nuutila(std::string &inputFile){
	Visualize v;
	UtilityStructs::StorageItems s;
	Nuutila p(inputFile);
	v.printProgramBottom();
	v.printLine( "The Nuutila Version of the SCC Algorithms is processing the graph");
	v.printProgramBottom();
	s = p.ApplySCC_Original();
	p.print_sccs(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("The Nuutila First Modified Version of the SCC Algorithms is processing the graph");
	v.printProgramBottom();
	s = p.ApplySCC_v1();
	p.print_sccs(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("The Nuutila Second Modified Version of the SCC Algorithms is processing the graph");
	v.printProgramBottom();
	s = p.ApplySCC_v2();
	p.print_sccs(s);
	v.printProgramBottom();
}

void Analyzer::solve_with_all(std::string &inputFile){
	Visualize v;
	Tarjan t(inputFile);
	UtilityStructs::StorageItems s;
	v.printProgramBottom();
	v.printLine( "Tarjan implementation is processing the graph");
	v.printProgramBottom();
	s = t.ApplySCC(true);
	v.printProgramBottom();
	std::cout << std::endl;
	Pearce p(inputFile);
	v.printProgramBottom();
	v.printLine( "Pearce SCC implementation mark 1 is processing the graph");
	v.printProgramBottom();
	s = p.Pea_Find_SCC1();
	p.print_result_max(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("Pearce SCC implementation mark 2 is processing the graph");
	v.printProgramBottom();
	s = p.Pea_Find_SCC2();
	p.print_result_min(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("Pearce SCC implementation mark 3 is processing the graph");
	v.printProgramBottom();
	s = p.Pea_Find_SCC3();
	p.print_result_min(s);
	v.printProgramBottom();
	Nuutila n(inputFile);
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine( "The Nuutila Version of the SCC Algorithms is processing the graph");
	v.printProgramBottom();
	s = n.ApplySCC_Original();
	n.print_sccs(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("The Nuutila First Modified Version of the SCC Algorithms is processing the graph");
	v.printProgramBottom();
	s = n.ApplySCC_v1();
	n.print_sccs(s);
	v.printProgramBottom();
	std::cout <<std::endl;
	v.printProgramBottom();
	v.printLine("The Nuutila Second Modified Version of the SCC Algorithms is processing the graph");
	v.printProgramBottom();
	s = n.ApplySCC_v2();
	n.print_sccs(s);
	v.printProgramBottom();
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <map>
#include <Eigen/Geometry>

#ifndef ABB_IRB1600_145_FK_FAST_HPP
#define ABB_IRB1600_145_FK_FAST_HPP

namespace ABB_IRB1600_145_FK_FAST
{
    const size_t ABB_IRB1600_145_NUM_ACTIVE_JOINTS = 6;
    const size_t ABB_IRB1600_145_NUM_LINKS = 8;

    const std::string ABB_IRB1600_145_ACTIVE_JOINT_1_NAME = "joint_1";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_2_NAME = "joint_2";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_3_NAME = "joint_3";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_4_NAME = "joint_4";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_5_NAME = "joint_5";
    const std::string ABB_IRB1600_145_ACTIVE_JOINT_6_NAME = "joint_6";

    typedef std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d>> VectorAffine3d;

    inline Eigen::Affine3d Get_base_joint1_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1245);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        Eigen::Affine3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Affine3d Get_link_1_joint_2_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.15, -0.1395, 0.362);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
        Eigen::Affine3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Affine3d Get_link_2_joint_3_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.0, 0.028, 0.7);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
        Eigen::Affine3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Affine3d Get_link_3_joint_4_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.314, 0.107, 0.0);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
        Eigen::Affine3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Affine3d Get_link_4_joint_5_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.286, 0.0, 0.0);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
        Eigen::Affine3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Affine3d Get_link_5_joint_6_LinkJointTransform(const double joint_val)
    {
        Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
        Eigen::Affine3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Affine3d Get_Fixed_link_6_joint_tool_LinkJointTransform(void)
    {
        Eigen::Translation3d pre_joint_translation(0.065, 0.0, 0.0);
        Eigen::Quaterniond pre_joint_rotation(0.7071067811865476, 0.0, 0.7071067811865476, 0.0);
        Eigen::Affine3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        return pre_joint_transform;
    }

    inline VectorAffine3d GetLinkTransforms(const std::vector<double>& configuration)
    {
        assert(configuration.size() == ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
        VectorAffine3d link_transforms(ABB_IRB1600_145_NUM_LINKS);
        link_transforms[0] = Eigen::Affine3d::Identity();
        link_transforms[1] = link_transforms[0] * Get_base_joint1_LinkJointTransform(configuration[0]);
        link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
        link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
        link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
        link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
        link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
        link_transforms[7] = link_transforms[6] * Get_Fixed_link_6_joint_tool_LinkJointTransform();
        return link_transforms;
    }

    inline VectorAffine3d GetLinkTransforms(std::map<std::string, double> configuration)
    {
        std::vector<double> configuration_vector(ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
        configuration_vector[0] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_1_NAME];
        configuration_vector[1] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_2_NAME];
        configuration_vector[2] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_3_NAME];
        configuration_vector[3] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_4_NAME];
        configuration_vector[4] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_5_NAME];
        configuration_vector[5] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_6_NAME];
        return GetLinkTransforms(configuration_vector);
    }
}

#endif // ABB_IRB1600_145_FK_FAST_HPP


#include <vector>
#include <iostream>
#include <fstream>
#include <assert.h>

#include <NTL/lzz_pX.h>

#include "oneqfe/ss.h"
#include "bounded/gvw.h"

template<class OneQS>
GVW<OneQS>::GVW(int keys, int depth, int kappa, int modulus, bool useDelta, CircuitDescription *description) {
  key_limit = keys;
  this->depth = depth;
  secret_shares = key_limit * key_limit * kappa;
  total_shares = depth * depth * key_limit * key_limit * secret_shares;
  delta_size = kappa;
  delta_pool_size = delta_size * key_limit * key_limit;
  this->useDelta = useDelta;

  assert(description->type == CIRCUIT_TYPE_INNER_PRODUCT_MOD_P);
  if (useDelta) {
    description = new InnerProductModPDeltaCircuitDescription(description->getMod(), description->circuit_size/description->getModBits(), delta_pool_size);
  }
  oneqfe = new OneQS(description);
  assert(modulus > total_shares);
  NTL::zz_p::init(modulus);
}

template<class OneQS>
GVW<OneQS>::GVW(int keys, int depth, int secret_shares, int total_shares, int delta_size, int delta_pool_size, int modulus, bool useDelta, CircuitDescription *description) {
  key_limit = keys;
  this->depth = depth;
  this->total_shares = total_shares;
  this->secret_shares = secret_shares;
  this->delta_size = delta_size;
  this->delta_pool_size = delta_pool_size;
  this->useDelta = useDelta;

  assert(description->type == CIRCUIT_TYPE_INNER_PRODUCT_MOD_P);
  if (useDelta) {
    description = new InnerProductModPDeltaCircuitDescription(description->getMod(), description->circuit_size/description->getModBits(), delta_pool_size);
  }
  oneqfe = new OneQS(description);
  assert(modulus > total_shares);
  NTL::zz_p::init(modulus);
}

template<class OneQS>
typename GVW<OneQS>::KeyPair GVW<OneQS>::Setup(int length) {
  typename GVW<OneQS>::KeyPair p;
  p.sk = GVW<OneQS>::MasterSecretKey(total_shares);
  p.pk = GVW<OneQS>::MasterPublicKey(total_shares);

  for (int i = 0; i < total_shares; i++) {
    typename OneQS::KeyPair p1 = oneqfe->Setup(length);
    p.sk.sks[i] = p1.sk;
    p.pk.pks[i] = p1.pk;
  }

  return p;
}

template<class OneQS>
typename GVW<OneQS>::SecretKey GVW<OneQS>::KeyGen(typename GVW<OneQS>::MasterSecretKey msk, Circuit *circuit) {
  typename GVW<OneQS>::SecretKey sk;

  // Pick random instances for the secret_shares
  sk.Gamma.resize(total_shares);
  for (size_t i = 0; i < sk.Gamma.size(); i++) {
    sk.Gamma[i] = i;
  }
  std::random_shuffle(sk.Gamma.begin(), sk.Gamma.end());
  sk.Gamma.resize(secret_shares * depth + 1);

  if (useDelta) {
    // Pick random values for Delta.
    sk.Delta.resize(delta_pool_size);
    for (size_t i = 0; i < sk.Delta.size(); i++) {
      sk.Delta[i] = i;
    }
    std::random_shuffle(sk.Delta.begin(), sk.Delta.end());
    sk.Delta.resize(delta_size);

    circuit = new InnerProductModPDeltaCircuit(*((InnerProductModPCircuit *) circuit), delta_pool_size, sk.Delta);
  }

  sk.sks.resize(secret_shares * depth + 1);
  for (size_t i = 0; i < sk.sks.size(); i++) {
    sk.sks[i] = oneqfe->KeyGen(msk.sks[sk.Gamma[i]], circuit);
  }
  return sk;
}

template<class OneQS>
typename GVW<OneQS>::CipherText GVW<OneQS>::Encrypt(typename GVW<OneQS>::MasterPublicKey mpk, std::vector<int> msg) {
  std::vector<NTL::zz_pX> msg_polys(msg.size());
  std::vector<NTL::zz_pX> zeta_polys(delta_pool_size);

  // Randomize polynomials
  for (size_t i = 0; i < msg.size(); i++) {
    NTL::random(msg_polys[i], (long) secret_shares);
    NTL::SetCoeff(msg_polys[i], 0, msg[i]);
  }

  // Randomize zeta polynomials if using
  if (useDelta) {
    for (int i = 0; i < delta_pool_size; i++) {
      NTL::random(zeta_polys[i], (long) (secret_shares * depth));
      NTL::SetCoeff(zeta_polys[i], 0, 0);
    }
  }

  typename GVW<OneQS>::CipherText ct(total_shares);

  for (int i = 0; i < total_shares; i++) {
    std::vector<int> poly_points(msg.size());

    // evaluate the polynomials
    for (size_t j = 0; j < msg.size(); j++) {
      NTL::zz_p poly_value, poly_point;
      NTL::zz_pX poly = msg_polys[j];
      NTL::conv(poly_point, (long) i + 1);
      NTL::eval(poly_value, poly, poly_point);
      poly_points[j] = (int) NTL::rep(poly_value);
    }

    if (useDelta) {
      poly_points.resize(msg.size() + delta_pool_size);

      for (int j = 0; j < delta_pool_size; j++) {
        NTL::zz_p poly_value, poly_point;
        NTL::zz_pX poly = zeta_polys[j];
        NTL::conv(poly_point, (long) i + 1);
        NTL::eval(poly_value, poly, poly_point);
        poly_points[j + msg.size()] = (int) NTL::rep(poly_value);
      }
    }

    // Encrypt the resulting points
    ct.cts[i] = oneqfe->Encrypt(mpk.pks[i], poly_points);
  }

  return ct;
}

template<class OneQS>
std::vector<int> GVW<OneQS>::Decrypt(typename GVW<OneQS>::SecretKey sk, typename GVW<OneQS>::CipherText ct) {
  std::vector<std::vector<int> > poly_outputs(sk.Gamma.size());

  // decrypt points on each polynomial
  for (size_t i = 0; i < sk.Gamma.size(); i++) {
    poly_outputs[i] = oneqfe->Decrypt(sk.sks[i], ct.cts[sk.Gamma[i]]);
  }

  std::vector<int> outputs(poly_outputs[0].size());
  for (size_t i = 0; i < outputs.size(); i++) {
    NTL::zz_pX f;

    // Set up values for polynomial interpolation
    NTL::vec_zz_p poly_in, poly_out;
    poly_in.SetLength(sk.Gamma.size());
    poly_out.SetLength(sk.Gamma.size());
    for (size_t j = 0; j < sk.Gamma.size(); j++) {
      poly_in[j] = sk.Gamma[j] + 1;
      poly_out[j] = (long) poly_outputs[j][i];
    }

    // Interpolate, and recover the desired outputs
    NTL::interpolate(f, poly_in, poly_out);
    NTL::zz_p zeroval, zero;
    NTL::conv(zero, (long) 0);
    NTL::eval(zeroval, f, zero);
    outputs[i] = (int) NTL::rep(zeroval);
  }

  return outputs;
}

template class GVW<SS_AES>;
template class GVW<SS_RSA>;
template class GVW<SS_SingletonAES>;
template class GVW<SS_SingletonRSA>;