// All content Copyright (C) 2018 Genomics plc
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include "utils/NeedlemanWunsch.hpp"

using wecall::utils::BasePairSequence;
using wecall::utils::NeedlemanWunsch;
using wecall::utils::NWPenalties;
using wecall::utils::NWVariant;

BOOST_AUTO_TEST_CASE( shouldRealWorld1 )
{
    const BasePairSequence ref =
        "ACGGCACCCCCAGAGCCACGAGCACACCGCCACCCCCAGAGCCACGAGCACACCGGCACCCCCAGAGCCATGAGCACACCGGCACCCCCAGAGCCAT";
    const BasePairSequence alt =
        "CCGGCACCCCCAGAGCCACGAGCACACCGGCACCCCCAGAGCCATGAGCACACCGGCACCCCCAGAGCCACGAGCACACCAGCACCCCCAGAGCCAC";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 6 );
    BOOST_CHECK_EQUAL( nWVariants[5], NWVariant( 0, 1, "C" ) );
    BOOST_CHECK_EQUAL( nWVariants[4], NWVariant( 29, 30, "G" ) );
    BOOST_CHECK_EQUAL( nWVariants[3], NWVariant( 44, 45, "T" ) );
    BOOST_CHECK_EQUAL( nWVariants[2], NWVariant( 70, 71, "C" ) );
    BOOST_CHECK_EQUAL( nWVariants[1], NWVariant( 80, 81, "A" ) );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 96, 97, "C" ) );
}

BOOST_AUTO_TEST_CASE( shouldRealWorld2 )
{
    const BasePairSequence ref = "TACCTGCCACCATGCCCAGCTAATTTTTTTTTTTTG";
    const BasePairSequence alt = "CACCTGCCACCATGCCCAGCTAATTTTTTTTTTTTTTTTTTTG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 2 );
    BOOST_CHECK_EQUAL( nWVariants[1], NWVariant( 0, 1, "C" ) );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 23, 23, "TTTTTTT" ) );
}

BOOST_AUTO_TEST_CASE( shouldRealWorld3 )
{
    const BasePairSequence ref = "AAGCCAGGTGTGGT";
    const BasePairSequence alt = "GCCAGGGGTGGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 3 );
    BOOST_CHECK_EQUAL( nWVariants[2], NWVariant( 0, 2, "" ) );
    BOOST_CHECK_EQUAL( nWVariants[1], NWVariant( 8, 9, "G" ) );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 13, 14, "G" ) );
}

BOOST_AUTO_TEST_CASE( shouldRealWorld4 )
{
    const BasePairSequence ref =
        "TCCTCTTCCTTCCTTCCTTTTCTCTCTCTTTCTTCCTTCCTTTCTTTCTTTCTCTTTCTTTTTCTTTTTCTTTCTTTCTTTTCTTTCTTTTCTTTCTTTTCCTTCCTTCC"
        "TTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTTCCTTCCTTCCTTCCTTTCTTTCTA";
    const BasePairSequence alt =
        "TCCTCTTCCTTCCTTCCTTTTCTCTCTCTCTCTTTCTTCCTTCCTTTCTTTCTTTCTCTTTCTTTTTCTTTTTCTTTCTTTCTTTTTCTTTCTTTCTTTTCTTTCTTTTC"
        "TTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTCCTTTCCTTCCTTCCTTCCTTTCTTTCTA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 3 );
    BOOST_CHECK_EQUAL( nWVariants[2], NWVariant( 20, 20, "TCTC" ) );
    BOOST_CHECK_EQUAL( nWVariants[1], NWVariant( 79, 79, "TTTTC" ) );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 98, 98, "TTCTTTCCTTCCTTCC" ) );
}

BOOST_AUTO_TEST_CASE( shouldRealWorld5 )
{
    const BasePairSequence ref = "GCCCCAGCCTCCCAAAGTGCATTGATTTTGTTGTTGTTGTGCTTATTTGCACTCCAGCCTGGCCTCTCCTTTCTTG";
    const BasePairSequence alt = "GCCCCAGCCTCCCAAAGTGCTGGGATTACAAGTGTGAACCATCGTGCCTGGCCTCTCCTTTCTTG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 9 );
    BOOST_CHECK_EQUAL( nWVariants[8], NWVariant( 20, 37, "" ) );
    BOOST_CHECK_EQUAL( nWVariants[7], NWVariant( 39, 40, "G" ) );
    BOOST_CHECK_EQUAL( nWVariants[6], NWVariant( 41, 42, "A" ) );
    BOOST_CHECK_EQUAL( nWVariants[5], NWVariant( 45, 45, "CAAG" ) );
    BOOST_CHECK_EQUAL( nWVariants[4], NWVariant( 46, 47, "G" ) );
    BOOST_CHECK_EQUAL( nWVariants[3], NWVariant( 49, 50, "A" ) );
    BOOST_CHECK_EQUAL( nWVariants[2], NWVariant( 52, 52, "CA" ) );
    BOOST_CHECK_EQUAL( nWVariants[1], NWVariant( 54, 55, "G" ) );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 55, 56, "T" ) );
}

BOOST_AUTO_TEST_CASE( shouldRealWorld6 )
{
    const BasePairSequence ref = "GAGGGTCCTGCAAGGAACTGCGGGAAGTCTGGAGACGGCAGG";
    const BasePairSequence alt = "AGCCCCCCACCACCCCTGCAAGGAACTGCGGGAAGTCTGGAGACGGCAGA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 3 );
    BOOST_CHECK_EQUAL( nWVariants[2], NWVariant( 0, 0, "AGCCCCCCACCACC" ) );
    BOOST_CHECK_EQUAL( nWVariants[1], NWVariant( 0, 6, "" ) );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 41, 42, "A" ) );
}

BOOST_AUTO_TEST_CASE( shouldNoVariants )
{
    const BasePairSequence ref = "ATG";
    const BasePairSequence alt = "ATG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 0 );
}

BOOST_AUTO_TEST_CASE( shouldPureOneBasedSNP )
{
    const BasePairSequence ref = "A";
    const BasePairSequence alt = "T";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 1, "T" ) );
}

BOOST_AUTO_TEST_CASE( shouldPureOneBasedDeletion )
{
    const BasePairSequence ref = "A";
    const BasePairSequence alt = "";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 1, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldPureOneBasedInsertion )
{
    const BasePairSequence ref = "";
    const BasePairSequence alt = "A";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 0, "A" ) );
}

BOOST_AUTO_TEST_CASE( shouldPureMultiBasedDeletion )
{
    const BasePairSequence ref = "AAAAAAAAAA";
    const BasePairSequence alt = "";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 10, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldPureMultiBasedInsertion )
{
    const BasePairSequence ref = "";
    const BasePairSequence alt = "AAAAAAAAAA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 0, "AAAAAAAAAA" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindSNPInCentre )
{
    const BasePairSequence ref = "ATG";
    const BasePairSequence alt = "AGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 2, "G" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindDeletionInCentre )
{
    const BasePairSequence ref = "ACGG";
    const BasePairSequence alt = "AGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 2, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindInsertionInCentre )
{
    const BasePairSequence ref = "AGG";
    const BasePairSequence alt = "ACGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 1, "C" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindTwoBaseDeletionInCentre )
{
    const BasePairSequence ref = "AGGC";
    const BasePairSequence alt = "AC";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 3, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindTwoBaseInsertionInCentre )
{
    const BasePairSequence ref = "AC";
    const BasePairSequence alt = "AGGC";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 1, "GG" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindSNPAtStart )
{
    const BasePairSequence ref = "ACG";
    const BasePairSequence alt = "TCG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 1, "T" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindDeletionAtStart )
{
    const BasePairSequence ref = "ACGG";
    const BasePairSequence alt = "CGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 1, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindInsertionAtStart )
{
    const BasePairSequence ref = "CGG";
    const BasePairSequence alt = "ACGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 0, "A" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindMultibaseDeletionAtStart )
{
    const BasePairSequence ref = "AACGG";
    const BasePairSequence alt = "CGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 2, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindMultibaseInsertionAtStart )
{
    const BasePairSequence ref = "CGG";
    const BasePairSequence alt = "AACGG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 0, 0, "AA" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindSNPAtEnd )
{
    const BasePairSequence ref = "ACG";
    const BasePairSequence alt = "ACT";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 2, 3, "T" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindDeletionAtEnd )
{
    const BasePairSequence ref = "ACGT";
    const BasePairSequence alt = "ACG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 3, 4, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldFindInsertionAtEnd )
{
    const BasePairSequence ref = "ACG";
    const BasePairSequence alt = "ACGT";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 3, 3, "T" ) );
}

BOOST_AUTO_TEST_CASE( shouldPreferTwoIndelsToTenSNPs )
{
    const BasePairSequence ref = "CACCATGCCCAGCTAAT";
    const BasePairSequence alt = "ACCATGCCCAGCTTAAT";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 2 );
    BOOST_CHECK_EQUAL( nWVariants[1], NWVariant( 0, 1, "" ) );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 13, 13, "T" ) );
}

BOOST_AUTO_TEST_CASE( shouldLeftAlignInsertion )
{
    const BasePairSequence ref = "ACG";
    const BasePairSequence alt = "ACCG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 1, "C" ) );
}

BOOST_AUTO_TEST_CASE( shouldLeftAlignDeletion )
{
    const BasePairSequence ref = "ACCG";
    const BasePairSequence alt = "ACG";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 2, "" ) );
}

BOOST_AUTO_TEST_CASE( shouldCreateVariantsWithGapChar )
{
    const BasePairSequence ref = "ANA";
    const BasePairSequence alt = "AAA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );
    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
    BOOST_CHECK_EQUAL( nWVariants[0], NWVariant( 1, 2, "A" ) );
}

BOOST_AUTO_TEST_CASE( shouldTimeAlgorithm100 )
{
    const bool skipped = true;
    if ( skipped )
    {
        return;
    }
    const BasePairSequence ref =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA";
    const BasePairSequence alt =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );

    struct timespec start, finish;
    double elapsed;

    clock_gettime( CLOCK_MONOTONIC, &start );

    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    clock_gettime( CLOCK_MONOTONIC, &finish );

    elapsed = ( finish.tv_sec - start.tv_sec );
    elapsed += ( finish.tv_nsec - start.tv_nsec ) / 1000000000.0;
    std::cout << "Time taken for n = 10^4: " << elapsed << std::endl;

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 0 );
}

BOOST_AUTO_TEST_CASE( shouldTimeAlgorithmMixedInsertion )
{
    const bool skipped = true;
    if ( skipped )
    {
        return;
    }
    const BasePairSequence ref =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA";
    const BasePairSequence alt =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );

    struct timespec start, finish;
    double elapsed;

    clock_gettime( CLOCK_MONOTONIC, &start );

    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    clock_gettime( CLOCK_MONOTONIC, &finish );

    elapsed = ( finish.tv_sec - start.tv_sec );
    elapsed += ( finish.tv_nsec - start.tv_nsec ) / 1000000000.0;
    std::cout << "Time taken for n = 10^5: " << elapsed << std::endl;

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
}

BOOST_AUTO_TEST_CASE( shouldTimeAlgorithmMixedInsertionWithUnnormalizedVariants )
{
    const bool skipped = true;
    if ( skipped )
    {
        return;
    }
    const BasePairSequence ref =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA";
    const BasePairSequence alt =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );

    struct timespec start, finish;
    double elapsed;

    clock_gettime( CLOCK_MONOTONIC, &start );

    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    clock_gettime( CLOCK_MONOTONIC, &finish );

    elapsed = ( finish.tv_sec - start.tv_sec );
    elapsed += ( finish.tv_nsec - start.tv_nsec ) / 1000000000.0;
    std::cout << "Time taken for n = 10^5: " << elapsed << std::endl;

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
}

BOOST_AUTO_TEST_CASE( shouldTimeAlgorithmMixedDeletion )
{
    const bool skipped = true;
    if ( skipped )
    {
        return;
    }
    const BasePairSequence ref =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAA";
    const BasePairSequence alt =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );

    struct timespec start, finish;
    double elapsed;

    clock_gettime( CLOCK_MONOTONIC, &start );

    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    clock_gettime( CLOCK_MONOTONIC, &finish );

    elapsed = ( finish.tv_sec - start.tv_sec );
    elapsed += ( finish.tv_nsec - start.tv_nsec ) / 1000000000.0;
    std::cout << "Time taken for n = 10^5: " << elapsed << std::endl;

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 1 );
}

BOOST_AUTO_TEST_CASE( shouldTimeAlgorithm1000 )
{
    const bool skipped = true;
    if ( skipped )
    {
        return;
    }
    const BasePairSequence ref =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAA";
    const BasePairSequence alt =
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
        "AAAAAAAAAA";

    const auto nWPenalties = NWPenalties();
    auto needlemanWunsch = NeedlemanWunsch( ref, alt, nWPenalties );

    struct timespec start, finish;
    double elapsed;

    clock_gettime( CLOCK_MONOTONIC, &start );

    needlemanWunsch.getScoreMatrix();
    auto nWVariants = needlemanWunsch.traceBack();

    clock_gettime( CLOCK_MONOTONIC, &finish );

    elapsed = ( finish.tv_sec - start.tv_sec );
    elapsed += ( finish.tv_nsec - start.tv_nsec ) / 1000000000.0;
    std::cout << "Time taken for n = 10^6: " << elapsed << std::endl;

    BOOST_REQUIRE_EQUAL( nWVariants.size(), 0 );
}


// File: eigen_matrix.hpp
// Project: eigen
// Created Date: 06/07/2021
// Author: Shun Suzuki
// -----
// Last Modified: 29/09/2021
// Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
// -----
// Copyright (c) 2021 Hapis Lab. All rights reserved.
//

#pragma once

#include <algorithm>
#include <memory>
#include <utility>
#include <vector>

#if _MSC_VER
#pragma warning(push)
#pragma warning(disable : 6031 6255 6294 26450 26451 26454 26495 26812)
#endif
#if defined(__GNUC__) && !defined(__llvm__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
#include <Eigen/Dense>
#if _MSC_VER
#pragma warning(pop)
#endif
#if defined(__GNUC__) && !defined(__llvm__)
#pragma GCC diagnostic pop
#endif

#include "autd3/core/hardware_defined.hpp"
#include "autd3/core/utils.hpp"
#include "autd3/gain/matrix.hpp"
#include "autd3/utils.hpp"

namespace autd::gain::holo {

template <typename T>
struct EigenMatrix {
  Eigen::Matrix<T, -1, -1, Eigen::ColMajor> data;

  explicit EigenMatrix(const size_t row, const size_t col) : data(row, col) {}
  explicit EigenMatrix(Eigen::Matrix<T, -1, -1, Eigen::ColMajor> other) : data(std::move(other)) {}
  virtual ~EigenMatrix() = default;
  EigenMatrix(const EigenMatrix& obj) = delete;
  EigenMatrix& operator=(const EigenMatrix& obj) = delete;
  EigenMatrix(const EigenMatrix&& v) = delete;
  EigenMatrix& operator=(EigenMatrix&& obj) = delete;

  void make_complex(const std::shared_ptr<const EigenMatrix<double>>& r, const std::shared_ptr<const EigenMatrix<double>>& i);
  void exp() { data = data.array().exp(); }
  void scale(const T s) { data *= s; }
  void reciprocal(const std::shared_ptr<const EigenMatrix<T>>& src) {
    data = Eigen::Matrix<T, -1, -1, Eigen::ColMajor>::Ones(src->data.rows(), src->data.cols()).cwiseQuotient(src->data);
  }
  void abs(const std::shared_ptr<const EigenMatrix<T>>& src) { data = src->data.cwiseAbs(); }
  void real(const std::shared_ptr<const EigenMatrix<complex>>& src);
  void arg(const std::shared_ptr<const EigenMatrix<complex>>& src);
  void hadamard_product(const std::shared_ptr<const EigenMatrix<T>>& a, const std::shared_ptr<const EigenMatrix<T>>& b) {
    data.noalias() = a->data.cwiseProduct(b->data);
  }
  virtual void pseudo_inverse_svd(const std::shared_ptr<EigenMatrix<T>>& matrix, double alpha, const std::shared_ptr<EigenMatrix<T>>&,
                                  const std::shared_ptr<EigenMatrix<T>>& s, const std::shared_ptr<EigenMatrix<T>>&,
                                  const std::shared_ptr<EigenMatrix<T>>&) {
    const Eigen::BDCSVD svd(matrix->data, Eigen::ComputeFullU | Eigen::ComputeFullV);
    s->data.fill(0);
    auto singular_values = svd.singularValues();
    const auto size = singular_values.size();
    for (Eigen::Index i = 0; i < size; i++) s->data(i, i) = singular_values(i) / (singular_values(i) * singular_values(i) + alpha);
    data.noalias() = svd.matrixV() * s->data * svd.matrixU().adjoint();
  }
  virtual void max_eigen_vector(const std::shared_ptr<EigenMatrix<T>>& ev);
  virtual void add(const T alpha, const std::shared_ptr<EigenMatrix<T>>& a) { data.noalias() += alpha * a->data; }
  virtual void mul(const TRANSPOSE trans_a, const TRANSPOSE trans_b, const T alpha, const std::shared_ptr<const EigenMatrix<T>>& a,
                   const std::shared_ptr<const EigenMatrix<T>>& b, const T beta) {
    data *= beta;
    switch (trans_a) {
      case TRANSPOSE::CONJ_TRANS:
        switch (trans_b) {
          case TRANSPOSE::CONJ_TRANS:
            data.noalias() += alpha * (a->data.adjoint() * b->data.adjoint());
            break;
          case TRANSPOSE::TRANS:
            data.noalias() += alpha * (a->data.adjoint() * b->data.transpose());
            break;
          case TRANSPOSE::NO_TRANS:
            data.noalias() += alpha * (a->data.adjoint() * b->data);
            break;
        }
        break;
      case TRANSPOSE::TRANS:
        switch (trans_b) {
          case TRANSPOSE::CONJ_TRANS:
            data.noalias() += alpha * (a->data.transpose() * b->data.adjoint());
            break;
          case TRANSPOSE::TRANS:
            data.noalias() += alpha * (a->data.transpose() * b->data.transpose());
            break;
          case TRANSPOSE::NO_TRANS:
            data.noalias() += alpha * (a->data.transpose() * b->data);
            break;
        }
        break;
      case TRANSPOSE::NO_TRANS:
        switch (trans_b) {
          case TRANSPOSE::CONJ_TRANS:
            data.noalias() += alpha * (a->data * b->data.adjoint());
            break;
          case TRANSPOSE::TRANS:
            data.noalias() += alpha * (a->data * b->data.transpose());
            break;
          case TRANSPOSE::NO_TRANS:
            data.noalias() += alpha * (a->data * b->data);
            break;
        }
        break;
    }
  }
  virtual void solve(const std::shared_ptr<EigenMatrix<T>>& b) {
    const Eigen::LLT<Eigen::Matrix<T, -1, -1, Eigen::ColMajor>> llt(data);
    llt.solveInPlace(b->data);
  }
  virtual T dot(const std::shared_ptr<const EigenMatrix<T>>& a) { return (data.adjoint() * a->data)(0); }
  [[nodiscard]] virtual double max_element() const;
  void concat_row(const std::shared_ptr<const EigenMatrix<T>>& a, const std::shared_ptr<const EigenMatrix<T>>& b) { data << a->data, b->data; }
  void concat_col(const std::shared_ptr<const EigenMatrix<T>>& a, const std::shared_ptr<const EigenMatrix<T>>& b) { data << a->data, b->data; }

  [[nodiscard]] T at(const size_t row, const size_t col) const { return data(row, col); }
  [[nodiscard]] size_t rows() const { return data.rows(); }
  [[nodiscard]] size_t cols() const { return data.cols(); }

  void set(const size_t row, const size_t col, T v) { data(row, col) = v; }
  void get_col(const std::shared_ptr<const EigenMatrix<T>>& src, const size_t i) {
    const auto& col = src->data.col(i);
    std::memcpy(data.data(), col.data(), sizeof(T) * col.size());
  }
  void fill(T v) { data.fill(v); }
  void get_diagonal(const std::shared_ptr<const EigenMatrix<T>>& src) {
    for (Eigen::Index i = 0; i < (std::min)(src->data.rows(), src->data.cols()); i++) data(i, 0) = src->data(i, i);
  }
  void create_diagonal(const std::shared_ptr<const EigenMatrix<T>>& v) {
    fill(0.0);
    data.diagonal() = v->data;
  }
  virtual void copy_from(const std::shared_ptr<const EigenMatrix<T>>& a) { data = a->data; }
  virtual void copy_from(const std::vector<T>& v) { std::memcpy(this->data.data(), v.data(), sizeof(T) * v.size()); }
  virtual void copy_from(const T* v) { std::memcpy(this->data.data(), v, sizeof(T) * this->data.size()); }

  // FIXME: following functions are too specialized
  void transfer_matrix(const double* foci, size_t foci_num, const std::vector<const double*>& positions, const std::vector<const double*>& directions,
                       double wavelength, double attenuation);
  void set_bcd_result(const std::shared_ptr<const EigenMatrix<T>>& vec, size_t index);
  void set_from_complex_drive(std::vector<core::DataArray>& dst, bool normalize, double max_coefficient);
  void set_from_arg(std::vector<core::DataArray>& dst, size_t n);
  void back_prop(const std::shared_ptr<const EigenMatrix<T>>& transfer, const std::shared_ptr<const EigenMatrix<T>>& amps);
  void sigma_regularization(const std::shared_ptr<const EigenMatrix<T>>& transfer, const std::shared_ptr<const EigenMatrix<T>>& amps, double gamma);
  void col_sum_imag(const std::shared_ptr<EigenMatrix<complex>>& src);
};

template <>
inline void EigenMatrix<complex>::make_complex(const std::shared_ptr<const EigenMatrix<double>>& r,
                                               const std::shared_ptr<const EigenMatrix<double>>& i) {
  data.real() = r->data;
  data.imag() = i->data;
}
template <>
inline void EigenMatrix<double>::real(const std::shared_ptr<const EigenMatrix<complex>>& src) {
  data = src->data.real();
}
template <>
inline void EigenMatrix<complex>::arg(const std::shared_ptr<const EigenMatrix<complex>>& src) {
  data = src->data.cwiseQuotient(src->data.cwiseAbs());
}
template <>
inline double EigenMatrix<double>::max_element() const {
  return this->data.maxCoeff();
}
template <>
inline double EigenMatrix<complex>::max_element() const {
  return std::sqrt(this->data.cwiseAbs2().maxCoeff());
}
template <>
inline void EigenMatrix<double>::max_eigen_vector(const std::shared_ptr<EigenMatrix<double>>&) {}
template <>
inline void EigenMatrix<complex>::max_eigen_vector(const std::shared_ptr<EigenMatrix<complex>>& ev) {
  const Eigen::ComplexEigenSolver<Eigen::Matrix<complex, -1, -1, Eigen::ColMajor>> ces(data);
  auto idx = 0;
  ces.eigenvalues().cwiseAbs2().maxCoeff(&idx);
  const Eigen::Matrix<complex, -1, 1, Eigen::ColMajor>& max_ev = ces.eigenvectors().col(idx);
  ev->copy_from(max_ev.data());
}

template <>
inline void EigenMatrix<complex>::transfer_matrix(const double* foci, const size_t foci_num, const std::vector<const double*>& positions,
                                                  const std::vector<const double*>& directions, const double wavelength, const double attenuation) {
  const auto m = static_cast<Eigen::Index>(foci_num);

  const auto wave_number = 2.0 * M_PI / wavelength;
  for (Eigen::Index i = 0; i < m; i++) {
    const auto tp = core::Vector3(foci[3 * i], foci[3 * i + 1], foci[3 * i + 2]);
    Eigen::Index k = 0;
    for (size_t dev = 0; dev < positions.size(); dev++) {
      const double* p = positions[dev];
      const auto dir = core::Vector3(directions[dev][0], directions[dev][1], directions[dev][2]);
      for (Eigen::Index j = 0; j < static_cast<Eigen::Index>(core::NUM_TRANS_IN_UNIT); j++, k++) {
        const auto pos = core::Vector3(p[3 * j], p[3 * j + 1], p[3 * j + 2]);
        data(i, k) = utils::transfer(pos, dir, tp, wave_number, attenuation);
      }
    }
  }
}

template <>
inline void EigenMatrix<complex>::set_bcd_result(const std::shared_ptr<const EigenMatrix<complex>>& vec, const size_t index) {
  const auto m = vec->data.size();
  const auto idx = static_cast<Eigen::Index>(index);
  for (Eigen::Index i = 0; i < idx; i++) data(idx, i) = std::conj(vec->data(i, 0));
  for (Eigen::Index i = idx + 1; i < m; i++) data(idx, i) = std::conj(vec->data(i, 0));
  for (Eigen::Index i = 0; i < idx; i++) data(i, idx) = vec->data(i, 0);
  for (Eigen::Index i = idx + 1; i < m; i++) data(i, idx) = vec->data(i, 0);
}

template <>
inline void EigenMatrix<complex>::set_from_complex_drive(std::vector<core::DataArray>& dst, const bool normalize, const double max_coefficient) {
  const Eigen::Index n = data.size();
  size_t dev_idx = 0;
  size_t trans_idx = 0;
  for (Eigen::Index j = 0; j < n; j++) {
    const auto f_amp = normalize ? 1.0 : std::abs(data(j, 0)) / max_coefficient;
    const auto phase = core::Utilities::to_phase(std::arg(data(j, 0)));
    const auto duty = core::Utilities::to_duty(f_amp);
    dst[dev_idx][trans_idx++] = core::Utilities::pack_to_u16(duty, phase);
    if (trans_idx == core::NUM_TRANS_IN_UNIT) {
      dev_idx++;
      trans_idx = 0;
    }
  }
}

template <>
inline void EigenMatrix<double>::set_from_arg(std::vector<core::DataArray>& dst, const size_t n) {
  size_t dev_idx = 0;
  size_t trans_idx = 0;
  for (Eigen::Index j = 0; j < static_cast<Eigen::Index>(n); j++) {
    constexpr uint8_t duty = 0xFF;
    const auto phase = core::Utilities::to_phase(data(j, 0));
    dst[dev_idx][trans_idx++] = core::Utilities::pack_to_u16(duty, phase);
    if (trans_idx == core::NUM_TRANS_IN_UNIT) {
      dev_idx++;
      trans_idx = 0;
    }
  }
}

template <>
inline void EigenMatrix<complex>::back_prop(const std::shared_ptr<const EigenMatrix<complex>>& transfer,
                                            const std::shared_ptr<const EigenMatrix<complex>>& amps) {
  const auto m = transfer->data.rows();
  const auto n = transfer->data.cols();

  Eigen::Matrix<double, -1, 1, Eigen::ColMajor> denominator(m);
  for (Eigen::Index i = 0; i < m; i++) {
    auto tmp = 0.0;
    for (Eigen::Index j = 0; j < n; j++) tmp += std::abs(transfer->data(i, j));
    denominator(i) = tmp;
  }

  for (Eigen::Index i = 0; i < m; i++) {
    auto c = amps->data(i) / denominator(i);
    for (Eigen::Index j = 0; j < n; j++) data(j, i) = c * std::conj(transfer->data(i, j));
  }
}

template <>
inline void EigenMatrix<complex>::sigma_regularization(const std::shared_ptr<const EigenMatrix<complex>>& transfer,
                                                       const std::shared_ptr<const EigenMatrix<complex>>& amps, const double gamma) {
  const auto m = transfer->data.rows();
  const auto n = transfer->data.cols();

  fill(ZERO);
  for (Eigen::Index j = 0; j < n; j++) {
    double tmp = 0;
    for (Eigen::Index i = 0; i < m; i++) tmp += std::abs(transfer->data(i, j) * amps->data(i));
    data(j, j) = complex(std::pow(std::sqrt(tmp / static_cast<double>(m)), gamma), 0.0);
  }
}

template <>
inline void EigenMatrix<double>::col_sum_imag(const std::shared_ptr<EigenMatrix<complex>>& src) {
  const auto n = data.size();
  for (Eigen::Index i = 0; i < n; i++) {
    double tmp = 0;
    for (Eigen::Index k = 0; k < n; k++) tmp += src->data(i, k).imag();
    data(i, 0) = tmp;
  }
}
}  // namespace autd::gain::holo


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * plane.cpp
 *
 *  Created on: Feb 1, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/plane.hpp"
#include <iostream>
#include <Eigen/SVD>
#include <Eigen/LU>

using namespace mrob;

Plane::Plane(uint_t timeLength):
        timeLength_(timeLength), isPlaneEstimated_(false)
{
    allPlanePoints_.reserve(timeLength_);
    // there should be an individual reservation of points
    for (uint_t i = 0; i < timeLength_; ++i)
    {
        allPlanePoints_.push_back(std::vector<Mat31>());
        // estimated number of points per observation TODO
        allPlanePoints_[i].reserve(512);
    }
    matrixS_.reserve(timeLength_);
    matrixQ_.reserve(timeLength_);

    //Initialize matrices for calculating Hessian
    gradQ_.reserve(6);// one matrix for each dimension
    lieGenerativeMatrices_.reserve(6);

    //TODO create all generative matrices
    Mat4 G;
    G << 0, 0, 0, 0,
         0, 0, -1, 0,
         0, 1, 0, 0,
         0, 0, 0, 0;
    lieGenerativeMatrices_.push_back(G);
    G << 0, 0, 1, 0,
         0, 0, 0, 0,
         -1, 0, 0, 0,
         0, 0, 0, 0;
    lieGenerativeMatrices_.push_back(G);
    G << 0, -1, 0, 0,
         1, 0, 0, 0,
         0, 0, 0, 0,
         0, 0, 0, 0;
    lieGenerativeMatrices_.push_back(G);
    G << 0, 0, 0, 1,
         0, 0, 0, 0,
         0, 0, 0, 0,
         0, 0, 0, 0;
    lieGenerativeMatrices_.push_back(G);
    G << 0, 0, 0, 0,
         0, 0, 0, 1,
         0, 0, 0, 0,
         0, 0, 0, 0;
    lieGenerativeMatrices_.push_back(G);
    G << 0, 0, 0, 0,
         0, 0, 0, 0,
         0, 0, 0, 1,
         0, 0, 0, 0;
    lieGenerativeMatrices_.push_back(G);
}

Plane::~Plane()
{
}

void Plane::reserve(uint_t d, uint_t t)
{
    if (t < timeLength_)
        allPlanePoints_[t].reserve(d);
}

void Plane::push_back_point(Mat31 &point, uint_t t)
{
    // XXX for now it only works on consecutive observations TODO
    if (t < timeLength_)
    {
        //homogeneousPoint << point, 1.0;
        //allPlanePoints_[t].push_back(homogeneousPoint);
        allPlanePoints_[t].push_back(point);
    }
}

std::vector<Mat31>& Plane::get_points(uint_t t)
{
    if (t >= timeLength_)
        return allPlanePoints_.back();
    return allPlanePoints_[t];
}

void Plane::clear_points()
{
    allPlanePoints_.clear();
}

double Plane::estimate_plane()
{
    calculate_all_matrices_S();
    calculate_all_matrices_Q();
    accumulatedQ_ = Mat4::Zero();
    for (Mat4 &Qi: matrixQ_)
    {
        accumulatedQ_ += Qi;
        //std::cout << Qi << std::endl;
    }

    // sum( v * p_i )
    Eigen::JacobiSVD<Mat4> svd(accumulatedQ_, Eigen::ComputeFullU );
    // XXX actually SelfAdjointEigenSolver is faster (https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html)
    planeEstimation_ = svd.matrixU().col(3);
    //std::cout << svd.matrixU() << "\n and solution \n" << planeEstimation_ <<  std::endl;
    //std::cout << "plane estimation error: " << svd.singularValues() <<  std::endl;
    lambda_ = svd.singularValues()(3);

    // new estimation is done, set flags TODO why?
    isPlaneEstimated_ = true;

    return lambda_;
}

double Plane::estimate_plane_incrementally(uint_t t)
{
    accumulatedQ_ -= matrixQ_[t];
    accumulatedQ_ +=  trajectory_->at(t).T() * matrixS_[t] * trajectory_->at(t).T().transpose();
    //std::cout << "new acc Q " << accumulatedQ_ << std::endl;
    Eigen::JacobiSVD<Mat4> svd(accumulatedQ_, Eigen::ComputeFullU );
    planeEstimation_ = svd.matrixU().col(3);
    return svd.singularValues()(3);
}

double Plane::get_error_incremental(uint_t t) const
{
    Mat4 Q = accumulatedQ_;
    //std::cout << "accumulated Q from prev version " << accumulatedQ_ << std::endl;
    Q -= matrixQ_[t];
    //std::cout << "substract: Q " << Q << std::endl;
    Q +=  trajectory_->at(t).T() * matrixS_[t] * trajectory_->at(t).T().transpose();
    //std::cout << "new acc Q " << Q << std::endl;
    Eigen::JacobiSVD<Mat4> svd(Q, Eigen::ComputeFullU );
    return svd.singularValues()(3);
}

void Plane::calculate_all_matrices_S(bool reset)
{
    if (reset)
        matrixS_.clear();
    if (matrixS_.empty())
    {
        numberPoints_ = 0;
        for (uint_t t = 0; t < timeLength_; ++t)
        {
            Mat4 S = Mat4::Zero();
            for ( Mat31 &p : allPlanePoints_[t])
            {
                Mat41 pHomog;
                pHomog << p , 1.0;
                S += pHomog * pHomog.transpose();
            }
            matrixS_.push_back(S);
            ++numberPoints_;
        }
    }
}

Mat31 Plane::get_mean_point(uint_t t)
{
    assert(!matrixS_.empty() && "Plane::get_mean_point: S matrix empty");
    return matrixS_[t].topRightCorner<3,1>()/matrixS_[t](3,3);
}

void Plane::calculate_all_matrices_Q()
{
    matrixQ_.clear();
    for (uint_t t = 0; t < timeLength_; ++t)
    {
        Mat4 Q;
        Q.noalias() = trajectory_->at(t).T() * matrixS_[t] * trajectory_->at(t).T().transpose();
        matrixQ_.push_back(Q);
    }
}

Mat61 Plane::calculate_gradient(uint_t t)
{
    Mat61 jacobian;
    gradQ_.clear();//used for Hessian, we bookeep all calculated gradients

    // calculate dQ/dxi for the submatrix S
    Mat4 dQ, &Q = matrixQ_[t];

    // dQ / d xi(0) = [0
    //               -q3
    //                q2
    //                 0]
    dQ.setZero();
    dQ.row(1) << -Q.row(2);
    dQ.row(2) <<  Q.row(1);
    dQ += dQ.transpose().eval();
    gradQ_.push_back(dQ);
    jacobian(0) = planeEstimation_.dot(dQ*planeEstimation_);

    // dQ / d xi(1) = [q3
    //                 0
    //                -q1
    //                 0]
    dQ.setZero();
    dQ.row(0) <<  Q.row(2);
    dQ.row(2) << -Q.row(0);
    dQ += dQ.transpose().eval();
    gradQ_.push_back(dQ);
    jacobian(1) = planeEstimation_.dot(dQ*planeEstimation_);

    // dQ / d xi(2) = [-q2
    //                 q1
    //                 0
    //                 0]
    dQ.setZero();
    dQ.row(0) << -Q.row(1);
    dQ.row(1) <<  Q.row(0);
    dQ += dQ.transpose().eval();
    gradQ_.push_back(dQ);
    jacobian(2) = planeEstimation_.dot(dQ*planeEstimation_);

    // dQ / d xi(3) = [q4
    //                 0
    //                 0
    //                 0]
    dQ.setZero();
    dQ.row(0) << Q.row(3);
    dQ += dQ.transpose().eval();
    gradQ_.push_back(dQ);
    jacobian(3) = planeEstimation_.dot(dQ*planeEstimation_);

    // dQ / d xi(4) = [0
    //                 q4
    //                 0
    //                 0]
    dQ.setZero();
    dQ.row(1) << Q.row(3);
    dQ += dQ.transpose().eval();
    gradQ_.push_back(dQ);
    jacobian(4) = planeEstimation_.dot(dQ*planeEstimation_);

    // dQ / d xi(5) = [0
    //                 0
    //                 q4
    //                 0]
    dQ.setZero();
    dQ.row(2) << Q.row(3);
    dQ += dQ.transpose().eval();
    gradQ_.push_back(dQ);
    jacobian(5) = planeEstimation_.dot(dQ*planeEstimation_);

    return jacobian;
}

Mat6 Plane::calculate_hessian(uint_t t)
{
    Mat6 hessian = Mat6::Zero();
    Mat4 ddQ, &Q = matrixQ_[t];

    // H = pi' * dd Q * pi, where dd Q = Bij + Bij' and
    // Bij = (Gi*Gj + Gj*Gi)Q*0.5 + Gi * dQ (previous gradient)
    // Upper triangular matrix
    for (uint_t i =0 ; i< 6 ; ++i)
    {
        for (uint_t j = i ; j< 6 ; ++j)
        {
            ddQ.setZero();
            ddQ = lieGenerativeMatrices_[i]*lieGenerativeMatrices_[j] + lieGenerativeMatrices_[j]*lieGenerativeMatrices_[i];
            ddQ *= 0.5 * Q;
            ddQ += lieGenerativeMatrices_[i] * gradQ_[j];
            ddQ += ddQ.transpose().eval();
            hessian(i,j) = planeEstimation_.dot(ddQ*planeEstimation_);
        }
    }
    return hessian;
}

void Plane::print() const
{
    for (uint_t t = 0; t <  timeLength_; ++t)
    {
        std::cout << "Plane time = " << t << std::endl;
        for (Mat31 p : allPlanePoints_[t])
            std::cout << p(0) << ", " << p(1) << ", " << p(2) << std::endl;
    }
}

void Plane::reset()
{
    matrixS_.clear();
    matrixQ_.clear();
    accumulatedQ_ = Mat4::Zero();
    planeEstimation_ = Mat41::Zero();
}


#include <iostream>
#include <string>
#include <armadillo>
#include <stdio.h>
#include <stdlib.h>
#include "CanonicalStructure.cpp"
#include "dataFormer.cpp"
#include "GDMP.cpp"
#include "loadData.cpp"
#include "loadTime.cpp"
#include "LowPassFilter.hpp"
#include "readQ.cpp"
#include <ros/ros.h>
#include "diff.cpp"
#include <thread>
#include <autharl_core>
#include <lwr_robot/robot.h>
#include "filters_smooth.cpp"

using namespace std;
using namespace arma;

/* This code is based on DMPs - General DMP - easy to reverse it
            Despina - Ekaterini Argiropoulos
                  June 2020
            C++ Develop for Kuka Implementaion
*/

int main(int argc, char** argv)
{

  /* statics */
  static string kind = "discrete";
  static int DIM = 7;
  static double EXTRA_TIME = 0; //sec
  static double MAIN_TIME;

  /* Count time duration of program */
  struct timeval startwtime, endwtime;
  double seq_time;
  time_t ttime = time(0);
  char* dt = ctime(&ttime);

  /* Start time*/
  gettimeofday (&startwtime, NULL);

  /* Logs */
  ofstream infoFile, warnFile, errorFile, dataFile;
  infoFile.open("Logs/info.log");
  warnFile.open("Logs/warn.log");
  errorFile.open("Logs/error.log");
  dataFile.open("Logs/data.log");

  infoFile << dt << endl;

  infoFile <<"Load Data: START" << endl;
  mat y_desired = readQ();
  infoFile <<"Load Data: DONE" << endl;

  // ofstream myFile;
  // std::ostringstream oss, ossd, ossdd;
  //
  // oss << "CHECK/yreadQ.txt";
  //
  // myFile.open((oss.str()).c_str());
  // myFile<<y_desired<<endl;
  // myFile.close();

  int DataSize = y_desired.n_cols;
  dataFile<<kind<<endl;
  dataFile<<DIM<<"x"<<DataSize<<endl;

  /* Set goal */
  vec goal(DIM);
  goal = y_desired.col(DataSize-1);

  /* Initialize the ROS node */
  ros::init(argc, argv, "get_desired_trajectory");
  ros::NodeHandle n;
  auto model = std::make_shared<arl::robot::ROSModel>();
  auto robot = std::make_shared<arl::robot::RobotSim>(model, 1e-3);

  // auto robot = std::make_shared<arl::lwr::Robot>(model);
  auto rviz = std::make_shared<arl::viz::RosStatePublisher>(robot);
  std::thread rviz_thread(&arl::viz::RosStatePublisher::run, rviz);

  /* Initialize joints */
  vec initial_config = y_desired.col(0); //{1.6968 ,   0.5681,   -1.1651 ,  -1.6293  ,  0.3011  ,  1.1462 ,   1.7304};

  robot->setMode(arl::robot::Mode::POSITION_CONTROL);
  robot->setJointTrajectory(initial_config, 10);

  /* Load Time*/
  double DTS = robot->cycle;
  vec timed(DataSize); // = loadTime(DataSize);

  infoFile <<"Load Time: START" << endl;
  for (int l = 0; l<DataSize; l++)
    timed(l) = l*DTS;
  infoFile <<"Load Time: DONE" << endl;

  /* Set time vars*/
  double ts = timed(3) - timed(2);
  MAIN_TIME = timed(DataSize-1);

  /*  diddMat to get velocity n' accel */
  mat dy_desired = diffMat(DataSize, DIM, ts, y_desired);
  mat ddy_desired = diffMat(DataSize, DIM, ts, dy_desired);

   /* Generate Canonical Structure */
  CanonicalStructure cs(DataSize);
  cs.generateFx(kind, timed);

  infoFile <<"Canonical Structure : DONE" << endl;

  /*  Init DPM  */
  GDMP dmp[DIM];
  infoFile <<"Initialize DMPs: DONE" << endl;

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

  /*  Training  */
  infoFile <<"Training of DMPs: START" << endl;

  for (int d = 0; d < DIM; d++)
  {
    dmp[d].id = d;
    dmp[d].training(y_desired.row(d),dy_desired.row(d),ddy_desired.row(d),timed);
    dataFile<<"id:"<<d<<'\t'<<"BFs = "<<dmp[d].BFs<<endl;
  }

  infoFile <<"Training of DMPs: DONE" << endl;

//////////////////////////////////////////////////////////////////////////////
                  ////////////SOLUTION//////////

  /*  Forward Solution  */
  static double TOTAL_DURATION = MAIN_TIME + EXTRA_TIME;
  int extra_samples = ceil(EXTRA_TIME/ts);

  infoFile <<"Solution Start " << endl;
  infoFile <<"Total Duration = "<<TOTAL_DURATION<< endl;
  infoFile <<"Extra Time = " <<EXTRA_TIME<< endl;

  int i  = 0;
  double t = 0;
  mat f_prev(3,DIM);

  Filter fsm[DIM](1);
  for (int d = 0; d < DIM-1; d++)
  {
    int window = round(2*dmp[d].tNyq/dmp[d].dt);
    fsm[d].setNcoeffs(window);
  }
  /*init_solution for each dmp*/
  infoFile <<"InitSolution of DMPs: START" << endl;
  for (int d = 0; d < DIM; d++)
  {
    f_prev.col(d) = dmp[d].init_solution_dt(goal[d], cs, MAIN_TIME,  extra_samples, t, i);
    dmp[d].y_smoothed.set_size(size(dmp[d].y));
    dmp[d].y_smoothed(0)= dmp[d].y(0);
  }
  infoFile <<"InitSolution of DMPs: DONE" << endl;

   // vecs to help for set n' get position
   vec commanded_pos(7), measured_pos(7);
   commanded_pos = initial_config;
   measured_pos = initial_config;

   // open file to store messures
   ofstream messuresFile, timeFile;

   messuresFile.open("Messures/robotJointPositions_so.txt");
   timeFile.open("Messures/robotJointTime_so.txt");

   measured_pos = robot->getJointPosition().toArma();
   messuresFile<<measured_pos<<endl;
   timeFile<<t<<endl;

   // Run Solution for each t
  infoFile <<"Solution of DMPs: START" << endl;
  for( t = ts ; t < (MAIN_TIME + EXTRA_TIME+ ts); t = t + ts)
  {
    // get and store robot messures
     measured_pos = robot->getJointPosition().toArma();

     messuresFile<<measured_pos<<endl;
     timeFile<<t<<endl;

     i++;

     // call for each DIM = 7 based on Quat
     for (int d = 0; d < DIM; d++)
     {
       f_prev.col(d) = dmp[d].run_solution_dt(t, goal[d], ts,  i, f_prev(0,d), f_prev(1,d), f_prev(2,d));
       dmp[d].y_smoothed(i) = fsm[d].filterOnline(dmp[d].y, i);
       commanded_pos(d) = dmp[d].y_smoothed(i);
     }
     // // set new position
     robot->setJointPosition(commanded_pos);
     //
     // // wait for next robot cycle
     robot->waitNextCycle();
  }
  infoFile <<"Solution of DMPs: DONE" << endl;

  // close file stored messures from robot
  messuresFile.close();
  timeFile.close();

  cout<<"FORWARD SOLUTION DONE"<<endl;

  // /* Initialize joints */
  // vec Rinitial_config = y_desired.col(DataSize-1);
  //
  // robot->setMode(arl::robot::Mode::POSITION_CONTROL);
  // robot->setJointTrajectory(Rinitial_config, 10);
///////////////////////////////////////////
  // char c = 'e';
  // cout<<"before gotit"<<endl;
  //
  // if ('R' == getchar()) // 27 is for ESC
  // {
  //   cout<<"gotit"<<endl;
  // }
///////////////////////////////////////////

  /* Reverse Solution*/
  i  = 0;
  t = 0;
  mat Rf_prev(3,DIM);

  /*init_Rsolution for each dmp*/
  infoFile <<"RInitSolution of DMPs: START" << endl;

  for (int d = 0; d < DIM; d++)
  {
    Rf_prev.col(d) = dmp[d].init_Rsolution_dt(goal[d], cs, MAIN_TIME,  extra_samples, t, i);
    dmp[d].Ry_smoothed.set_size(size(dmp[d].y));
    dmp[d].Ry_smoothed(0)= dmp[d].Ry(0);
  }
  infoFile <<"InitSolution of DMPs: DONE" << endl;

   // vecs to help for set n' get position
   vec Rcommanded_pos(7), Rmeasured_pos(7);
   Rcommanded_pos.zeros();
   Rmeasured_pos.zeros();

   // open file to store messures
   ofstream RmessuresFile, RtimeFile;

   RmessuresFile.open("Messures/RrobotJointPositions_so.txt");
   RtimeFile.open("Messures/RrobotJointTime_so.txt");

   Rmeasured_pos = robot->getJointPosition().toArma();
   RmessuresFile<<Rmeasured_pos<<endl;
   RtimeFile<<t<<endl;

   // Run RSolution for each t
  infoFile <<"RSolution of DMPs: START" << endl;

  for( t = ts ; t < (MAIN_TIME + EXTRA_TIME + ts); t = t + ts)
  {
     // get and store robot messures
     Rmeasured_pos = robot->getJointPosition().toArma();

     RmessuresFile<<Rmeasured_pos<<endl;
     RtimeFile<<t<<endl;

     i++;

     for (int d = 0; d < DIM; d++)
     {
       Rf_prev.col(d) = dmp[d].run_Rsolution_dt(t, goal[d],  ts, i, Rf_prev(0,d), Rf_prev(1,d), Rf_prev(2,d));
       dmp[d].Ry_smoothed(i) = fsm[d].filterOnline(dmp[d].Ry, i);
       Rcommanded_pos(d) = dmp[d].Ry_smoothed(i);
     }

    // set new position
    robot->setJointPosition(Rcommanded_pos);

    // wait for next robot cycle
    robot->waitNextCycle();
  }
  infoFile <<"RSolution of DMPs: DONE" << endl;

  // close file stored messures from robot
  RmessuresFile.close();

  cout<<"REVERSE SOLUTION DONE"<<endl;
  //////////////////////////////////////////////////////////////////////////////
                    ////////////RESULTS//////////
cout<<"SAVE RESULTS START"<<endl;

    /* Save results*/
    for (int d = 0; d < DIM; d++)
    {
      ofstream myFile;
      std::ostringstream oss, ossd, ossdd, ossw, ossc, osspsi, oss_sm;

      oss << "CHECK/y" << d <<".log";

      myFile.open((oss.str()).c_str());
      for (int i = 0; i < dmp[d].NSamples; i++)
        myFile<<dmp[d].y(i)<<endl;
      myFile.close();

      ossd << "CHECK/dy" << d <<".log";

      myFile.open((ossd.str()).c_str());
      for (int i = 0; i < dmp[d].NSamples; i++)
        myFile<<dmp[d].dy(i)<<endl;
      myFile.close();

      ossdd << "CHECK/ddy" << d <<".log";

      myFile.open((ossdd.str()).c_str());
      for (int i = 0; i < dmp[d].NSamples; i++)
        myFile<<dmp[d].ddy(i)<<endl;
      myFile.close();

      ossw << "CHECK/w" << d <<".log";

      myFile.open((ossw.str()).c_str());
      for (int i = 0; i < dmp[d].BFs; i++)
        myFile<<dmp[d].w(i)<<endl;
      myFile.close();

      osspsi << "CHECK/psi" << d <<".log";

      myFile.open((osspsi.str()).c_str());
      for (int b = 0; b <dmp[d].BFs; b++)
      {
        for (int i = 0; i < dmp[d].NSamples+dmp[d].extra_train; i++)
          myFile<<dmp[d].psi(b,i)<<'\t';
        myFile<<endl;
      }
      myFile.close();

      ossc << "CHECK/c" << d <<".log";

      myFile.open((ossc.str()).c_str());
      for (int i = 0; i < dmp[d].BFs; i++)
        myFile<<dmp[d].c(i)<<endl;
      myFile.close();

      oss_sm << "CHECK/y_smoothed" << d <<".log";

      myFile.open((oss_sm.str()).c_str());
      for (int i = 0; i < dmp[d].y.n_cols; i++)
        myFile<<dmp[d].y_smoothed(i)<<endl;
      myFile.close();

    }

  //////////////////////////////////////
  /* Save Rresults*/
  for (int d = 0; d < DIM; d++)
  {
    ofstream myFile;
    std::ostringstream oss, ossd, ossdd, oss_sm;

    oss << "CHECK/Ry" << d <<".log";

    myFile.open((oss.str()).c_str());
    for (int i = 0; i < dmp[d].NSamples; i++)
      myFile<<dmp[d].Ry(i)<<endl;
    myFile.close();

    ossd << "CHECK/Rdy" << d <<".log";

    myFile.open((ossd.str()).c_str());
    for (int i = 0; i < dmp[d].NSamples; i++)
      myFile<<dmp[d].Rdy(i)<<endl;
    myFile.close();

    ossdd << "CHECK/Rddy" << d <<".log";

    myFile.open((ossdd.str()).c_str());
    for (int i = 0; i < dmp[d].NSamples; i++)
      myFile<<dmp[d].Rddy(i)<<endl;
    myFile.close();

    oss_sm << "CHECK/Ry_smoothed" << d <<".log";

    myFile.open((oss_sm.str()).c_str());
    for (int i = 0; i < dmp[d].Ry.n_cols; i++)
        myFile<<dmp[d].Ry_smoothed(i)<<endl;
    myFile.close();
  }

  cout<<"SAVE RESULTS DONE"<<endl;

  /* End time*/
  gettimeofday (&endwtime, NULL);
  seq_time = (double)((endwtime.tv_usec - startwtime.tv_usec)/1.0e6 + endwtime.tv_sec - startwtime.tv_sec);
  infoFile<<"GDMP clock time = "<<seq_time<<endl;

  // close Log files
  infoFile.close();
  warnFile.close();
  errorFile.close();
  dataFile.close();

  cout<<"Thank you \n \t Bye Bye"<<endl;

  return 0;

}


/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef _MSC_VER
#   define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>

#include "sql_test_suite_fixture.h"

using namespace ignite;

using namespace boost::unit_test;

BOOST_FIXTURE_TEST_SUITE(SqlValueExpressionTestSuite, ignite::SqlTestSuiteFixture)

BOOST_AUTO_TEST_CASE(TestCase)
{
    TestType in;

    in.i32Field = 82;

    testCache.Put(1, in);

    CheckSingleResult<int32_t>(
        "SELECT "
            "CASE i32Field WHEN 82 "
                "THEN (i32Field / 2) "
                "ELSE (i32Field / 3) "
            "END "
        "FROM TestType", in.i32Field / 2);

    CheckSingleResult<int32_t>(
        "SELECT "
            "CASE i32Field WHEN 22 "
                "THEN (i32Field / 2) "
                "ELSE (i32Field / 3) "
            "END "
        "FROM TestType", in.i32Field / 3);;
}

BOOST_AUTO_TEST_CASE(TestCast)
{
    TestType in;

    in.i32Field = 12345;
    in.strField = "54321";

    testCache.Put(1, in);

    CheckSingleResult<int32_t>("SELECT CAST(strField AS INT) + i32Field FROM TestType",
        common::LexicalCast<int32_t>(in.strField) + in.i32Field);

    CheckSingleResult<std::string>("SELECT CAST(i32Field AS VARCHAR) || strField FROM TestType",
        common::LexicalCast<std::string>(in.i32Field) + in.strField);
}

BOOST_AUTO_TEST_CASE(TestCoalesce)
{
    CheckSingleResult<std::string>("SELECT COALESCE('One', 'Two', 'Three')", "One");
    CheckSingleResult<std::string>("SELECT COALESCE(NULL, 'Two', 'Three')", "Two");
    CheckSingleResult<std::string>("SELECT COALESCE(NULL, 'Two', NULL)", "Two");
    CheckSingleResult<std::string>("SELECT COALESCE(NULL, NULL, 'Three')", "Three");
}

BOOST_AUTO_TEST_CASE(TestNullif)
{
    TestType in;

    in.strField = "SomeValue";

    testCache.Put(1, in);

    CheckSingleResult<std::string>("SELECT NULLIF(strField, 'blablabla') FROM TestType", in.strField);
    CheckSingleResult<std::string>("SELECT NULLIF(strField, 'SomeValue') FROM TestType", "");
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <boost/hana/assert.hpp>
#include <boost/hana/core/is_a.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/value.hpp>
namespace hana = boost::hana;

void function() { }
void function_index(...) { }

int main() {
    // times member function
    {
        int counter = 0;
        hana::int_c<3>.times([&] { ++counter; });
        BOOST_HANA_RUNTIME_CHECK(counter == 3);

        // Call .times with a normal function used to fail.
        hana::int_c<3>.times(function);

        // make sure times can be accessed as a static member function too
        decltype(hana::int_c<5>)::times([]{ });

        // make sure xxx.times can be used as a function object
        auto z = hana::int_c<5>.times;
        (void)z;
    }

    // times.with_index
    {
        int index = 0;
        hana::int_c<3>.times.with_index([&](auto i) {
            static_assert(hana::is_an<hana::integral_constant_tag<int>>(i), "");
            BOOST_HANA_RUNTIME_CHECK(hana::value(i) == index);
            ++index;
        });

        // Calling .times.with_index with a normal function used to fail.
        hana::int_c<3>.times.with_index(function_index);

        // make sure times.with_index can be accessed as a static member
        // function too
        auto times = hana::int_c<5>.times;
        decltype(times)::with_index([](auto) { });

        // make sure xxx.times.with_index can be used as a function object
        auto z = hana::int_c<5>.times.with_index;
        (void)z;

        // make sure we're calling the function in the right order
        int current = 0;
        hana::int_c<5>.times.with_index([&](auto i) {
            BOOST_HANA_RUNTIME_CHECK(hana::value(i) == current);
            ++current;
        });
    }
}


/**
 * @file ImageDeconvolution.hpp
 * @author Jorge Agüero Zamora
 * @brief Contains `ImageDeconvolution` class in charge of applying filters to images
 * @version 0.1
 * @date 2021-06-13
 *
 */
#ifndef IMAGEDECONVOLUTION_H
#define IMAGEDECONVOLUTION_H

#define FILT_IMAGE_SUFFIX "_filtered.png" //!< suffix used in saved filtered image filenames

#include <vector>
#include <string>
#include <armadillo>

#include "FilterInfo.hpp"
#include "VecImage.hpp"
#include "ImageLoader.hpp"

namespace AppLogic
{
    /**
     * @brief Allows to apply existing filter to images
     *
     */
    class ImageDeconvolution
    {
    private:
        vector<FilterInfo> loaded_filters; //!< filter information of existing filters
        vector<VecImage> working_images; //!< list of loaded images
        // vector<VecImage> filtered_images; //!< lisf of filtered images
        vector<string> loaded_file_paths; //!< paths to the loaded image files
        vector<string> loaded_file_names; //!< names of the loaded images
        bool is_canceled; //!< indicates whether image saving process is canceled
        mat F_matrix; //!< Filter matrix

    public:

        /**
         * @brief Construct a new Image Deconvolution object
         *
         */
        ImageDeconvolution();

        /**
         * @brief Destroy the Image Deconvolution object
         *
         */
        ~ImageDeconvolution();

        /**
         * @brief deletes all loaded image information
         *
         */
        inline void clearWorkingImages()
        {
            this->working_images.clear();
            this->loaded_file_paths.clear();
            this->loaded_file_names.clear();
        }

        /**
         * @brief returns the path of the loaded image specified by index
         *
         * @param index the index of the loaded image to get the path for
         * @return string path of the loaded image in the filesystem
         */
        string getImagePath(size_t index);

        /**
         * @brief Get the Loaded Filters object
         *
         * @return vector<FilterInfo> list of FilterInfo objects containing the information of the existing filters
         */
        inline vector<FilterInfo> getLoadedFilters()
        {
            return this->loaded_filters;
        };

        /**
         * @brief returns a new image of the selected image with the filter applied.
         *
         * Aplies the current folder to the image identified by `image_id`
         *
         * @param image_id index of the image to apply the filter to
         * @return VecImage new `VecImage` with the filter applied
         */
        VecImage applyFilterToImage(size_t image_id);

        /**
         * @brief returns a pointer to the loaded `VecImage` specified by `index`
         *
         * @param index the index of the loaded image to get
         * @return VecImage* pointer to the `VecImage` object of the loaded image
         */
        VecImage * getLoadedImage(const size_t index);

        // bool applyFilterToWorkingImages();

        /**
         * @brief load filter matrix from file
         *
         * Looks for the corresponding *.mat file with the same
         * name as the filter identified by the index `id`,
         * and loads the matrix to be used to apply to images.
         *
         * @param id index of the `FilterInfo` in `loaded_filters` with the name of the filter to be loaded
         * @return true if the matrix is loaded correctly
         * @return false if there is an error loading the matrix
         */
        bool loadFilter(int index);

        /**
         * @brief applies filter to image and saves it in the specified path
         *
         * Applies filter to the loaded image specified by the index `image_id`, using the
         * loaded filter and saves it to the folder path provided
         * as a PNG (.png) image. This operation does not modify the loaded image.
         *
         * @param image_id index of the image to apply noise to and save
         * @param folder_path name of the file to save the dirty image
         * @return true if image is saved corectly
         * @return false if there is an error saving the image
         */
        bool saveFilteredImage(size_t image_id, string folder_path);

        /**
         * @brief applies filter to all working images and saves them in the specified path
         *
         * Applies filter to all loaded images using the loded filter and
         * saves them to the folder path provided as PNG (.png) images. This operation does not
         * modify the loaded images.
         *
         * @param folder_path path where the images will be saved
         * @return true if images are saved correctly
         * @return false if an eror is encountered or saving process is canceled
         */
        bool saveAllFilteredImages(string folder_path);

        /**
         * @brief loads images from specified full path to a folder
         *
         * This function will look for the first valid image in the folder and will load
         * all other valid images of the same resolution as that first loaded image, other
         * images are ignored.
         *
         * @param folder_path full path to the directory containing the images
         * @return vector<string> list of names of the images that were loaded
         */
        vector<string> loadImagesFromFolder(string folder_path);

        /**
         * @brief load images from a zip file
         *
         * This function will go through all files in a zip file. It  will look for the first
         * valid image in the folder and will load all other valid images of the same resolution
         * as the first loaded image, other images are ignored.
         *
         * @param file_path full path of a valid zip file
         * @return vector<string> list of names of the images that were loaded
         */
        vector<string> loadImagesFromZip(string file_path);

        /**
         * @brief loads a single image and adds it to the working images
         *
         * @param file_path path of the image file
         * @return string name of the loaded image
         */
        string loadSingleImage(string file_path);

        /**
         * @brief deletes filter specified by `index`
         *
         * Checks the application data folder in order to delete the *.finfo and *.mat files
         * that ath the name of the filter specified by `index` in `filters`.
         *
         * @param index index of the `FilterInfo` that has the name of the filter to delete
         * @return true if the filter is deleted successfully
         * @return false if there is an error deleting the filters
         */
        bool deleteFilter(int index);

        /**
         * @brief cancels the process of saving images with filter applied
         *
         */
        void cancelFilterAllImages();

        /**
         * @brief checks application data directory in order to load existing filter information
         *
         * @return vector<FilterInfo> list of FilterInfo objects for existing filters in filesystem
         */
        vector<FilterInfo> loadExistingFilters();

        /**
         * @brief returns pointer to the FilterInfo object fo current filter.
         *
         * @return FilterInfo* Object containing the calculation information for the curent filter
         */
        FilterInfo *getFilterInfo(int index);

        int getFilterIndex(const string filtername);

         // bool exportFilter(size_t index, const string path_to_file);
        // bool importFilter(const string path_to_file);

    };
}

#endif


#pragma once

#include <boost/numeric/ublas/lu.hpp>
#include <immintrin.h>
#include <omp.h>
#include <tuple>

#include "matrix.hpp"

using namespace std;
namespace ub = boost::numeric::ublas;

void lu_factorize(Matrix &m, omp_sched_t sched_type, size_t chunk_size);
void lu_factorize_sequential(Matrix &m);
void lu_factorize_sequential_vectorized(Matrix &m);
void lu_factorize_parallel(Matrix &m, omp_sched_t sched_type,
                           size_t chunk_size);
void lu_factorize_parallel_vectorized(Matrix &m, omp_sched_t sched_type,
                           size_t chunk_size);
bool check_lu_correctness(ub::matrix<double> input, Matrix lu);


#pragma once

#include "mona/camera.hpp"
#include "mona/targets/target.hpp"
#include "mona/internal/shader.hpp"
#include "mona/resource.hpp"

#include <armadillo>
#include <glm/vec3.hpp>

#include <vector>

namespace mona
{
    class surface_mesh: public resource
    {
        mutable shader s;
        unsigned int vao = 0;
        unsigned int vbo = 0;
        unsigned int ibo = 0;

    public:

        std::vector<glm::vec3> vertices;
        std::vector<glm::vec3> normals;
        std::vector<glm::uvec3> indices;

        surface_mesh(const arma::fmat& x, const arma::fmat& y, const arma::fmat& z);
        ~surface_mesh();

        auto draw(const glm::mat4& mvp) const -> void;
    };
}


// ROS includes.
#include <ros/ros.h>
#include <ros/time.h>

//The services
#include <robo7_srvs/discretize_map.h>

//Input all the libraries needed
#include <tf2/convert.h>
#include <Eigen/Geometry>
#include <math.h>
#include <ros/ros.h>

//The messages
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Float32.h>
#include <robo7_msgs/XY_coordinates.h>
#include <robo7_msgs/cornerList.h>
#include <robo7_msgs/wallList.h>
#include <robo7_msgs/aWall.h>
#include <robo7_msgs/wallPoint.h>

//The extra libraries
#include </usr/include/eigen3/Eigen/Dense>
using Eigen::MatrixXd;

// Control @ 10 Hz
double control_frequency = 20.0;

class discretize_the_map
{
public:
  ros::NodeHandle n;
  ros::Publisher point_coordinates;
  ros::Publisher point_cloud_input;
  ros::ServiceServer scan_service;

  discretize_the_map()
  {
    scan_service = n.advertiseService("/maze_map/map_discretization", &discretize_the_map::discretization_Sequence, this);

    point_coordinates = n.advertise<robo7_msgs::XY_coordinates>("/scan/point_cloud_coordinates", 1);
    point_cloud_input = n.advertise<robo7_msgs::cornerList>("/lidar_map/point_cloud", 1);
  }

  bool discretization_Sequence(robo7_srvs::discretize_map::Request &req,
         robo7_srvs::discretize_map::Response &res)
	{
    wall_list = req.walls;

    //Initialisation
    wall_discretized.number = 0;
    wall_discretized.the_points.clear();

    // ROS_INFO("nb of walls : %d", wall_list.number);

    for(int i = 0; i < wall_list.number; i++)
    {
        x1 = wall_list.walls[i].init_point.x;
        y1 = wall_list.walls[i].init_point.y;
        x2 = wall_list.walls[i].end_point.x;
        y2 = wall_list.walls[i].end_point.y;
        // ROS_INFO("(x_init, y_init) = (%lf, %lf)", x1, y1);
        // ROS_INFO("(x_end, y_end) = (%lf, %lf)", x2, y2);

        //Discretized map
        // ROS_INFO("nb of inliers : %d", wall_list.walls[i].nb_inliers);
        N_step = wall_list.walls[i].nb_inliers;
        x_step = (x2-x1)/N_step;
        y_step = (y2-y1)/N_step;
        aPoint.x = x1;
        aPoint.y = y1;
        aPoint.z = 0;
        for(int i=0; i < N_step + 1; i++)
        {
          aPoint.x = x1 + i*x_step;
          aPoint.y = y1 + i*y_step;
          wall_discretized.the_points.push_back(aPoint);
          wall_discretized.number++;
        }
    }

    res.discretized_walls = wall_discretized;
    res.success = true;
    return true;
  }

private:
  //Input and output of the service
  robo7_msgs::wallList wall_list;
  robo7_msgs::wallPoint wall_discretized;

  geometry_msgs::Vector3 aPoint;
  int N_step;

  float x1, y1, x2, y2;
  float x_step, y_step;
};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "discretize_the_map");

	discretize_the_map discretize_the_map_;

	ros::Rate loop_rate(100);

	ROS_INFO("Discretization of the map running");

	ros::spin();

	return 0;
}


/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#include <boost/unordered_set.hpp>
#include <graphlab.hpp>
#include <graphlab/ui/metrics_server.hpp>
#include <graphlab/macros_def.hpp>

typedef graphlab::vertex_id_type color_type;

/*
 * no edge data
 */
typedef graphlab::empty edge_data_type;

/*
 * This is the gathering type which accumulates an (unordered) set of
 * all neighboring colors
 * It is a simple wrapper around a boost::unordered_set with
 * an operator+= which simply performs a set union.
 *
 * This struct can be significantly accelerated for small sets.
 * Small collections of vertex IDs should not require the overhead
 * of the unordered_set.
 */
struct set_union_gather {
  boost::unordered_set<color_type> colors;

  /*
   * Combining with another collection of vertices.
   * Union it into the current set.
   */
  set_union_gather& operator+=(const set_union_gather& other) {
    foreach(graphlab::vertex_id_type othervid, other.colors) {
      colors.insert(othervid);
    }
    return *this;
  }

  // serialize
  void save(graphlab::oarchive& oarc) const {
    oarc << colors;
  }

  // deserialize
  void load(graphlab::iarchive& iarc) {
    iarc >> colors;
  }
};

/*
 * Define the type of the graph
 */
typedef graphlab::distributed_graph<color_type,
                                    edge_data_type> graph_type;

/*
 * On gather, we accumulate a set of all adjacent colors.
 */
class graph_coloring:
      public graphlab::ivertex_program<graph_type,
                                      set_union_gather>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }

  /*
   * For each edge, figure out the ID of the "other" vertex
   * and accumulate a set of the neighborhood vertex IDs.
   */
  gather_type gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    set_union_gather gather;
    color_type other_color = edge.source().id() == vertex.id() ?
                                 edge.target().data(): edge.source().data();
    gather.colors.insert(other_color);
    return gather;
  }

  /*
   * the gather result now contains the colors in the neighborhood.
   * pick a different color and store it
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& neighborhood) {
    // find the smallest color not described in the neighborhood
    size_t neighborhoodsize = neighborhood.colors.size();
    for (color_type curcolor = 0; curcolor < neighborhoodsize + 1; ++curcolor) {
      if (neighborhood.colors.count(curcolor) == 0) {
        vertex.data() = curcolor;
        break;
      }
    }
  }

  edge_dir_type scatter_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }

  /*
   * For each edge, count the intersection of the neighborhood of the
   * adjacent vertices. This is the number of triangles this edge is involved
   * in.
   */
  void scatter(icontext_type& context,
              const vertex_type& vertex,
              edge_type& edge) const {
    // both points have different colors!
    if (edge.source().data() == edge.target().data()) {
      context.signal(edge.source().id() == vertex.id() ?
                      edge.target() : edge.source());
    }
  }
};

/*
 * A saver which saves a file where each line is a vid / color pair
 */
struct save_colors{
  std::string save_vertex(graph_type::vertex_type v) {
    return graphlab::tostr(v.id()) + "\t" +
           graphlab::tostr(v.data()) + "\n";
  }
  std::string save_edge(graph_type::edge_type e) {
    return "";
  }
};

int main(int argc, char** argv) {

  //global_logger().set_log_level(LOG_INFO);

  // Initialize control plane using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  dc.cout() << "This program computes a simple graph coloring of a"
            "provided graph.\n\n";

  graphlab::command_line_options clopts("Graph coloring. "
    "Given a graph, this program computes a graph coloring of the graph."
    "The Asynchronous engine is used.");
  std::string prefix, format;
  std::string output;
  size_t powerlaw = 0;
  clopts.attach_option("graph", prefix,
                       "Graph input. reads all graphs matching prefix*");
  clopts.attach_option("format", format,
                       "The graph format");
   clopts.attach_option("output", output,
                       "A prefix to save the output.");
    clopts.attach_option("powerlaw", powerlaw,
                       "Generate a synthetic powerlaw out-degree graph. ");

  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (prefix.length() == 0 && powerlaw == 0) {
    clopts.print_description();
    return EXIT_FAILURE;
  }
  if (output == "") {
    dc.cout() << "Warning! Output will not be saved\n";
  }

  graphlab::launch_metric_server();
  // load graph
  graph_type graph(dc, clopts);

  if(powerlaw > 0) { // make a synthetic graph
    dc.cout() << "Loading synthetic Powerlaw graph." << std::endl;
    graph.load_synthetic_powerlaw(powerlaw, false, 2, 100000000);
  } else { // Load the graph from a file
    if (prefix == "") {
      dc.cout() << "--graph is not optional\n";
      return EXIT_FAILURE;
    }
    else if (format == "") {
      dc.cout() << "--format is not optional\n";
      return EXIT_FAILURE;
    }
    graph.load_format(prefix, format);
  }
  graph.finalize();

  dc.cout() << "Number of vertices: " << graph.num_vertices() << std::endl
    << "Number of edges:    " << graph.num_edges() << std::endl;

  graphlab::timer ti;

  // create engine to count the number of triangles
  dc.cout() << "Coloring..." << std::endl;
  graphlab::async_consistent_engine<graph_coloring> engine(dc, graph, clopts);
  engine.signal_all();
  engine.start();

  dc.cout() << "Colored in " << ti.current_time() << " seconds" << std::endl;
  if (output != "") {
    graph.save(output,
              save_colors(),
              false, /* no compression */
              true, /* save vertex */
              false, /* do not save edge */
              1); /* one file per machine */
  }

  graphlab::stop_metric_server();

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <stdlib.h>
#include <iostream>
#include <stack>
#include <queue>
#include <ctime>
#include <boost/operators.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

typedef std::pair< int, int > Position;
Position knight_jumps[8]
    = { Position(2, -1), Position(1, -2), Position(-1, -2), Position(-2, -1),
          Position(-2, 1), Position(-1, 2), Position(1, 2), Position(2, 1) };

Position operator+(const Position& p1, const Position& p2)
{
    return Position(p1.first + p2.first, p1.second + p2.second);
}

struct knights_tour_graph;
struct knight_adjacency_iterator
: public boost::forward_iterator_helper< knight_adjacency_iterator, Position,
      std::ptrdiff_t, Position*, Position >
{
    knight_adjacency_iterator() {}
    knight_adjacency_iterator(int ii, Position p, const knights_tour_graph& g)
    : m_pos(p), m_g(&g), m_i(ii)
    {
        valid_position();
    }
    Position operator*() const { return m_pos + knight_jumps[m_i]; }
    void operator++()
    {
        ++m_i;
        valid_position();
    }
    bool operator==(const knight_adjacency_iterator& x) const
    {
        return m_i == x.m_i;
    }

protected:
    void valid_position();
    Position m_pos;
    const knights_tour_graph* m_g;
    int m_i;
};

struct knights_tour_graph
{
    typedef Position vertex_descriptor;
    typedef std::pair< vertex_descriptor, vertex_descriptor > edge_descriptor;
    typedef knight_adjacency_iterator adjacency_iterator;
    typedef void out_edge_iterator;
    typedef void in_edge_iterator;
    typedef void edge_iterator;
    typedef void vertex_iterator;
    typedef int degree_size_type;
    typedef int vertices_size_type;
    typedef int edges_size_type;
    typedef directed_tag directed_category;
    typedef disallow_parallel_edge_tag edge_parallel_category;
    typedef adjacency_graph_tag traversal_category;
    knights_tour_graph(int n) : m_board_size(n) {}
    int m_board_size;
};
int num_vertices(const knights_tour_graph& g)
{
    return g.m_board_size * g.m_board_size;
}

void knight_adjacency_iterator::valid_position()
{
    Position new_pos = m_pos + knight_jumps[m_i];
    while (m_i < 8
        && (new_pos.first < 0 || new_pos.second < 0
            || new_pos.first >= m_g->m_board_size
            || new_pos.second >= m_g->m_board_size))
    {
        ++m_i;
        new_pos = m_pos + knight_jumps[m_i];
    }
}

std::pair< knights_tour_graph::adjacency_iterator,
    knights_tour_graph::adjacency_iterator >
adjacent_vertices(
    knights_tour_graph::vertex_descriptor v, const knights_tour_graph& g)
{
    typedef knights_tour_graph::adjacency_iterator Iter;
    return std::make_pair(Iter(0, v, g), Iter(8, v, g));
}

struct compare_first
{
    template < typename P > bool operator()(const P& x, const P& y)
    {
        return x.first < y.first;
    }
};

template < typename Graph, typename TimePropertyMap >
bool backtracking_search(Graph& g,
    typename graph_traits< Graph >::vertex_descriptor src,
    TimePropertyMap time_map)
{
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef std::pair< int, Vertex > P;
    std::stack< P > S;
    int time_stamp = 0;

    S.push(std::make_pair(time_stamp, src));
    while (!S.empty())
    {
        Vertex x;
        boost::tie(time_stamp, x) = S.top();
        put(time_map, x, time_stamp);
        // all vertices have been visited, success!
        if (time_stamp == num_vertices(g) - 1)
            return true;

        bool deadend = true;
        typename graph_traits< Graph >::adjacency_iterator i, end;
        for (boost::tie(i, end) = adjacent_vertices(x, g); i != end; ++i)
            if (get(time_map, *i) == -1)
            {
                S.push(std::make_pair(time_stamp + 1, *i));
                deadend = false;
            }

        if (deadend)
        {
            put(time_map, x, -1);
            S.pop();
            boost::tie(time_stamp, x) = S.top();
            while (get(time_map, x) != -1)
            { // unwind stack to last unexplored vertex
                put(time_map, x, -1);
                S.pop();
                boost::tie(time_stamp, x) = S.top();
            }
        }

    } // while (!S.empty())
    return false;
}

template < typename Vertex, typename Graph, typename TimePropertyMap >
int number_of_successors(Vertex x, Graph& g, TimePropertyMap time_map)
{
    int s_x = 0;
    typename graph_traits< Graph >::adjacency_iterator i, end;
    for (boost::tie(i, end) = adjacent_vertices(x, g); i != end; ++i)
        if (get(time_map, *i) == -1)
            ++s_x;
    return s_x;
}

template < typename Graph, typename TimePropertyMap >
bool warnsdorff(Graph& g, typename graph_traits< Graph >::vertex_descriptor src,
    TimePropertyMap time_map)
{
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef std::pair< int, Vertex > P;
    std::stack< P > S;
    int time_stamp = 0;

    S.push(std::make_pair(time_stamp, src));
    while (!S.empty())
    {
        Vertex x;
        boost::tie(time_stamp, x) = S.top();
        put(time_map, x, time_stamp);
        // all vertices have been visited, success!
        if (time_stamp == num_vertices(g) - 1)
            return true;

        // Put adjacent vertices into a local priority queue
        std::priority_queue< P, std::vector< P >, compare_first > Q;
        typename graph_traits< Graph >::adjacency_iterator i, end;
        int num_succ;
        for (boost::tie(i, end) = adjacent_vertices(x, g); i != end; ++i)
            if (get(time_map, *i) == -1)
            {
                num_succ = number_of_successors(*i, g, time_map);
                Q.push(std::make_pair(num_succ, *i));
            }
        bool deadend = Q.empty();
        // move vertices from local priority queue to the stack
        for (; !Q.empty(); Q.pop())
        {
            boost::tie(num_succ, x) = Q.top();
            S.push(std::make_pair(time_stamp + 1, x));
        }
        if (deadend)
        {
            put(time_map, x, -1);
            S.pop();
            boost::tie(time_stamp, x) = S.top();
            while (get(time_map, x) != -1)
            { // unwind stack to last unexplored vertex
                put(time_map, x, -1);
                S.pop();
                boost::tie(time_stamp, x) = S.top();
            }
        }

    } // while (!S.empty())
    return false;
}

struct board_map
{
    typedef int value_type;
    typedef Position key_type;
    typedef read_write_property_map_tag category;
    board_map(int* b, int n) : m_board(b), m_size(n) {}
    friend int get(const board_map& ba, Position p);
    friend void put(const board_map& ba, Position p, int v);
    friend std::ostream& operator<<(std::ostream& os, const board_map& ba);

private:
    int* m_board;
    int m_size;
};

int get(const board_map& ba, Position p)
{
    return ba.m_board[p.first * ba.m_size + p.second];
}

void put(const board_map& ba, Position p, int v)
{
    ba.m_board[p.first * ba.m_size + p.second] = v;
}

std::ostream& operator<<(std::ostream& os, const board_map& ba)
{
    for (int i = 0; i < ba.m_size; ++i)
    {
        for (int j = 0; j < ba.m_size; ++j)
            os << get(ba, Position(i, j)) << "\t";
        os << std::endl;
    }
    return os;
}

int main(int argc, char* argv[])
{
    int N;
    if (argc == 2)
        N = atoi(argv[1]);
    else
        N = 8;

    knights_tour_graph g(N);
    int* board = new int[num_vertices(g)];
    board_map chessboard(board, N);
    for (int i = 0; i < N; ++i)
        for (int j = 0; j < N; ++j)
            put(chessboard, Position(i, j), -1);

    bool ret = warnsdorff(g, Position(0, 0), chessboard);

    if (ret)
        for (int i = 0; i < N; ++i)
        {
            for (int j = 0; j < N; ++j)
                std::cout << get(chessboard, Position(i, j)) << "\t";
            std::cout << std::endl;
        }
    else
        std::cout << "method failed" << std::endl;
    return 0;
}


//  (c) Copyright Fernando Luis Cacciola Carballal 2000-2004
//  Use, modification, and distribution is subject to the Boost Software
//  License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See library home page at http://www.boost.org/libs/numeric/conversion
//
// Contact the author at: fernando_cacciola@hotmail.com
//
#ifndef BOOST_NUMERIC_CONVERSION_CONVERTER_POLICIES_FLC_12NOV2002_HPP
#define BOOST_NUMERIC_CONVERSION_CONVERTER_POLICIES_FLC_12NOV2002_HPP

#include <typeinfo> // for std::bad_cast

#include <boost/config.hpp>
#include <boost/config/no_tr1/cmath.hpp> // for std::floor and std::ceil
#include <boost/throw_exception.hpp>

#include <functional>

#include "boost/type_traits/is_arithmetic.hpp"

#include "boost/mpl/if.hpp"
#include "boost/mpl/integral_c.hpp"

namespace boost { namespace numeric
{

template<class S>
struct Trunc
{
  typedef S source_type ;

  typedef typename mpl::if_< is_arithmetic<S>,S,S const&>::type argument_type ;

  static source_type nearbyint ( argument_type s )
  {
#if !defined(BOOST_NO_STDC_NAMESPACE)
    using std::floor ;
    using std::ceil  ;
#endif

    return s < static_cast<S>(0) ? ceil(s) : floor(s) ;
  }

  typedef mpl::integral_c< std::float_round_style, std::round_toward_zero> round_style ;
} ;

template<class S>
struct Floor
{
  typedef S source_type ;

  typedef typename mpl::if_< is_arithmetic<S>,S,S const&>::type argument_type ;

  static source_type nearbyint ( argument_type s )
  {
#if !defined(BOOST_NO_STDC_NAMESPACE)
    using std::floor ;
#endif

    return floor(s) ;
  }

  typedef mpl::integral_c< std::float_round_style, std::round_toward_neg_infinity> round_style ;
} ;

template<class S>
struct Ceil
{
  typedef S source_type ;

  typedef typename mpl::if_< is_arithmetic<S>,S,S const&>::type argument_type ;

  static source_type nearbyint ( argument_type s )
  {
#if !defined(BOOST_NO_STDC_NAMESPACE)
    using std::ceil ;
#endif

    return ceil(s) ;
  }

  typedef mpl::integral_c< std::float_round_style, std::round_toward_infinity> round_style ;
} ;

template<class S>
struct RoundEven
{
  typedef S source_type ;

  typedef typename mpl::if_< is_arithmetic<S>,S,S const&>::type argument_type ;

  static source_type nearbyint ( argument_type s )
  {
    // Algorithm contributed by Guillaume Melquiond

#if !defined(BOOST_NO_STDC_NAMESPACE)
    using std::floor ;
    using std::ceil  ;
#endif

    // only works inside the range not at the boundaries
    S prev = floor(s);
    S next = ceil(s);

    S rt = (s - prev) - (next - s); // remainder type

    S const zero(0.0);
    S const two(2.0);

    if ( rt < zero )
      return prev;
    else if ( rt > zero )
      return next;
    else
    {
      bool is_prev_even = two * floor(prev / two) == prev ;
      return ( is_prev_even ? prev : next ) ;
    }
  }

  typedef mpl::integral_c< std::float_round_style, std::round_to_nearest> round_style ;
} ;

enum range_check_result
{
  cInRange     = 0 ,
  cNegOverflow = 1 ,
  cPosOverflow = 2
} ;

class bad_numeric_cast : public std::bad_cast
{
  public:

    virtual const char * what() const BOOST_NOEXCEPT_OR_NOTHROW
      {  return "bad numeric conversion: overflow"; }
};

class negative_overflow : public bad_numeric_cast
{
  public:

    virtual const char * what() const BOOST_NOEXCEPT_OR_NOTHROW
      {  return "bad numeric conversion: negative overflow"; }
};
class positive_overflow : public bad_numeric_cast
{
  public:

    virtual const char * what() const BOOST_NOEXCEPT_OR_NOTHROW
      { return "bad numeric conversion: positive overflow"; }
};

struct def_overflow_handler
{
  void operator() ( range_check_result r ) // throw(negative_overflow,positive_overflow)
  {
#ifndef BOOST_NO_EXCEPTIONS
    if ( r == cNegOverflow )
      throw negative_overflow() ;
    else if ( r == cPosOverflow )
           throw positive_overflow() ;
#else
    if ( r == cNegOverflow )
      ::boost::throw_exception(negative_overflow()) ;
    else if ( r == cPosOverflow )
           ::boost::throw_exception(positive_overflow()) ;
#endif
  }
} ;

struct silent_overflow_handler
{
  void operator() ( range_check_result ) {} // throw()
} ;

template<class Traits>
struct raw_converter
{
  typedef typename Traits::result_type   result_type   ;
  typedef typename Traits::argument_type argument_type ;

  static result_type low_level_convert ( argument_type s ) { return static_cast<result_type>(s) ; }
} ;

struct UseInternalRangeChecker {} ;

} } // namespace boost::numeric

#endif


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/remfix.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" remfix real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::remfix;
  using r_t = decltype(remfix(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_IEEE_EQUAL(remfix(bs::Inf<T>(), bs::Inf<T>()), bs::Nan<T>());
  STF_IEEE_EQUAL(remfix(bs::Minf<T>(), bs::Minf<T>()), bs::Nan<T>());
  STF_IEEE_EQUAL(remfix(bs::Nan<T>(), bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL(remfix(bs::Mone<T>(), bs::Mone<T>()), bs::Zero<T>());
  STF_EQUAL(remfix(bs::One<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(remfix(bs::Zero<T>(), bs::Nan<T>()), bs::Zero<T>());
} // end of test for floating_

STF_CASE_TPL (" remfix unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::remfix;
  using r_t = decltype(remfix(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(remfix(bs::One<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(remfix(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

STF_CASE_TPL (" remfix signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::remfix;
  using r_t = decltype(remfix(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(remfix(bs::Mone<T>(), bs::Mone<T>()), bs::Zero<T>());
  STF_EQUAL(remfix(bs::One<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(remfix(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for signed_int_