/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "binomial-transition.h"

// Local project headers:
#include "hmm/fit/error-model-fitter.h"
#include "hmm/fit/parameter-fitter.h"

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
const double TOL = 0.000000001;
}  // namespace

// BinomialTransition is abstract, so we need to override undefined methods.
class TestableBinomialTransition : public BinomialTransition {
public:
    TestableBinomialTransition(double q, int channel)
            : BinomialTransition(q, channel) {}
    using BinomialTransition::improve_fit;
    virtual void improve_fit(const PeptideStateVector& forward_psv,
                             const PeptideStateVector& backward_psv,
                             const PeptideStateVector& next_backward_psv,
                             int num_edmans,
                             double probability,
                             ErrorModelFitter* fitter) const override;
};

void TestableBinomialTransition::improve_fit(
        const PeptideStateVector& forward_psv,
        const PeptideStateVector& backward_psv,
        const PeptideStateVector& next_backward_psv,
        int num_edmans,
        double probability,
        ErrorModelFitter* fitter) const {}

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(step_suite)
BOOST_AUTO_TEST_SUITE(binomial_transition_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    double q = 0.2;
    int channel = -1;  // can be ignored for this test.
    TestableBinomialTransition bt(q, channel);
    BOOST_TEST(bt.q == q);
    BOOST_TEST(bt.channel == channel);
}

BOOST_AUTO_TEST_CASE(reserve_zero_test, *tolerance(TOL)) {
    double q = 0.2;
    int channel = -1;  // can be ignored for this test.
    TestableBinomialTransition bt(q, channel);
    bt.reserve(0);
    BOOST_TEST(bt.prob(0, 0) == 1.0);
}

BOOST_AUTO_TEST_CASE(reserve_one_test, *tolerance(TOL)) {
    double q = 0.2;
    double p = 0.8;
    int channel = -1;  // can be ignored for this test.
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    BOOST_TEST(bt.prob(0, 0) == 1.0);
    BOOST_TEST(bt.prob(1, 0) == q);
    BOOST_TEST(bt.prob(1, 1) == p);
}

BOOST_AUTO_TEST_CASE(reserve_two_test, *tolerance(TOL)) {
    double q = 0.2;
    double p = 0.8;
    int channel = -1;  // can be ignored for this test.
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    BOOST_TEST(bt.prob(0, 0) == 1.0);
    BOOST_TEST(bt.prob(1, 0) == q);
    BOOST_TEST(bt.prob(1, 1) == p);
    BOOST_TEST(bt.prob(2, 0) == q * q);
    BOOST_TEST(bt.prob(2, 1) == 2 * q * p);
    BOOST_TEST(bt.prob(2, 2) == p * p);
}

BOOST_AUTO_TEST_CASE(reserve_three_test, *tolerance(TOL)) {
    double q = 0.2;
    double p = 0.8;
    int channel = -1;  // can be ignored for this test.
    TestableBinomialTransition bt(q, channel);
    bt.reserve(3);
    BOOST_TEST(bt.prob(0, 0) == 1.0);
    BOOST_TEST(bt.prob(1, 0) == q);
    BOOST_TEST(bt.prob(1, 1) == p);
    BOOST_TEST(bt.prob(2, 0) == q * q);
    BOOST_TEST(bt.prob(2, 1) == 2 * q * p);
    BOOST_TEST(bt.prob(2, 2) == p * p);
    BOOST_TEST(bt.prob(3, 0) == q * q * q);
    BOOST_TEST(bt.prob(3, 1) == 3 * q * q * p);
    BOOST_TEST(bt.prob(3, 2) == 3 * q * p * p);
    BOOST_TEST(bt.prob(3, 3) == p * p * p);
}

BOOST_AUTO_TEST_CASE(reserve_no_shrink_test, *tolerance(TOL)) {
    double q = 0.2;
    double p = 0.8;
    int channel = -1;  // can be ignored for this test.
    TestableBinomialTransition bt(q, channel);
    bt.reserve(3);
    bt.reserve(1);  // This operation should be silently ignored.
    BOOST_TEST(bt.prob(0, 0) == 1.0);
    BOOST_TEST(bt.prob(1, 0) == q);
    BOOST_TEST(bt.prob(1, 1) == p);
    BOOST_TEST(bt.prob(2, 0) == q * q);
    BOOST_TEST(bt.prob(2, 1) == 2 * q * p);
    BOOST_TEST(bt.prob(2, 2) == p * p);
    BOOST_TEST(bt.prob(3, 0) == q * q * q);
    BOOST_TEST(bt.prob(3, 1) == 3 * q * q * p);
    BOOST_TEST(bt.prob(3, 2) == 3 * q * p * p);
    BOOST_TEST(bt.prob(3, 3) == p * p * p);
}

BOOST_AUTO_TEST_CASE(forward_in_place_trivial_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(0);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 1.0;  // loc is {0, 0}
    int edmans = 0;
    bt.forward(&edmans, &psv);
    BOOST_TEST(psv.tensor[loc] == 1.0);  // loc is {0, 0}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_trivial_test, *tolerance(TOL)) {
//     double q = 0.05;
//     double p = 0.95;
//     int channel = 0;
//     TestableBinomialTransition bt(q, channel);
//     bt.reserve(0);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 1;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 1.0;  // loc is {0, 0}
//     int edmans = 0;
//     bt.forward(tsr1, &edmans, &psv2);
//     BOOST_TEST(psv2.tensor[loc] == 1.0);  // loc is {0, 0}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_basic_transition_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.7;  // loc is {0, 1}
    int edmans = 0;
    bt.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3 + 0.7 * q);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.7 * p);  // loc is {0, 1}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_basic_transition_test, *tolerance(TOL))
// {
//     double q = 0.05;
//     double p = 0.95;
//     int channel = 0;
//     TestableBinomialTransition bt(q, channel);
//     bt.reserve(1);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 2;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.3;  // loc is {0, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.7;  // loc is {0, 1}
//     int edmans = 0;
//     bt.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.3 + 0.7 * q);  // loc is {0, 0}
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.7 * p);  // loc is {0, 1}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_bigger_transition_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.2;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.3;  // loc is {0, 1}
    loc[1] = 2;
    psv.tensor[loc] = 0.7;  // loc is {0, 2}
    int edmans = 0;
    bt.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    // loc is {0, 0}
    BOOST_TEST(psv.tensor[loc] == 0.2 + 0.3 * q + 0.7 * q * q);
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.3 * p + 0.7 * 2 * q * p);  // loc is {0, 1}
    loc[1] = 2;
    BOOST_TEST(psv.tensor[loc] == 0.7 * p * p);  // loc is {0, 2}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_bigger_transition_test, *tolerance(TOL))
// {
//     double q = 0.05;
//     double p = 0.95;
//     int channel = 0;
//     TestableBinomialTransition bt(q, channel);
//     bt.reserve(2);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 3;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.2;  // loc is {0, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.3;  // loc is {0, 1}
//     loc[1] = 2;
//     psv1.tensor[loc] = 0.7;  // loc is {0, 2}
//     int edmans = 0;
//     bt.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     // loc is {0, 0}
//     BOOST_TEST(psv2.tensor[loc] == 0.2 + 0.3 * q + 0.7 * q * q);
//     loc[1] = 1;
//     // loc is {0, 1}
//     BOOST_TEST(psv2.tensor[loc] == 0.3 * p + 0.7 * 2 * q * p);
//     loc[1] = 2;
//     BOOST_TEST(psv2.tensor[loc] == 0.7 * p * p);  // loc is {0, 2}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_multiple_edmans_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 3;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.2;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.8;  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {1, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.7;  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    psv.tensor[loc] = 0.4;  // loc is {2, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.6;  // loc is {2, 1}
    int edmans = 2;
    bt.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.2 + 0.8 * q);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.8 * p);  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3 + 0.7 * q);  // loc is {1, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.7 * p);  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.4 + 0.6 * q);  // loc is {2, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.6 * p);  // loc is {2, 1}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_multiple_edmans_test, *tolerance(TOL)) {
//     double q = 0.05;
//     double p = 0.95;
//     int channel = 0;
//     TestableBinomialTransition bt(q, channel);
//     bt.reserve(2);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 3;
//     shape[1] = 2;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.2;  // loc is {0, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.8;  // loc is {0, 1}
//     loc[0] = 1;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.3;  // loc is {1, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.7;  // loc is {1, 1}
//     loc[0] = 2;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.4;  // loc is {2, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.6;  // loc is {2, 1}
//     int edmans = 2;
//     bt.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.2 + 0.8 * q);  // loc is {0, 0}
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.8 * p);  // loc is {0, 1}
//     loc[0] = 1;
//     loc[1] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.3 + 0.7 * q);  // loc is {1, 0}
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.7 * p);  // loc is {1, 1}
//     loc[0] = 2;
//     loc[1] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.4 + 0.6 * q);  // loc is {2, 0}
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.6 * p);  // loc is {2, 1}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_other_dye_colors_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 1;  // corresponds to 2nd dim of tensor
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 4;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    shape[2] = 2;
    shape[3] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    loc[3] = 0;
    psv.tensor[loc] = 0.1;  // loc is {0, 0, 0, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.2;  // loc is {0, 0, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0, 1, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.4;  // loc is {0, 0, 1, 1}
    loc[1] = 1;
    loc[2] = 0;
    loc[3] = 0;
    psv.tensor[loc] = 0.5;  // loc is {0, 1, 0, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.6;  // loc is {0, 1, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    psv.tensor[loc] = 0.7;  // loc is {0, 1, 1, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.8;  // loc is {0, 1, 1, 1}
    int edmans = 0;
    bt.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.1 + 0.3 * q);  // loc is {0, 0, 0, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.2 + 0.4 * q);  // loc is {0, 0, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3 * p);  // loc is {0, 0, 1, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.4 * p);  // loc is {0, 0, 1, 1}
    loc[1] = 1;
    loc[2] = 0;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.5 + 0.7 * q);  // loc is {0, 1, 0, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.6 + 0.8 * q);  // loc is {0, 1, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.7 * p);  // loc is {0, 1, 1, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.8 * p);  // loc is {0, 1, 1, 1}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_other_dye_colors_test, *tolerance(TOL))
// {
//     double q = 0.05;
//     double p = 0.95;
//     int channel = 1;  // corresponds to 2nd dim of tensor
//     TestableBinomialTransition bt(q, channel);
//     bt.reserve(1);
//     int order = 4;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 2;
//     shape[2] = 2;
//     shape[3] = 2;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     loc[2] = 0;
//     loc[3] = 0;
//     psv1.tensor[loc] = 0.1;  // loc is {0, 0, 0, 0}
//     loc[3] = 1;
//     psv1.tensor[loc] = 0.2;  // loc is {0, 0, 0, 1}
//     loc[2] = 1;
//     loc[3] = 0;
//     psv1.tensor[loc] = 0.3;  // loc is {0, 0, 1, 0}
//     loc[3] = 1;
//     psv1.tensor[loc] = 0.4;  // loc is {0, 0, 1, 1}
//     loc[1] = 1;
//     loc[2] = 0;
//     loc[3] = 0;
//     psv1.tensor[loc] = 0.5;  // loc is {0, 1, 0, 0}
//     loc[3] = 1;
//     psv1.tensor[loc] = 0.6;  // loc is {0, 1, 0, 1}
//     loc[2] = 1;
//     loc[3] = 0;
//     psv1.tensor[loc] = 0.7;  // loc is {0, 1, 1, 0}
//     loc[3] = 1;
//     psv1.tensor[loc] = 0.8;  // loc is {0, 1, 1, 1}
//     int edmans = 0;
//     bt.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     loc[2] = 0;
//     loc[3] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.1 + 0.3 * q);  // loc is {0, 0, 0, 0}
//     loc[3] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.2 + 0.4 * q);  // loc is {0, 0, 0, 1}
//     loc[2] = 1;
//     loc[3] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.3 * p);  // loc is {0, 0, 1, 0}
//     loc[3] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.4 * p);  // loc is {0, 0, 1, 1}
//     loc[1] = 1;
//     loc[2] = 0;
//     loc[3] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.5 + 0.7 * q);  // loc is {0, 1, 0, 0}
//     loc[3] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.6 + 0.8 * q);  // loc is {0, 1, 0, 1}
//     loc[2] = 1;
//     loc[3] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.7 * p);  // loc is {0, 1, 1, 0}
//     loc[3] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.8 * p);  // loc is {0, 1, 1, 1}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(backward_in_place_trivial_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(0);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 1.0;  // loc is {0, 0}
    int edmans = 0;
    bt.backward(psv, &edmans, &psv);
    BOOST_TEST(psv.tensor[loc] == 1.0);  // loc is {0, 0}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_trivial_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(0);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 1;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 1.0;  // loc is {0, 0}
    int edmans = 0;
    bt.backward(psv1, &edmans, &psv2);
    BOOST_TEST(psv2.tensor[loc] == 1.0);  // loc is {0, 0}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_basic_transition_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.7;  // loc is {0, 1}
    int edmans = 0;
    bt.backward(psv, &edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == q * 0.3 + p * 0.7);  // loc is {0, 1}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_basic_transition_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.7;  // loc is {0, 1}
    int edmans = 0;
    bt.backward(psv1, &edmans, &psv2);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.3);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv2.tensor[loc] == q * 0.3 + p * 0.7);  // loc is {0, 1}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_bigger_transition_test,
                     *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.2;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.3;  // loc is {0, 1}
    loc[1] = 2;
    psv.tensor[loc] = 0.7;  // loc is {0, 2}
    int edmans = 0;
    bt.backward(psv, &edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.2);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == q * 0.2 + p * 0.3);  // loc is {0, 1}
    loc[1] = 2;
    // loc is {0, 2}
    BOOST_TEST(psv.tensor[loc] == q * q * 0.2 + 2 * q * p * 0.3 + p * p * 0.7);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_bigger_transition_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 0.2;  // loc is {0, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.3;  // loc is {0, 1}
    loc[1] = 2;
    psv1.tensor[loc] = 0.7;  // loc is {0, 2}
    int edmans = 0;
    bt.backward(psv1, &edmans, &psv2);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.2);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv2.tensor[loc] == q * 0.2 + p * 0.3);  // loc is {0, 1}
    loc[1] = 2;
    // loc is {0, 2}
    BOOST_TEST(psv2.tensor[loc] == q * q * 0.2 + 2 * q * p * 0.3 + p * p * 0.7);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_multiple_edmans_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 3;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.2;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.8;  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {1, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.7;  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    psv.tensor[loc] = 0.4;  // loc is {2, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.6;  // loc is {2, 1}
    int edmans = 2;
    bt.backward(psv, &edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.2);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == q * 0.2 + p * 0.8);  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3);  // loc is {1, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == q * 0.3 + p * 0.7);  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.4);  // loc is {2, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == q * 0.4 + p * 0.6);  // loc is {2, 1}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_multiple_edmans_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 3;
    shape[1] = 2;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 0.2;  // loc is {0, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.8;  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    psv1.tensor[loc] = 0.3;  // loc is {1, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.7;  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    psv1.tensor[loc] = 0.4;  // loc is {2, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.6;  // loc is {2, 1}
    int edmans = 2;
    bt.backward(psv1, &edmans, &psv2);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.2);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv2.tensor[loc] == q * 0.2 + p * 0.8);  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.3);  // loc is {1, 0}
    loc[1] = 1;
    BOOST_TEST(psv2.tensor[loc] == q * 0.3 + p * 0.7);  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.4);  // loc is {2, 0}
    loc[1] = 1;
    BOOST_TEST(psv2.tensor[loc] == q * 0.4 + p * 0.6);  // loc is {2, 1}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_other_dye_colors_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 1;  // corresponds to 2nd dim of tensor
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 4;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    shape[2] = 2;
    shape[3] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    loc[3] = 0;
    psv.tensor[loc] = 0.1;  // loc is {0, 0, 0, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.2;  // loc is {0, 0, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0, 1, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.4;  // loc is {0, 0, 1, 1}
    loc[1] = 1;
    loc[2] = 0;
    loc[3] = 0;
    psv.tensor[loc] = 0.5;  // loc is {0, 1, 0, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.6;  // loc is {0, 1, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    psv.tensor[loc] = 0.7;  // loc is {0, 1, 1, 0}
    loc[3] = 1;
    psv.tensor[loc] = 0.8;  // loc is {0, 1, 1, 1}
    int edmans = 0;
    bt.backward(psv, &edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.1);  // loc is {0, 0, 0, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.2);  // loc is {0, 0, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == q * 0.1 + p * 0.3);  // loc is {0, 0, 1, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == q * 0.2 + p * 0.4);  // loc is {0, 0, 1, 1}
    loc[1] = 1;
    loc[2] = 0;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.5);  // loc is {0, 1, 0, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.6);  // loc is {0, 1, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    BOOST_TEST(psv.tensor[loc] == q * 0.5 + p * 0.7);  // loc is {0, 1, 1, 0}
    loc[3] = 1;
    BOOST_TEST(psv.tensor[loc] == q * 0.6 + p * 0.8);  // loc is {0, 1, 1, 1}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_other_dye_colors_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 1;  // corresponds to 2nd dim of tensor
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 4;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    shape[2] = 2;
    shape[3] = 2;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    loc[3] = 0;
    psv1.tensor[loc] = 0.1;  // loc is {0, 0, 0, 0}
    loc[3] = 1;
    psv1.tensor[loc] = 0.2;  // loc is {0, 0, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    psv1.tensor[loc] = 0.3;  // loc is {0, 0, 1, 0}
    loc[3] = 1;
    psv1.tensor[loc] = 0.4;  // loc is {0, 0, 1, 1}
    loc[1] = 1;
    loc[2] = 0;
    loc[3] = 0;
    psv1.tensor[loc] = 0.5;  // loc is {0, 1, 0, 0}
    loc[3] = 1;
    psv1.tensor[loc] = 0.6;  // loc is {0, 1, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    psv1.tensor[loc] = 0.7;  // loc is {0, 1, 1, 0}
    loc[3] = 1;
    psv1.tensor[loc] = 0.8;  // loc is {0, 1, 1, 1}
    int edmans = 0;
    bt.backward(psv1, &edmans, &psv2);
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    loc[3] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.1);  // loc is {0, 0, 0, 0}
    loc[3] = 1;
    BOOST_TEST(psv2.tensor[loc] == 0.2);  // loc is {0, 0, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    BOOST_TEST(psv2.tensor[loc] == q * 0.1 + p * 0.3);  // loc is {0, 0, 1, 0}
    loc[3] = 1;
    BOOST_TEST(psv2.tensor[loc] == q * 0.2 + p * 0.4);  // loc is {0, 0, 1, 1}
    loc[1] = 1;
    loc[2] = 0;
    loc[3] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.5);  // loc is {0, 1, 0, 0}
    loc[3] = 1;
    BOOST_TEST(psv2.tensor[loc] == 0.6);  // loc is {0, 1, 0, 1}
    loc[2] = 1;
    loc[3] = 0;
    BOOST_TEST(psv2.tensor[loc] == q * 0.5 + p * 0.7);  // loc is {0, 1, 1, 0}
    loc[3] = 1;
    BOOST_TEST(psv2.tensor[loc] == q * 0.6 + p * 0.8);  // loc is {0, 1, 1, 1}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(improve_fit_trivial_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(0);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 1;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 1.0;  // loc is {0, 0}
    bpsv.tensor[loc] = 1.0;
    nbpsv.tensor[loc] = 1.0;
    delete[] loc;
    int edmans = 0;
    double probability = 1.0;
    ParameterFitter pf;
    bt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &pf);
    BOOST_TEST(pf.numerator == 0.0);
    BOOST_TEST(pf.denominator == 0.0);
}

BOOST_AUTO_TEST_CASE(improve_fit_basic_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv.tensor[loc] = 0.32;
    nbpsv.tensor[loc] = 0.33;
    loc[1] = 1;
    fpsv.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv.tensor[loc] = 0.72;
    nbpsv.tensor[loc] = 0.73;
    delete[] loc;
    int edmans = 0;
    double probability = 1.0;
    ParameterFitter pf;
    bt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &pf);
    BOOST_TEST(pf.get() == (0.71 * q * 0.33) / (0.71 * 0.72));
}

BOOST_AUTO_TEST_CASE(improve_fit_bigger_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(2);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv.tensor[loc] = 0.32;
    nbpsv.tensor[loc] = 0.33;
    loc[1] = 1;
    fpsv.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv.tensor[loc] = 0.72;
    nbpsv.tensor[loc] = 0.73;
    loc[1] = 2;
    fpsv.tensor[loc] = 0.91;  // loc is {0, 2}
    bpsv.tensor[loc] = 0.92;
    nbpsv.tensor[loc] = 0.93;
    delete[] loc;
    int edmans = 0;
    double probability = 1.0;
    ParameterFitter pf;
    bt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &pf);
    BOOST_TEST(pf.get()
               == (0.71 * q * 0.33 + 0.91 * (q * p * 2.0) * 0.73
                   + 0.91 * (q * q) * 0.33 * 2.0)
                          / (0.71 * 0.72 + 0.91 * 0.92 * 2.0));
}

BOOST_AUTO_TEST_CASE(improve_fit_multiple_edmans_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 2;
    shape[1] = 2;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv.tensor[loc] = 0.32;
    nbpsv.tensor[loc] = 0.33;
    loc[1] = 1;
    fpsv.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv.tensor[loc] = 0.72;
    nbpsv.tensor[loc] = 0.73;
    loc[0] = 1;
    loc[1] = 0;
    fpsv.tensor[loc] = 0.41;  // loc is {1, 0}
    bpsv.tensor[loc] = 0.42;
    nbpsv.tensor[loc] = 0.43;
    loc[1] = 1;
    fpsv.tensor[loc] = 0.81;  // loc is {1, 1}
    bpsv.tensor[loc] = 0.82;
    nbpsv.tensor[loc] = 0.83;
    delete[] loc;
    int edmans = 1;
    double probability = 1.0;
    ParameterFitter pf;
    bt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &pf);
    BOOST_TEST(pf.get()
               == (0.71 * q * 0.33 + 0.81 * q * 0.43)
                          / (0.71 * 0.72 + 0.81 * 0.82));
}

BOOST_AUTO_TEST_CASE(improve_fit_other_dye_color_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 3;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    shape[2] = 2;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    fpsv.tensor[loc] = 0.31;  // loc is {0, 0, 0}
    bpsv.tensor[loc] = 0.32;
    nbpsv.tensor[loc] = 0.33;
    loc[2] = 1;
    fpsv.tensor[loc] = 0.231;  // loc is {0, 0, 1}
    bpsv.tensor[loc] = 0.232;
    nbpsv.tensor[loc] = 0.233;
    loc[1] = 1;
    loc[2] = 0;
    fpsv.tensor[loc] = 0.71;  // loc is {0, 1, 0}
    bpsv.tensor[loc] = 0.72;
    nbpsv.tensor[loc] = 0.73;
    loc[2] = 1;
    fpsv.tensor[loc] = 0.271;  // loc is {0, 1, 1}
    bpsv.tensor[loc] = 0.272;
    nbpsv.tensor[loc] = 0.273;
    delete[] loc;
    int edmans = 0;
    double probability = 1.0;
    ParameterFitter pf;
    bt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &pf);
    BOOST_TEST(pf.get()
               == (0.71 * q * 0.33 + 0.271 * q * 0.233)
                          / (0.71 * 0.72 + 0.271 * 0.272));
}

BOOST_AUTO_TEST_CASE(improve_fit_different_probability_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv.tensor[loc] = 0.32;
    nbpsv.tensor[loc] = 0.33;
    loc[1] = 1;
    fpsv.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv.tensor[loc] = 0.72;
    nbpsv.tensor[loc] = 0.73;
    delete[] loc;
    int edmans = 0;
    double probability = 0.123456789;
    ParameterFitter pf;
    bt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &pf);
    BOOST_TEST(pf.get() == (0.71 * q * 0.33) / (0.71 * 0.72));
}

BOOST_AUTO_TEST_CASE(improve_fit_twice_test, *tolerance(TOL)) {
    double q = 0.05;
    double p = 0.95;
    int channel = 0;
    TestableBinomialTransition bt(q, channel);
    bt.reserve(1);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector fpsv1(order, shape);
    PeptideStateVector bpsv1(order, shape);
    PeptideStateVector nbpsv1(order, shape);
    PeptideStateVector fpsv2(order, shape);
    PeptideStateVector bpsv2(order, shape);
    PeptideStateVector nbpsv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv1.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv1.tensor[loc] = 0.32;
    nbpsv1.tensor[loc] = 0.33;
    fpsv2.tensor[loc] = 0.231;
    bpsv2.tensor[loc] = 0.232;
    nbpsv2.tensor[loc] = 0.233;
    loc[1] = 1;
    fpsv1.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv1.tensor[loc] = 0.72;
    nbpsv1.tensor[loc] = 0.73;
    fpsv2.tensor[loc] = 0.271;
    bpsv2.tensor[loc] = 0.272;
    nbpsv2.tensor[loc] = 0.273;
    delete[] loc;
    int edmans = 0;
    double prob1 = 0.123456789;
    double prob2 = 0.987654321;
    ParameterFitter pf;
    bt.improve_fit(fpsv1, bpsv1, nbpsv1, edmans, prob1, &pf);
    bt.improve_fit(fpsv2, bpsv2, nbpsv2, edmans, prob2, &pf);
    BOOST_TEST(pf.get()
               == (0.71 * q * 0.33 / prob1 + 0.271 * q * 0.233 / prob2)
                          / (0.71 * 0.72 / prob1 + 0.271 * 0.272 / prob2));
}

BOOST_AUTO_TEST_SUITE_END()  // binomial_transition_suite
BOOST_AUTO_TEST_SUITE_END()  // step_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

#include "PGR_5.h"
#include "quality.h"
#include "normal_box_surface.h"

using namespace std;
using namespace Eigen;
using namespace KDL;

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

	ros::init(argc, argv, "grasp_dataset_populate");	// ROS node
	ros::NodeHandle nh;

	string relative_path_file_in;
	string relative_path_file_out;
	string file_name_in;
	string file_name_out;
	//int set = 0;
	nh.param<std::string>("file_name_in", relative_path_file_in, "/box_db_2/box_db_quality_PGR_matlab" );
	nh.param<std::string>("file_name_out", relative_path_file_out, "/box_db_2/" );
	//nh.param<std::string>("set", set, 0);

	///////////////////// load the data_base ////////////////////////////////////
	std::string path = ros::package::getPath("grasp_learning");
	file_name_in = path + relative_path_file_in;
	ifstream file(file_name_in);

	double q = 0;

	double xb = 0;
	double yb = 0;
	double zb = 0;

	double xp = 0;
	double yp = 0;
	double zp = 0;

	double qx = 0;
	double qy = 0;
	double qz = 0;
	double qw = 0;

	file_name_out = path + relative_path_file_out;

   	ofstream file_output; //output file for matlab
   	std::string name = "box_db_pop";
    file_output.open( file_name_out + name, ofstream::app);

    ofstream file_output_1; //output file for klampt
    std::string name_csv = "box_db_pop.csv";
    file_output_1.open(file_name_out + name_csv, ofstream::app);

	///////////////////////////////// get values from file for each line //////////////////////////////////
	for(std::string line; getline( file, line, '\n' ); ) // for each line
	{
		std::vector<double> values_inline;
    	std::istringstream iss_line(line);
    	for(std::string value; getline(iss_line, value, ' ' ); )
    		values_inline.push_back(stod(value));

    	//if( values_inline[0] > 0 )
    	//{
    		q = values_inline[0];

    		xb = values_inline[1];
			yb = values_inline[2];
			zb = values_inline[3];

			xp = values_inline[4];
			yp = values_inline[5];
			zp = values_inline[6];

			qx = values_inline[7];
			qy = values_inline[8];
			qz = values_inline[9];
			qw = values_inline[10];

			Eigen::MatrixXd T_point = MatrixXd::Identity(4,4);

			T_point(0,3) = xp;
			T_point(1,3) = yp;
			T_point(2,3) = zp;

			KDL::Rotation Rot_point = Rotation::Quaternion(qx,qy,qz,qw);

			T_point(0,0) = Rot_point.data[0];
			T_point(0,1) = Rot_point.data[1];
			T_point(0,2) = Rot_point.data[2];
			T_point(1,0) = Rot_point.data[3];
			T_point(1,1) = Rot_point.data[4];
			T_point(1,2) = Rot_point.data[5];
			T_point(2,0) = Rot_point.data[6];
			T_point(2,1) = Rot_point.data[7];
			T_point(2,2) = Rot_point.data[8];

			Eigen::MatrixXd T_rot_same_face = MatrixXd::Identity(4,4);
			Eigen::MatrixXd T_rot_opposite1 = MatrixXd::Identity(4,4);
			Eigen::MatrixXd T_rot_opposite2 = MatrixXd::Identity(4,4);

			Eigen::MatrixXd R0 = MatrixXd::Identity(3,3);
			Eigen::MatrixXd R1 = MatrixXd::Identity(3,3);
			Eigen::MatrixXd R2 = MatrixXd::Identity(3,3);

			R1 << 1.0, 0.0, 0.0,
				0.0, cos(180.0*M_PI/180.0), -sin(180.0*M_PI/180.0),
				0.0, sin(180.0*M_PI/180.0), cos(180.0*M_PI/180.0);

			R0 << cos(180.0*(M_PI/180.0)), sin(180.0*(M_PI/180.0)), 0.0,
				-sin(180.0*(M_PI/180.0)), cos(180.0*(M_PI/180.0)), 0.0,
				0.0,				0.0, 				1.0;

			R2 << cos(180.0*(M_PI/180.0)),  0.0, sin(180.0*(M_PI/180.0)),
					        0.0  ,  1.0,    0.0 ,
				-sin(180.0*(M_PI/180.0)), 0.0, cos(180.0*(M_PI/180.0));

			T_rot_same_face.block<3,3>(0,0) = R0;
			T_rot_opposite1.block<3,3>(0,0) = R1;
			T_rot_opposite2.block<3,3>(0,0) = R2;

			cout << T_rot_same_face << endl;
			cout << T_rot_opposite1 << endl;
			cout << T_rot_opposite2 << endl;

			cout << T_point << endl;

			Eigen::MatrixXd T0 = MatrixXd::Identity(4,4);
			Eigen::MatrixXd T1 = MatrixXd::Identity(4,4);
			Eigen::MatrixXd T2 = MatrixXd::Identity(4,4);

			T0 = T_rot_same_face * T_point; //around z
			T1 = T_rot_opposite1 * T_point; //around x
			T2 = T_rot_opposite2 * T_point; //around y

			cout << "-------------- T0 - Z - 1----------------"<< endl;
			cout << T0 << endl;
			cout << "-------------- T1 - X - 3----------------"<< endl;
			cout << T1 << endl;
			cout << "-------------- T2 - Y - 2----------------"<< endl;
			cout << T2 << endl;

			Eigen::Matrix3f TR0(3,3);
			Eigen::Matrix3f TR1(3,3);
			Eigen::Matrix3f TR2(3,3);
			Eigen::Matrix3f TRpoint(3,3);

			for(int i = 0 ; i <3 ; i++)
			{	for(int j = 0 ; j <3 ; j++)
				{
					TR0(i,j) = T0(i,j);
					TR1(i,j) = T1(i,j);
					TR2(i,j) = T2(i,j);
					TRpoint(i,j) = T_point(i,j);
				}
			}

			Eigen::Quaternionf q0(TR0);
			Eigen::Quaternionf q1(TR1);
			Eigen::Quaternionf q2(TR2);
			Eigen::Quaternionf qp(TRpoint);

			file_output<<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<xp<<' '<<yp<<' '<<zp<<' '<<qx<<' '<<qy<<' '<<qz<<' '<<qw<<endl;
			file_output<<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<T0(0,3)<<' '<<T0(1,3)<<' '<<T0(2,3)<<' '<<q0.x()<<' '<<q0.y()<<' '<<q0.z()<<' '<<q0.w()<<endl;
			file_output<<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<T1(0,3)<<' '<<T1(1,3)<<' '<<T1(2,3)<<' '<<q1.x()<<' '<<q1.y()<<' '<<q1.z()<<' '<<q1.w()<<endl;
			file_output<<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<T2(0,3)<<' '<<T2(1,3)<<' '<<T2(2,3)<<' '<<q2.x()<<' '<<q2.y()<<' '<<q2.z()<<' '<<q2.w()<<endl;

			file_output_1<<xb<<','<<yb<<','<<zb<<','<<xp<<','<<yp<<','<<zp<<','<<qx<<','<<qy<<','<<qz<<','<<qw<<endl;
			file_output_1<<xb<<','<<yb<<','<<zb<<','<<T0(0,3)<<','<<T0(1,3)<<','<<T0(2,3)<<','<<q0.x()<<','<<q0.y()<<','<<q0.z()<<','<<q0.w()<<endl;
			file_output_1<<xb<<','<<yb<<','<<zb<<','<<T1(0,3)<<','<<T1(1,3)<<','<<T1(2,3)<<','<<q1.x()<<','<<q1.y()<<','<<q1.z()<<','<<q1.w()<<endl;
			file_output_1<<xb<<','<<yb<<','<<zb<<','<<T2(0,3)<<','<<T2(1,3)<<','<<T2(2,3)<<','<<q2.x()<<','<<q2.y()<<','<<q2.z()<<','<<q2.w()<<endl;

			cout <<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<xp<<' '<<yp<<' '<<zp<<' '<<qx<<' '<<qy<<' '<<qz<<' '<<qw<<endl;
			cout <<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<T0(0,3)<<' '<<T0(1,3)<<' '<<T0(2,3)<<' '<<q0.x()<<' '<<q0.y()<<' '<<q0.z()<<' '<<q0.w()<<endl;
			cout <<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<T1(0,3)<<' '<<T1(1,3)<<' '<<T1(2,3)<<' '<<q1.x()<<' '<<q1.y()<<' '<<q1.z()<<' '<<q1.w()<<endl;
			cout <<q<<' '<<xb<<' '<<yb<<' '<<zb<<' '<<T2(0,3)<<' '<<T2(1,3)<<' '<<T2(2,3)<<' '<<q2.x()<<' '<<q2.y()<<' '<<q2.z()<<' '<<q2.w()<<endl;

    }

    ros::spinOnce();
	return 0;

}


#include "pch.h"
#include "CppUnitTest.h"
#include "../../Validator/cpp/Validator/ConstraintsLoading.h"
#include "../../StaticStabilityRamos/StaticStabilityRamos/StaticStabilityRamos.h"
#include <boost/geometry.hpp>

using namespace Microsoft::VisualStudio::CppUnitTestFramework;
using namespace validator;

namespace StaticStability
{
	TEST_CLASS(ResultantForce)	{
		const int dim = 4;
	public:

		TEST_METHOD(Single) {
			// Items are stacked
			ItemType type(1, dim, dim, dim, 10, false, 0);

			Item item1(0, 1, 0, type);

			// Instance Creation
			Instance instance("", Vehicle(0, 0, 0, 0, 0, 0, 0, 0, 0), {}, {}, 0, 0);
			instance.items.emplace_back(item1);

			// Tour Creation
			Tour tour(1, {0}, {1});

			// Positions
			instance.items.at(item1.id).setPosition(validator::Point(0, 0, 0));
			bPoint p = StaticStabilityRamos::getResultantForcePoint(instance.items.at(item1.id), instance);

			// The Point of the Resultant Force is not influenced
			Assert::AreEqual(boost::geometry::get<0>(p), dim * 0.5);
			Assert::AreEqual(boost::geometry::get<1>(p), dim * 0.5);

		}

		TEST_METHOD(Stacked) {
			// Items are stacked
			ItemType type(1, dim, dim, dim, 10, false, 0);

			Item item1(0, 1, 0, type);
			Item item2(1, 1, 0, type);

			// Instance Creation
			Instance instance("", Vehicle(0, 0, 0, 0, 0, 0, 0, 0, 0), {}, {}, 0, 0);
			instance.items.emplace_back(item1);
			instance.items.emplace_back(item2);

			// Tour Creation
			std::vector<unsigned int> customer_ids{ 1 };
			std::vector<unsigned int> item_ids{ 0, 1 };
			Tour tour(1, customer_ids, item_ids);

			// Positions
			instance.items.at(item1.id).setPosition(validator::Point(0, 0, 0));
			instance.items.at(item2.id).setPosition(validator::Point(0, 0, dim));

			const unsigned int endPos = tour.item_ids.size();
			validator::ConstraintsLoading::getRelevantItems(instance.items.at(item1.id), tour, endPos, instance);
			validator::ConstraintsLoading::getRelevantItems(instance.items.at(item2.id), tour, endPos, instance);

			bPoint p = StaticStabilityRamos::getResultantForcePoint(instance.items.at(item1.id), instance);

			// The Point of the Resultant Force is not influenced
			Assert::AreEqual(boost::geometry::get<0>(p), dim * 0.5);
			Assert::AreEqual(boost::geometry::get<1>(p), dim * 0.5);

		}

		TEST_METHOD(Middle) {
			// Items are stacked
			ItemType type(1, dim, dim, dim, 10, false, 0);

			Item item1(0, 1, 0, type);
			Item item2(1, 1, 0, type);

			// Instance Creation
			Instance instance("", Vehicle(0, 0, 0, 0, 0, 0, 0, 0, 0), {}, {}, 0, 0);
			instance.items.emplace_back(item1);
			instance.items.emplace_back(item2);

			// Tour Creation
			std::vector<unsigned int> customer_ids{ 1 };
			std::vector<unsigned int> item_ids{ 0, 1 };
			Tour tour(1, customer_ids, item_ids);

			// Positions
			instance.items.at(item1.id).setPosition(validator::Point(0, 0, 0));
			instance.items.at(item2.id).setPosition(validator::Point(0, dim/2, dim));

			const unsigned int endPos = tour.item_ids.size();
			validator::ConstraintsLoading::getRelevantItems(instance.items.at(item1.id), tour, endPos, instance);
			validator::ConstraintsLoading::getRelevantItems(instance.items.at(item2.id), tour, endPos, instance);

			bPoint p = StaticStabilityRamos::getResultantForcePoint(instance.items.at(item1.id), instance);

			// The Point of the Resultant Force is in the center of both item's center of gravity
			Assert::AreEqual(dim * 0.5, boost::geometry::get<0>(p));
			Assert::AreEqual(dim * 0.75, boost::geometry::get<1>(p));

		}

		TEST_METHOD(Weighted) {
			int mass2 = 10;
			int mass1 = 2 * mass2;

			// Items are stacked
			ItemType type1(1, dim, dim, dim, mass1, false, 0);
			ItemType type2(1, dim, dim, dim, mass2, false, 0);

			Item item1(0, 1, 0, type1);
			Item item2(1, 1, 0, type2);

			// Instance Creation
			Instance instance("", Vehicle(0, 0, 0, 0, 0, 0, 0, 0, 0), {}, {}, 0, 0);
			instance.items.emplace_back(item1);
			instance.items.emplace_back(item2);

			// Tour Creation
			std::vector<unsigned int> customer_ids{ 1 };
			std::vector<unsigned int> item_ids{ 0, 1 };
			Tour tour(1, customer_ids, item_ids);

			// Positions
			instance.items.at(item1.id).setPosition(validator::Point(0, 0, 0));
			instance.items.at(item2.id).setPosition(validator::Point(0, dim / 2, dim));

			const unsigned int endPos = tour.item_ids.size();
			validator::ConstraintsLoading::getRelevantItems(instance.items.at(item1.id), tour, endPos, instance);
			validator::ConstraintsLoading::getRelevantItems(instance.items.at(item2.id), tour, endPos, instance);

			bPoint p = StaticStabilityRamos::getResultantForcePoint(instance.items.at(item1.id), instance);

			// Due to different masses, the point is drawn to the center
			Assert::AreEqual(dim * 0.5, boost::geometry::get<0>(p));
			Assert::AreEqual(dim * 0.5, boost::geometry::get<1>(p));

		}
	};
}


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

#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <sstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<int, stats<tag::sum, tag::sum_of_weights, tag::sum_of_variates<int, tag::covariate1> >, int> acc;

    acc(1, weight = 2, covariate1 = 3);
    BOOST_CHECK_EQUAL(2, sum(acc));
    BOOST_CHECK_EQUAL(2, sum_of_weights(acc));
    BOOST_CHECK_EQUAL(3, sum_of_variates(acc));

    acc(2, weight = 4, covariate1 = 6);
    BOOST_CHECK_EQUAL(10, sum(acc));
    BOOST_CHECK_EQUAL(6, sum_of_weights(acc));
    BOOST_CHECK_EQUAL(9, sum_of_variates(acc));

    acc(3, weight = 6, covariate1 = 9);
    BOOST_CHECK_EQUAL(28, sum(acc));
    BOOST_CHECK_EQUAL(12, sum_of_weights(acc));
    BOOST_CHECK_EQUAL(18, sum_of_variates(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_persistency
//
void test_persistency()
{
    std::stringstream ss;
    {
        accumulator_set<int, stats<tag::sum, tag::sum_of_weights, tag::sum_of_variates<int, tag::covariate1> >, int> acc;
        acc(1, weight = 2, covariate1 = 3);
        acc(2, weight = 4, covariate1 = 6);
        acc(3, weight = 6, covariate1 = 9);
        BOOST_CHECK_EQUAL(28, sum(acc));
        BOOST_CHECK_EQUAL(12, sum_of_weights(acc));
        BOOST_CHECK_EQUAL(18, sum_of_variates(acc));

        boost::archive::text_oarchive oa(ss);
        acc.serialize(oa, 0);
    }
    accumulator_set<int, stats<tag::sum, tag::sum_of_weights, tag::sum_of_variates<int, tag::covariate1> >, int> acc;
    boost::archive::text_iarchive ia(ss);
    acc.serialize(ia, 0);
    BOOST_CHECK_EQUAL(28, sum(acc));
    BOOST_CHECK_EQUAL(12, sum_of_weights(acc));
    BOOST_CHECK_EQUAL(18, sum_of_variates(acc));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("sum test");

    test->add(BOOST_TEST_CASE(&test_stat));
    test->add(BOOST_TEST_CASE(&test_persistency));

    return test;
}


#include "Dual_Constraint_Group.hxx"

#include <boost/filesystem.hpp>

#include <vector>

void write_objectives(const boost::filesystem::path &output_dir,
                      const El::BigFloat &objective_const,
                      const std::vector<El::BigFloat> &dual_objective_b);

void write_bilinear_bases(
  const boost::filesystem::path &output_dir, const int &rank,
  const std::vector<Dual_Constraint_Group> &dual_constraint_groups);

void write_blocks(
  const boost::filesystem::path &output_dir, const int &rank,
  const int &num_procs, const std::vector<size_t> &indices,
  const std::vector<Dual_Constraint_Group> &dual_constraint_groups);

void write_primal_objective_c(
  const boost::filesystem::path &output_dir,
  const std::vector<size_t> &indices,
  const std::vector<Dual_Constraint_Group> &dual_constraint_groups);

void write_free_var_matrix(
  const boost::filesystem::path &output_dir,
  const std::vector<size_t> &indices, const size_t &dual_objectives_b_size,
  const std::vector<Dual_Constraint_Group> &dual_constraint_groups);

void write_sdpb_input_files(
  const boost::filesystem::path &output_dir, const int &rank,
  const int &num_procs, const std::vector<size_t> &indices,
  const El::BigFloat &objective_const,
  const std::vector<El::BigFloat> &dual_objective_b,
  const std::vector<Dual_Constraint_Group> &dual_constraint_groups)
{
  boost::filesystem::create_directories(output_dir);
  if(rank == 0)
    {
      write_objectives(output_dir, objective_const, dual_objective_b);
    }
  write_bilinear_bases(output_dir, rank, dual_constraint_groups);
  write_blocks(output_dir, rank, num_procs, indices, dual_constraint_groups);
  write_primal_objective_c(output_dir, indices, dual_constraint_groups);
  write_free_var_matrix(output_dir, indices, dual_objective_b.size(),
                        dual_constraint_groups);
}


#pragma once

#include <Discregrid/mesh/triangle_mesh.hpp>

namespace std
{
    template <>
    struct hash<Eigen::Vector3f>
    {
        std::size_t operator()(Eigen::Vector3f const &x) const
        {
            std::size_t seed = 0;
            std::hash<float> hasher;
            seed ^= hasher(x[0]) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
            seed ^= hasher(x[1]) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
            seed ^= hasher(x[2]) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
            return seed;
        }
    };

    template <>
    struct less<Eigen::Vector3f>
    {
        bool operator()(Eigen::Vector3f const &left, Eigen::Vector3f const &right) const
        {
            for (auto i = 0u; i < 3u; ++i)
            {
                if (left(i) < right(i))
                    return true;
                else if (left(i) > right(i))
                    return false;
            }
            return false;
        }
    };
}
#include <Discregrid/utility/lru_cache.hpp>

#include <Discregrid/acceleration/bounding_sphere_hierarchy.hpp>

#include <array>
#include <map>
#include <set>
#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

namespace Discregrid
{

    enum class NearestEntity;
    class TriangleMesh;
    class Halfedge;
    class MeshDistance
    {

        struct Candidate
        {
            bool operator<(Candidate const &other) const { return b < other.b; }
            unsigned int node_index;
            float b, w;
        };

    public:
        MeshDistance(TriangleMesh const &mesh, bool precompute_normals = true);

        // Returns the shortest unsigned distance from a given point x to
        // the stored mesh.
        // Thread-safe function.
        float distance(Eigen::Vector3f const &x, Eigen::Vector3f *nearest_point = nullptr,
                       unsigned int *nearest_face = nullptr, NearestEntity *ne = nullptr) const;

        // Requires a closed two-manifold mesh as input data.
        // Thread-safe function.
        float signedDistance(Eigen::Vector3f const &x) const;
        float signedDistanceCached(Eigen::Vector3f const &x) const;

        float unsignedDistance(Eigen::Vector3f const &x) const;
        float unsignedDistanceCached(Eigen::Vector3f const &x) const;

    private:
        Eigen::Vector3f vertex_normal(unsigned int v) const;
        Eigen::Vector3f edge_normal(Halfedge const &h) const;
        Eigen::Vector3f face_normal(unsigned int f) const;

        void callback(unsigned int node_index, TriangleMeshBSH const &bsh,
                      Eigen::Vector3f const &x,
                      float &dist) const;

        bool predicate(unsigned int node_index, TriangleMeshBSH const &bsh,
                       Eigen::Vector3f const &x, float &dist) const;

    private:
        TriangleMesh const &m_mesh;
        TriangleMeshBSH m_bsh;

        using FunctionValueCache = LRUCache<Eigen::Vector3f, float>;
        mutable std::vector<TriangleMeshBSH::TraversalQueue> m_queues;
        mutable std::vector<unsigned int> m_nearest_face;
        mutable std::vector<FunctionValueCache> m_cache;
        mutable std::vector<FunctionValueCache> m_ucache;

        std::vector<Eigen::Vector3f> m_face_normals;
        std::vector<Eigen::Vector3f> m_vertex_normals;
        bool m_precomputed_normals;
    };

}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE NonLinearity2D
#include "poplibs_test/NonLinearity.hpp"
#include "poplibs_test/Util.hpp"
#include "poputil/VertexTemplates.hpp"
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <popnn/NonLinearity.hpp>
#include <popnn/NonLinearityDefUtil.hpp>
#include <popnn/codelets.hpp>

#include <cassert>
#include <cmath>
#include <stdexcept>
#include <vector>

#include <boost/program_options.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace popnn;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_support;

#define TOL 0.1 // tolerance of 0.1%
#define FLOAT_ATOL 1e-20
#define HALF_ATOL 1e-7

namespace {

// Generated with python: "{0:-,.6f}".format(random.gauss(0.0,5.0))
constexpr static std::size_t randomDataSize = 54;
constexpr static double randomData[randomDataSize] = {
    -1.363063,  6.590855,  -1.808181, -7.897238, -4.266652,  4.064484,
    -12.879016, -8.521756, -3.760860, 13.313155, 9.388029,   -3.281752,
    -0.848680,  -3.044330, -5.974442, -4.652190, 0.683414,   0.468458,
    -2.227848,  3.479158,  2.000612,  -9.059770, -11.321158, 3.265349,
    7.258343,   -5.725096, 2.390649,  -5.182225, 2.477468,   -1.410790,
    5.631778,   1.608285,  -3.547397, 4.153984,  7.495993,   1.061097,
    3.186139,   -1.215145, 5.905051,  0.284197,  3.458912,   -10.597435,
    3.889679,   -4.992706, 8.237274,  -3.864746, -2.701962,  9.659042,
    -2.789558,  -0.937477, 11.091992, 8.830758,  -3.798772};

constexpr static std::size_t randomData2Size = 54;
constexpr static double randomData2[randomData2Size] = {
    6.063585,  -1.603059, 3.668168,  -3.164358, -5.669303, 6.042537,  -1.827524,
    3.141997,  7.839743,  -0.449503, 11.704780, 4.080705,  6.092508,  1.941723,
    -4.315962, -3.851170, -4.832400, -2.369191, 4.311729,  12.653507, 0.090832,
    -5.663074, 11.635682, -1.027572, -1.115758, 3.978761,  7.596279,  -2.769141,
    4.046201,  -3.727586, 3.798045,  4.005900,  -0.328572, -2.739621, 5.095390,
    4.176589,  6.089753,  5.570235,  6.978266,  2.231976,  -0.563166, 2.612484,
    -2.182849, 6.476901,  5.321058,  3.173620,  1.060784,  7.694968,  -3.347791,
    -8.521936, -2.760514, 6.953309,  2.192015};

struct SliceDesc {
  std::vector<Interval> intervals;
};

bool doTest(const DeviceType &deviceType, const Type &dataType,
            const NonLinearityType &nlType, bool testFwd, bool testBwd,
            bool inPlace) {
  auto device = createTestDevice(deviceType);
  const auto &target = device.getTarget();
  Graph graph(target);
  popnn::addCodelets(graph);

  const auto vectorWidth = target.getVectorWidth(dataType);

  constexpr auto maxRegions = 3;
  // In each inner region of the 2D tensor, test up to at least 3 full
  // 64-bit loops to catch all inner loop paths plus up to 64-bits of
  // trailing elements plus up to 32-bits of leading elements.
  const auto maxElementsPerRegion =
      (vectorWidth * 3) + (vectorWidth + (vectorWidth / 2));
  const auto maxElements = maxElementsPerRegion * maxRegions;
  assert(maxElements <= randomDataSize);
  assert(maxElements <= randomData2Size);

  auto acts = graph.addVariable(dataType, {maxElements});
  auto out = graph.addVariable(dataType, {maxElements});
  auto outgrad = graph.addVariable(dataType, {maxElements});
  auto ingrad = graph.addVariable(dataType, {maxElements});
  graph.setTileMapping(acts, 0);
  graph.setTileMapping(out, 0);
  graph.setTileMapping(outgrad, 0);
  graph.setTileMapping(ingrad, 0);

  // Generate some test data
  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  auto rawHostActsIn = allocateHostMemoryForTensor(
      acts, "test in", graph, uploadProg, downloadProg, tmap);
  auto rawHostOut = allocateHostMemoryForTensor(out, "test out", graph,
                                                uploadProg, downloadProg, tmap);
  auto rawHostGradOut = allocateHostMemoryForTensor(
      outgrad, "test outgrad", graph, uploadProg, downloadProg, tmap);
  auto rawHostGradIn = allocateHostMemoryForTensor(
      ingrad, "test ingrad", graph, uploadProg, downloadProg, tmap);

  boost::multi_array<double, 1> hostActsIn(boost::extents[maxElements]),
      hostGradOut(boost::extents[maxElements]);
  std::copy(&randomData[0], &randomData[maxElements], hostActsIn.data());
  std::copy(&randomData2[0], &randomData2[maxElements], hostGradOut.data());

  boost::multi_array<double, 1> modelActsOut(boost::extents[maxElements]);
  boost::multi_array<double, 1> modelGradIn(boost::extents[maxElements]);

  const auto fwdVertexClass = templateVertex(
      inPlace ? "popnn::NonLinearity2DInPlace" : "popnn::NonLinearity2D",
      dataType, nlType);
  const auto bwdVertexClass =
      templateVertex("popnn::NonLinearityGrad2D", dataType, nlType);

  std::vector<Program> programs;
  std::vector<SliceDesc> programSlices;
  unsigned uid = 0;
  for (std::size_t nRegions = 1; nRegions <= maxRegions; ++nRegions) {
    std::size_t currRegion = 0;
    std::vector<Interval> intervals(nRegions);
    bool filled = false;
    for (std::size_t offset = 0; offset < vectorWidth; ++offset) {
      for (std::size_t n = 1; n < maxElementsPerRegion - offset; ++n) {
        const auto regionOffset = currRegion * maxElementsPerRegion;
        intervals[currRegion] = {regionOffset + offset,
                                 regionOffset + offset + n};
        currRegion = (currRegion + 1) % nRegions;
        filled |= (currRegion == 0);

        // Only test if we've filled all the regions
        if (filled) {
          // Get slices into our variable
          auto actTestSlices = acts.slices(intervals);
          auto outTestSlices = out.slices(intervals);
          auto outgradTestSlices = outgrad.slices(intervals);
          auto ingradTestSlices = ingrad.slices(intervals);
          ComputeSet fwdCS, bwdCS;
          if (testFwd) {
            fwdCS = graph.addComputeSet("cs_fwd_" + std::to_string(uid));
            auto fwdV = graph.addVertex(fwdCS, fwdVertexClass);
            graph.setTileMapping(fwdV, 0);
            graph.connect(fwdV["data"], actTestSlices);
            if (!inPlace) {
              graph.connect(fwdV["out"], outTestSlices);
            }
            if (!testBwd) {
              programs.emplace_back(Sequence{Execute(fwdCS)});
            }
          }
          if (testBwd) {
            bwdCS = graph.addComputeSet("cs_bwd_" + std::to_string(uid));
            auto bwdV = graph.addVertex(bwdCS, bwdVertexClass);
            graph.setTileMapping(bwdV, 0);
            graph.connect(bwdV["out"], actTestSlices);
            graph.connect(bwdV["outGrad"], outgradTestSlices);
            graph.connect(bwdV["inGrad"], ingradTestSlices);
            if (!testFwd) {
              programs.emplace_back(Sequence{Execute(bwdCS)});
            }
          }
          if (testFwd && testBwd) {
            programs.emplace_back(Sequence{Execute(bwdCS), Execute(fwdCS)});
          }
          programSlices.emplace_back(SliceDesc{intervals});
          uid++;
        }
      }
    }
  }
  const auto numTests = programs.size();
  const auto uploadProgIndex = programs.size();
  programs.push_back(uploadProg);
  const auto downloadProgIndex = programs.size();
  programs.push_back(downloadProg);

  // The multiple levels of function calls and loops in the NonLinearity2D
  // vertices manage to overflow the stack sometimes in the C++ codelets at
  // present.
  Engine e(graph, programs);
  attachStreams(e, tmap);

  boost::multi_array<double, 1> hostActsOut(boost::extents[maxElements]),
      hostGradIn(boost::extents[maxElements]);
  const auto relativeTolerance = TOL;
  const auto absoluteTolerance = dataType == FLOAT ? FLOAT_ATOL : HALF_ATOL;

  bool success = true;
  device.bind([&](const Device &d) {
    e.load(d);
    for (std::size_t testId = 0; testId < numTests; ++testId) {
      // Fill out the model data, applying the non-linearity to regions to be
      // processed by the vertex. This allows us to detect over/underwrites
      // by the vertex as a bonus
      std::copy(hostActsIn.data(),
                hostActsIn.data() + hostActsIn.num_elements(),
                modelActsOut.data());
      // fill areas of output that should be untouched by backward vertex with
      // values that should never be output by the vertex
      std::fill_n(modelGradIn.data(), modelGradIn.num_elements(), 50.0);
      for (const auto &region : programSlices[testId].intervals) {
        const auto offset = region.begin();
        const auto n = region.size();
        std::copy(hostGradOut.data() + offset, hostGradOut.data() + offset + n,
                  modelGradIn.data() + offset);
        poplibs_test::bwdNonLinearity(nlType, hostActsIn.data() + offset,
                                      modelGradIn.data() + offset, n);
        poplibs_test::nonLinearity(nlType, hostActsIn.data() + offset,
                                   modelActsOut.data() + offset, n);
      }

      copy(target, hostActsIn, dataType, rawHostActsIn.get());
      copy(target, hostActsIn, dataType, rawHostOut.get());
      copy(target, hostGradOut, dataType, rawHostGradOut.get());
      // fill areas of output that should be untouched by backward vertex with
      // values that should never be output by the vertex
      std::fill_n(hostGradIn.data(), hostGradIn.num_elements(), 50.0);
      copy(target, hostGradIn, dataType, rawHostGradIn.get());
      e.run(uploadProgIndex);
      e.run(testId);
      e.run(downloadProgIndex);
      if (inPlace) {
        copy(target, dataType, rawHostActsIn.get(), hostActsOut);
      } else {
        copy(target, dataType, rawHostOut.get(), hostActsOut);
      }
      copy(target, dataType, rawHostGradIn.get(), hostGradIn);
      if (testFwd) {
        success &= checkIsClose(
            "fwd_" + std::to_string(testId), hostActsOut.data(),
            {hostActsOut.num_elements()}, modelActsOut.data(),
            modelActsOut.num_elements(), relativeTolerance, absoluteTolerance);
      }
      if (testBwd) {
        success &= checkIsClose("bwd_" + std::to_string(testId),
                                hostGradIn.data(), {hostGradIn.num_elements()},
                                modelGradIn.data(), modelGradIn.num_elements(),
                                relativeTolerance, absoluteTolerance);
      }
    }
  });
  return success;
}

} // end anonymous namespace

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType;
  Type dataType;
  NonLinearityType nlType;
  bool testFwd = true;
  bool testBwd = true;
  bool inPlace = true;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("data-type",
     po::value<Type>(&dataType)->required(),
     "Data type for the non-linearity")
    ("test-fwd",
      po::value<bool>(&testFwd)->default_value(testFwd),
     "Test bwd (grad) vertex")
    ("test-bwd",
      po::value<bool>(&testBwd)->default_value(testBwd),
     "Test bwd (grad) vertex")
    ("fwd-inplace",
      po::value<bool>(&inPlace)->default_value(inPlace),
      "Test the fwd in place vertex")
    ("nl-type",
     po::value<NonLinearityType>(&nlType)->required(),
     "Non-linearity type");
  // clang-format on
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }
  if (nlType != NonLinearityType::GELU && nlType != NonLinearityType::SWISH &&
      testFwd) {
    throw poputil::poplibs_error("Fwd non-linearity codelets for TANH, SIGMOID"
                                 " and RELU are implemented as unary ops. ");
  }

  if (!doTest(deviceType, dataType, nlType, testFwd, testBwd, inPlace))
    return 1;
  return 0;
}


/*
 * pheap.hpp
 *
 *  Created on: Jun 19, 2014
 *      Author: yuu
 */

#ifndef PHEAP_HPP_
#define PHEAP_HPP_

// Copyright 2012 Ethan Burns. All rights reserved.
// Use of this source code is governed by an MIT-style
// license that can be found in the LICENSE file.

#include "fatal.hpp"
#include <vector>
#include <limits>
#include <cassert>

//#include <boost/lockfree/queue.hpp>
#include "pqueue.hpp"
//#define DEBUG
#ifdef DEBUG
#define printl()
//#define printl() printf("%d:\n", __LINE__)
#else
#define printl()
#endif

template<class HeapElm> class PHeap {

	struct Maxq {
		Maxq(void) :
				fill(0), max(0) {
//			bins.resize(1);
//			bins[0] = new boost::lockfree::queue<HeapElm*>(1);
		}

		// p corresponds to g value for Astar.
		void push(HeapElm *n, int p) {
			assert(p >= 0);
//			printf("bins.size() = %u\n", bins.size());
			if (bins.size() <= (unsigned int) p) {
				bins.resize(p + 1);
			}
			if (p > max)
				max = p;

			// TODO: AD HOC
//			n->openind = bins[p].size();
			n->openind = bins[p].size();
//			printf("%d: p = %d\n", __LINE__, p);

			bins[p].push(n);

			fill++;
			printl();
		}

		HeapElm *pop(void) {
			for (; bins[max].empty(); max--) {
				if (max == 0) {
					break;
				}
			}
			// TODO: The element needs to be atomically pop

			HeapElm *n;
#ifdef DEBUG
			printf("pop : g = %d\n", max);
#endif
			bins[max].wait_and_pop(n);
//			bins[max]->pop(n);
			// NOT SURE
//			while (n == NULL) {
//				bins[max]->pop(n);
//			}
			n->openind = -1;
			fill--;

			printl();
			return n;
		}

		void rm(HeapElm *n, unsigned long p) {
			assert(p < bins.size());

			std::vector<HeapElm*> &bin = bins[p];

			unsigned int i = n->openind;
			assert(i < bin.size());

			if (bin.size() > 1) {
				bin[i] = bin[bin.size() - 1];
				bin[i]->openind = i;
			}

			bin.pop_back();
			n->openind = -1;
			fill--;
		}

		bool empty() {
			return fill == 0;
		}

		int fill, max;
		std::vector<p_queue<HeapElm*>> bins;
	};

	int fill, min;
	std::vector<Maxq> qs;

public:
	PHeap(unsigned int sz) :
			fill(0), min(0), qs(sz) {
	}

	static const char *kind(void) {
		return "2d bucketed";
	}

	void push(HeapElm *n) {
		int p0 = n->f;

		assert(p0 < qs.size());

		if (p0 < min)
			min = p0;
#ifdef DEBUG
		printf("				Push f = %d, g = %d\n", p0, n->g);
#endif
		qs[p0].push(n, n->g);
		fill++;
	}

	HeapElm *pop(void) {
		while(isempty()) {
#ifdef DEBUG
			printf("empty\n");
#endif
		}
		for (; (unsigned int) min < qs.size() && qs[min].empty(); min++)
			;
		while (min >= 100) {
			printf("lol");
			min = 0;
			for (; (unsigned int) min < qs.size() && qs[min].empty(); min++)
				;
		}
		fill--;
		// TODO: need to check if qs[min] is empty or not
#ifdef DEBUG
		printf("Pop: f = %d, ", min);
#endif
		HeapElm* e = qs[min].pop();
		while (e == NULL) {
			for (; (unsigned int) min < qs.size() && qs[min].empty(); min++)
				;
			e = qs[min].pop();
		}
		return e;
	}

	void pre_update(HeapElm*n) {
		if (n->openind < 0)
			return;
		assert((unsigned int ) n->f < qs.size());
		qs[n->f].rm(n, n->g);
		fill--;
	}

	void post_update(HeapElm *n) {
		assert(n->openind < 0);
		push(n);
	}

	bool isempty(void) {
		return fill == 0;
	}

	bool mem(HeapElm *n) {
		return n->openind >= 0;
	}

	void clear(void) {
		qs.clear();
		min = 0;
	}
};

#endif /* PHEAP_HPP_ */


// Example program showing signals with custom combiners.
//
// Copyright Douglas Gregor 2001-2004.
// Copyright Frank Mori Hess 2009.
//
// 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)
// For more information, see http://www.boost.org

#include <iostream>
#include <boost/signals2/signal.hpp>
#include <vector>

float product(float x, float y) { return x * y; }
float quotient(float x, float y) { return x / y; }
float sum(float x, float y) { return x + y; }
float difference(float x, float y) { return x - y; }

//[ custom_combiners_maximum_def_code_snippet
// combiner which returns the maximum value returned by all slots
template<typename T>
struct maximum
{
  typedef T result_type;

  template<typename InputIterator>
  T operator()(InputIterator first, InputIterator last) const
  {
    // If there are no slots to call, just return the
    // default-constructed value
    if(first == last ) return T();
    T max_value = *first++;
    while (first != last) {
      if (max_value < *first)
        max_value = *first;
      ++first;
    }

    return max_value;
  }
};
//]

void maximum_combiner_example()
{
  // signal which uses our custom "maximum" combiner
  boost::signals2::signal<float (float x, float y), maximum<float> > sig;

//[ custom_combiners_maximum_usage_code_snippet
  sig.connect(&product);
  sig.connect(&quotient);
  sig.connect(&sum);
  sig.connect(&difference);

  // Outputs the maximum value returned by the connected slots, in this case
  // 15 from the product function.
  std::cout << "maximum: " << sig(5, 3) << std::endl;
//]
}

//[ custom_combiners_aggregate_values_def_code_snippet
// aggregate_values is a combiner which places all the values returned
// from slots into a container
template<typename Container>
struct aggregate_values
{
  typedef Container result_type;

  template<typename InputIterator>
  Container operator()(InputIterator first, InputIterator last) const
  {
    Container values;

    while(first != last) {
      values.push_back(*first);
      ++first;
    }
    return values;
  }
};
//]

void aggregate_values_example()
{
  // signal which uses aggregate_values as its combiner
  boost::signals2::signal<float (float, float),
    aggregate_values<std::vector<float> > > sig;

//[ custom_combiners_aggregate_values_usage_code_snippet
  sig.connect(&quotient);
  sig.connect(&product);
  sig.connect(&sum);
  sig.connect(&difference);

  std::vector<float> results = sig(5, 3);
  std::cout << "aggregate values: ";
  std::copy(results.begin(), results.end(),
    std::ostream_iterator<float>(std::cout, " "));
  std::cout << "\n";
//]
}

int main()
{
  maximum_combiner_example();
  aggregate_values_example();
}


/*
 * Copyright 2009-2018 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE aobasis_test
#include <boost/test/unit_test.hpp>
#include <votca/xtp/aobasis.h>
#include <votca/xtp/aomatrix.h>
#include <votca/xtp/orbitals.h>
#include <votca/xtp/convergenceacc.h>
#include <fstream>

using namespace votca::xtp;
using namespace std;

BOOST_AUTO_TEST_SUITE(aobasis_test)

BOOST_AUTO_TEST_CASE(FillNormBasis_test) {
std::ofstream basisfile("notnormalized.xml");
basisfile << "<basis name=\"def2-TZVP\">"<< std::endl;
basisfile << "  <element name=\"Al\">" << std::endl;
basisfile << "    <shell scale=\"1.0\" type=\"D\">"<< std::endl;
basisfile << "      <constant decay=\"1.570000e+00\">"<< std::endl;
basisfile << "        <contractions factor=\"2.000000e-01\" type=\"D\"/>"<<std::endl;
basisfile << "      </constant>"<<std::endl;
basisfile << "      <constant decay=\"3.330000e-01\">"<<std::endl;
basisfile << "        <contractions factor=\"1.000000e+00\" type=\"D\"/>"<<std::endl;
basisfile << "      </constant>"<<std::endl;
basisfile << "    </shell> "<<std::endl;
basisfile << "  </element> "<<std::endl;
basisfile << "</basis> "<<std::endl;
basisfile.close();

std::ofstream xyzfile("Al.xyz");
xyzfile << " 1" << std::endl;
xyzfile << " Al" << std::endl;
xyzfile << " Al            .000000     .000000     .000000" << std::endl;
xyzfile.close();

Orbitals orbitals;
orbitals.LoadFromXYZ("Al.xyz");
BasisSet basis;
basis.LoadBasisSet("notnormalized.xml");
AOBasis aobasis;
aobasis.AOBasisFill(basis,orbitals.QMAtoms());

const AOShell* shell=aobasis.getShell(0);
std::vector<double> ref_results={0.1831079647,0.9155398233};
int i=0;
bool check_norm=true;
for(const AOGaussianPrimitive& gaussian:*shell){
  if(std::abs(ref_results[i]-gaussian.getContraction()[2])>1e-7){
   check_norm=false;
   break;
  }
  i++;
}

i=0;
if(!check_norm){
  for(const AOGaussianPrimitive& gaussian:*shell){
  std::cout<<"Ref:"<<ref_results[i]<<" result:"<<gaussian.getContraction()[2]<<std::endl;
   i++;
  }

}
BOOST_CHECK_EQUAL(check_norm, 1);

}

BOOST_AUTO_TEST_CASE(ReorderMos_test) {

  ofstream xyzfile("molecule.xyz");
  xyzfile << " 5" << endl;
  xyzfile << " methane" << endl;
  xyzfile << " C            .000000     .000000     .000000" << endl;
  xyzfile << " H            .629118     .629118     .629118" << endl;
  xyzfile << " H           -.629118    -.629118     .629118" << endl;
  xyzfile << " H            .629118    -.629118    -.629118" << endl;
  xyzfile << " H           -.629118     .629118    -.629118" << endl;
  xyzfile.close();

  ofstream basisfile("3-21G.xml");
  basisfile <<"<basis name=\"3-21G\">" << endl;
  basisfile << "  <element name=\"H\">" << endl;
  basisfile << "    <shell scale=\"1.0\" type=\"S\">" << endl;
  basisfile << "      <constant decay=\"5.447178e+00\">" << endl;
  basisfile << "        <contractions factor=\"1.562850e-01\" type=\"S\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "      <constant decay=\"8.245470e-01\">" << endl;
  basisfile << "        <contractions factor=\"9.046910e-01\" type=\"S\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "    </shell>" << endl;
  basisfile << "    <shell scale=\"1.0\" type=\"S\">" << endl;
  basisfile << "      <constant decay=\"1.831920e-01\">" << endl;
  basisfile << "        <contractions factor=\"1.000000e+00\" type=\"S\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "    </shell>" << endl;
  basisfile << "  </element>" << endl;
  basisfile << "  <element name=\"C\">" << endl;
  basisfile << "    <shell scale=\"1.0\" type=\"S\">" << endl;
  basisfile << "      <constant decay=\"1.722560e+02\">" << endl;
  basisfile << "        <contractions factor=\"6.176690e-02\" type=\"S\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "      <constant decay=\"2.591090e+01\">" << endl;
  basisfile << "        <contractions factor=\"3.587940e-01\" type=\"S\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "      <constant decay=\"5.533350e+00\">" << endl;
  basisfile << "        <contractions factor=\"7.007130e-01\" type=\"S\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "    </shell>" << endl;
  basisfile << "    <shell scale=\"1.0\" type=\"SP\">" << endl;
  basisfile << "      <constant decay=\"3.664980e+00\">" << endl;
  basisfile << "        <contractions factor=\"-3.958970e-01\" type=\"S\"/>" << endl;
  basisfile << "        <contractions factor=\"2.364600e-01\" type=\"P\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "      <constant decay=\"7.705450e-01\">" << endl;
  basisfile << "        <contractions factor=\"1.215840e+00\" type=\"S\"/>" << endl;
  basisfile << "        <contractions factor=\"8.606190e-01\" type=\"P\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "    </shell>" << endl;
  basisfile << "    <shell scale=\"1.0\" type=\"SP\">" << endl;
  basisfile << "      <constant decay=\"1.958570e-01\">" << endl;
  basisfile << "        <contractions factor=\"1.000000e+00\" type=\"S\"/>" << endl;
  basisfile << "        <contractions factor=\"1.000000e+00\" type=\"P\"/>" << endl;
  basisfile << "      </constant>" << endl;
  basisfile << "    </shell>" << endl;
  basisfile << "    <shell scale=\"1.0\" type=\"D\">"<< std::endl;
  basisfile << "      <constant decay=\"1.570000e+00\">"<< std::endl;
  basisfile << "        <contractions factor=\"2.000000e-01\" type=\"D\"/>"<<std::endl;
  basisfile << "      </constant>"<<std::endl;
  basisfile << "    </shell> "<<std::endl;
  basisfile << "  </element>" << endl;
  basisfile << "</basis>" << endl;
  basisfile.close();

  Orbitals orbitals;
  orbitals.LoadFromXYZ("molecule.xyz");
  BasisSet basis;
  basis.LoadBasisSet("3-21G.xml");
  AOBasis aobasis;
  aobasis.AOBasisFill(basis,orbitals.QMAtoms());
  AOOverlap overlap;
  overlap.Fill(aobasis);

  AOKinetic kinetic;
  kinetic.Fill(aobasis);
  AOESP esp;
  esp.Fillnucpotential(aobasis,orbitals.QMAtoms());
  Eigen::MatrixXd H=kinetic.Matrix()+esp.getNuclearpotential();

  double levelshift=0.1;
  ConvergenceAcc d;
  Orbitals orb;
  int occlevels=5;
  d.Configure(ConvergenceAcc::closed,false,false,10,false,0,0,levelshift,0,2*occlevels,0);
  d.setOverlap(&overlap,1e-8);
  d.SolveFockmatrix(orb.MOEnergies(),orb.MOCoefficients(),H);

  Eigen::MatrixXd ref=orb.MOCoefficients();
  aobasis.ReorderMOs(orb.MOCoefficients(),"xtp","gaussian");
  aobasis.ReorderMOs(orb.MOCoefficients(),"gaussian","xtp");

  Eigen::MatrixXd overlap_b=overlap.Matrix();
  aobasis.ReorderMatrix(overlap_b,"xtp","nwchem");
  aobasis.ReorderMatrix(overlap_b,"nwchem","xtp");
  bool check_reorder_overlap=overlap_b.isApprox(overlap.Matrix(),1e-7);
  BOOST_CHECK_EQUAL(check_reorder_overlap, 1);

  bool check_reorder_gaus=ref.isApprox(orb.MOCoefficients(),1e-7);
  if(!check_reorder_gaus){
    std::cout<<"ref"<<std::endl;
    std::cout<<ref<<std::endl;
    std::cout<<"reordered"<<std::endl;
    std::cout<<orb.MOCoefficients()<<std::endl;
  }

  BOOST_CHECK_EQUAL(check_reorder_gaus, 1);

  aobasis.ReorderMOs(orb.MOCoefficients(),"xtp","nwchem");
  aobasis.ReorderMOs(orb.MOCoefficients(),"nwchem","xtp");

   bool check_reorder_nwchem=ref.isApprox(orb.MOCoefficients(),1e-7);

  BOOST_CHECK_EQUAL(check_reorder_nwchem, 1);
}

BOOST_AUTO_TEST_SUITE_END()


#include "EVTester.hpp"
#include "EVMath.hpp"
#include "processor.hpp"
#include "trigger.hpp"
#include "postProcessor.hpp"
#include "bridge.hpp"
#include "dspPipeline.hpp"
#include "PCIe.hpp"
#include <boost/tokenizer.hpp>
#include <fstream>
#include <iostream>
#include "controller.hpp"

uint32_t testSize = 1000;

Bridge* bridgeThread_1;

void loadFromRand (boost::lockfree::queue<buffer*, boost::lockfree::fixed_sized<false>> *outputQ)
{
    // Create dummy buffer
    buffer* tempBuffer;
    std::srand(std::time(0));
    for (uint32_t i = 0; i < testSize; i++) {
        // initialize buffer
        tempBuffer = bufferAllocator.allocate(1);
        bufferAllocator.construct(tempBuffer);

        // fill buffer with random data
        for (uint32_t j = 0; j < BUFFER_SIZE; j++) {
            tempBuffer->data[j] = std::rand();
        }

        // Push onto queue
        outputQ->push(tempBuffer);
    }
}

void TestSincInterpolation()
{
    int numInterpolatedPoints;
    int numOriginalPoints = 300;
    DataPoint* testPoints = (DataPoint*)malloc(numOriginalPoints * sizeof(DataPoint));
    std::ofstream myFile;

    for(int i = 0; i < numOriginalPoints; i++) {
        testPoints[i].value = sin(double(10 * i) * 0.05) + (2 * sin(double(3 * i) * 0.05));
        testPoints[i].time = double(10 * i) * 0.05;
    }

    DataPoint* points = SincInterpolate(testPoints, numOriginalPoints, &numInterpolatedPoints, 3, 20);

    myFile.open("InterpolatedPoints.csv");
    for(int i = 0; i < numInterpolatedPoints; i++) {
        myFile << points[i].time;
        myFile << ",";
        myFile << points[i].value;
        myFile << "\n";
   }
    myFile.close();

    myFile.open("OriginalValues.csv");
    for(int i = 0; i < numOriginalPoints; i++) {
        myFile << testPoints[i].time;
        myFile << ",";
        myFile << testPoints[i].value;
        myFile << "\n";
    }

    myFile.close();
}

void testTriggerThroughput()
{
    uint32_t testSize = 1000;

    // Create dummy queue
    boost::lockfree::queue<buffer*, boost::lockfree::fixed_sized<false>> newDataQueue{1000};

    // Create dummy buffer
    buffer* tempBuffer;
    std::srand(std::time(0));
    for (uint32_t i = 0; i < testSize; i++) {
        // initialize buffer
        tempBuffer = bufferAllocator.allocate(1);
        bufferAllocator.construct(tempBuffer);

        // fill buffer with random data
        for (uint32_t j = 0; j < BUFFER_SIZE; j++) {
            tempBuffer->data[j] = std::rand();
        }

        // Push onto queue
        newDataQueue.push(tempBuffer);
    }

    // Create trigger method
    Trigger trigger(&newDataQueue, &newDataQueue, 127);

    // Create pointer to current buffer
    buffer *currentBuffer;

    // Take Timestamp
    auto start = std::chrono::high_resolution_clock::now();

    // Run loop
    while (newDataQueue.pop(currentBuffer)) {
        trigger.checkTriggerRising(currentBuffer);
    }

    // Take Timestamp
    auto end = std::chrono::high_resolution_clock::now();

    auto timeElapsed = end - start;

    uint32_t bytesTriggered = testSize * BUFFER_SIZE;
    double triggeredBps = ((double)bytesTriggered * S_TO_NS
                            / timeElapsed.count());
    double triggeredGBps = (((double)bytesTriggered * S_TO_NS)
                            / (timeElapsed.count()
                            * GIB_TO_GB));

    INFO << "Time Elapsed Triggering: " << timeElapsed.count() << " ns";
    INFO << "Triggered B: " << bytesTriggered << " B";
    INFO << "Triggered B/s: " << triggeredBps;
    INFO << "Triggered GiB/s: " << triggeredGBps;
}

/*******************************************************************************
 * testCsv()
 *
 * Runs csv data through the pipeline
 *
 * Arguments:
 *   char* filename - name of source csv file to put into pipeline
 *
 * Return: void
 ******************************************************************************/
//void testCsv(char * filename)
//{
//    boost::lockfree::queue<buffer*, boost::lockfree::fixed_sized<false>> newDataQueue{1000};
//    boost::lockfree::queue<EVPacket*, boost::lockfree::fixed_sized<false>> cmdQueue{1000};
//    boost::lockfree::queue<EVPacket*, boost::lockfree::fixed_sized<false>> bridge_rx{1000};
//    loadFromFile(filename, &newDataQueue);
//
//    bridgeThread_1 = new Bridge("testPipe", &bridge_rx, &cmdQueue);
//
//    dspThread1 = new dspPipeline(&newDataQueue);
//
//    // start transfering to js
//    bridgeThread_1->TxStart();
//    bridgeThread_1->RxStart();
//
//	// Wait to recieve all messages back
//    INFO << "Start node application now";
//
//    dspThread1->dspPipelineUnPause();
//
//    std::this_thread::sleep_for(std::chrono::seconds(10));
//
//    INFO << "Test is done. Performing Cleanup";
//
//    delete bridgeThread_1;
//    delete dspThread1;
//
//}

/*******************************************************************************
 * runSocketTest()
 *
 * Creats a bridge, sends a test packet across the bridge and cleans up after
 * recieving a response.
 *
 * Arguments:
 *   None
 * Return:
 *   None
 ******************************************************************************/
void runSocketTest ()
{
    boost::lockfree::queue<EVPacket*, boost::lockfree::fixed_sized<false>> cmdQueue{1000};
    boost::lockfree::queue<EVPacket*, boost::lockfree::fixed_sized<false>> bridge_rx{1000};
    char in[10] = {};

    // Create packet
    EVPacket* testPacket = (EVPacket*)malloc(sizeof(EVPacket));
    testPacket->command = 1;
    testPacket->packetID = 0x0808;
    testPacket->dataSize = 5;
    testPacket->data = (int8_t*)malloc(5);
    testPacket->data[0] = 1;
    testPacket->data[1] = 2;
    testPacket->data[2] = 3;
    testPacket->data[3] = 4;
    testPacket->data[4] = 5;

    // Pass packet to tx queue
    Bridge* bridgeThread_1 = new Bridge("testPipe", &bridge_rx, &cmdQueue);
    bridge_rx.push(testPacket);

    // start transfering
    bridgeThread_1->TxStart();
    bridgeThread_1->RxStart();

    std::cin >> in;

    delete bridgeThread_1;
}

boost::lockfree::queue<buffer*, boost::lockfree::fixed_sized<false>> testerDataQueue{1000};

/*******************************************************************************
 * runPCIeTEST()
 *
 * Attempts to connect to the Xilinx FPGA through PCIe, and attempts to write and read 8Mbytes
 *
 * Arguments:
 *   None
 * Return:
 *   None
 ******************************************************************************/
void runPCIeTest() {

    controller* troller = new controller(&testerDataQueue);
    troller->setWindowSize(1000);
    troller->testADCData();
    delete troller;
}


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

  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/round.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>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/maxflint.hpp>
#include <boost/simd/function/next.hpp>
#include <boost/simd/function/prev.hpp>

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

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

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(round(bs::Inf<T>()), bs::Inf<r_t>());
  STF_EQUAL(round(bs::Minf<T>()), bs::Minf<r_t>());
  STF_IEEE_EQUAL(round(bs::Nan<T>()), bs::Nan<r_t>());
#endif
  STF_EQUAL(round(bs::Mhalf<T>()), bs::Mone<r_t>());
  STF_EQUAL(round(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(round(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(round(bs::Zero<T>()), bs::Zero<r_t>());
  STF_EQUAL(round(bs::Maxflint<T>()-bs::Half<T>()),bs::Maxflint<T>());
  STF_EQUAL(round(bs::Maxflint<T>()),bs::Maxflint<T>());
  STF_EQUAL(round(T(1.4)), T(1));
  STF_EQUAL(round(T(1.5)), T(2));
  STF_EQUAL(round(T(1.6)), T(2));
  STF_EQUAL(round(T(2.5)), T(3));
  STF_EQUAL(round(T(-1.4)), T(-1));
  STF_EQUAL(round(T(-1.5)), T(-2));
  STF_EQUAL(round(T(-1.6)), T(-2));
  STF_EQUAL(round(T(-2.5)), T(-3));
  STF_EQUAL(round(bs::Half<T>()), bs::One<r_t>());
  STF_EQUAL(round(prev(prev(bs::Half<T>()))),  bs::Zero<T>());
  STF_EQUAL(round(prev(bs::Half<T>())),  bs::Zero<T>());
  STF_EQUAL(round(     bs::Half<T>()) ,  bs::One <T>());
  STF_EQUAL(round(next(bs::Half<T>())),  bs::One <T>());
  T z = bs::Maxflint < T>()/T(2)+T(1);
  STF_EQUAL(round(z), z);

} // end of test for floating_

STF_CASE_TPL (" round real2",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::round;
  using bs::next;
  using bs::prev;
  using r_t = decltype(round(T()));

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

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(round(bs::Inf<T>(), 2), bs::Inf<r_t>());
  STF_EQUAL(round(bs::Minf<T>(), 2), bs::Minf<r_t>());
  STF_IEEE_EQUAL(round(bs::Nan<T>(), 2), bs::Nan<r_t>());
#endif
  STF_EQUAL(round(bs::Mhalf<T>(), 2), bs::Mhalf<r_t>());
  STF_EQUAL(round(bs::Mone<T>(), 2), bs::Mone<r_t>());
  STF_EQUAL(round(bs::One<T>(), 2), bs::One<r_t>());
  STF_EQUAL(round(bs::Zero<T>(), 2), bs::Zero<r_t>());
  STF_EQUAL(round(bs::Maxflint<T>()-bs::Half<T>(), 2),bs::Maxflint<T>());
  STF_EQUAL(round(bs::Maxflint<T>(), 2),bs::Maxflint<T>());
  STF_EQUAL(round(T(1.44), 1), T(1.4));
  STF_EQUAL(round(T(1.45), 1), T(1.5));
  STF_EQUAL(round(T(1.46), 1), T(1.5));
  STF_EQUAL(round(T(2.45), 1), T(2.5));
  STF_EQUAL(round(T(-1.44), 1), T(-1.4));
  STF_EQUAL(round(T(-1.45), 1), T(-1.5));
  STF_EQUAL(round(T(-1.46), 1), T(-1.5));
  STF_EQUAL(round(T(-2.45), 1), T(-2.5));
  STF_ULP_EQUAL(round(T(145), -2), T(100), 0.5);
  STF_ULP_EQUAL(round(T(150), -2), T(200), 0.5);
  STF_ULP_EQUAL(round(T(156), -2), T(200), 0.5);
  STF_ULP_EQUAL(round(T(250), -2), T(300), 0.5);
  STF_ULP_EQUAL(round(T(-145), -2), T(-100), 0.5);
  STF_ULP_EQUAL(round(T(-155), -2), T(-200), 0.5);
  STF_ULP_EQUAL(round(T(-156), -2), T(-200), 0.5);
  STF_ULP_EQUAL(round(T(-255), -2), T(-300), 0.5);
} // end of test for floating_

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

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

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

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

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

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


#define BOOST_TEST_MODULE lue framework core shape
#include <boost/test/unit_test.hpp>
#include "lue/framework/core/shape.hpp"

namespace {

using Index = std::uint32_t;

template<
    lue::Rank rank>
using Shape = lue::Shape<Index, rank>;

}  // Anonymous namespace

BOOST_AUTO_TEST_CASE(construct_0d)
{
    {
        Shape<0> shape{};
        BOOST_CHECK_EQUAL(shape.size(), 0);
    }
}

BOOST_AUTO_TEST_CASE(construct_2d)
{
    {
        Shape<2> shape{};
        BOOST_CHECK_EQUAL(shape.size(), 2);
        BOOST_CHECK_EQUAL(shape[0], Index{0});
        BOOST_CHECK_EQUAL(shape[1], Index{0});
    }

    {
        Shape<2> shape{30, 40};
        BOOST_CHECK_EQUAL(shape.size(), 2);
        BOOST_CHECK_EQUAL(shape[0], Index{30});
        BOOST_CHECK_EQUAL(shape[1], Index{40});
    }
}

BOOST_AUTO_TEST_CASE(nr_elements)
{
    {
        Shape<0> shape{};
        BOOST_CHECK_EQUAL(lue::nr_elements(shape), 0);
    }

    {
        Shape<2> shape{};
        BOOST_CHECK_EQUAL(lue::nr_elements(shape), 0);
    }

    {
        Shape<2> shape{30, 40};
        BOOST_CHECK_EQUAL(lue::nr_elements(shape), 1200);
    }

    {
        Shape<2> shape{57926, 57926};
        BOOST_CHECK_EQUAL(lue::nr_elements(shape), 3355421476);
    }
}