#include "../sweepline_state.hpp"

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

#include <algorithm>

BOOST_AUTO_TEST_SUITE(sweepline_tests)

BOOST_AUTO_TEST_CASE(zigzag_test)
{
    //
    //     1     3     5
    //    / \ b / \ d /
    //   / a \ / c \ /
    //  0     2     4
    //
    std::vector<coordinate> coords {
        coordinate {0, 0}, coordinate {1, 1}, coordinate {2, 0}, coordinate {3, 1}, coordinate {4, 0}, coordinate {5, 1}
    };
    std::vector<coordinate> points {
        coordinate {1, 0.25}, coordinate {2, 0.75}, coordinate {3, 0.25}, coordinate {4, 0.75}
    };

    sweepline_state state(coords, 0);

    state.move_sweepline(coords[1]);

    state.insert_edge({0, 1});
    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 1);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {0, 1}));

    state.insert_edge({1, 2});
    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 2);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {0, 1}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {1, 2}));

    state.move_sweepline(coords[3]);

    state.insert_edge({2, 3});
    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 3);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {0, 1}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {1, 2}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[2], (sweepline_state::edge {2, 3}));

    state.insert_edge({3, 4});
    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 4);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {0, 1}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {1, 2}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[2], (sweepline_state::edge {2, 3}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[3], (sweepline_state::edge {3, 4}));

    state.move_sweepline(coords[5]);

    state.insert_edge({4, 5});
    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 5);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {0, 1}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {1, 2}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[2], (sweepline_state::edge {2, 3}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[3], (sweepline_state::edge {3, 4}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[4], (sweepline_state::edge {4, 5}));

    state.move_sweepline(points[0]);
    auto intersect_0 = state.get_first_intersecting(points[0]);
    BOOST_CHECK(intersect_0 != state.intersecting_edges.end());
    BOOST_CHECK_EQUAL(*intersect_0, (sweepline_state::edge {1, 2}));

    state.move_sweepline(points[1]);
    auto intersect_1 = state.get_first_intersecting(points[1]);
    BOOST_CHECK(intersect_1 != state.intersecting_edges.end());
    BOOST_CHECK_EQUAL(*intersect_1, (sweepline_state::edge {2, 3}));

    state.move_sweepline(points[2]);
    auto intersect_2 = state.get_first_intersecting(points[2]);
    BOOST_CHECK(intersect_2 != state.intersecting_edges.end());
    BOOST_CHECK_EQUAL(*intersect_2, (sweepline_state::edge {3, 4}));

    state.move_sweepline(points[3]);
    auto intersect_3 = state.get_first_intersecting(points[3]);
    BOOST_CHECK(intersect_3 != state.intersecting_edges.end());
    BOOST_CHECK_EQUAL(*intersect_3, (sweepline_state::edge {4, 5}));
}

BOOST_AUTO_TEST_CASE(insert_test)
{
    // simulate a rotating sweepline on the following line
    // vertices are inserted in the order given
    //                    1
    //    0              /
    //   / \            /
    //  / a \          /  b
    // x     2  3     /
    //       | / \ c /
    //       |/   \ /
    //       4     5
    std::vector<coordinate> coords {
        coordinate {0, 0},  // 0
        coordinate {1, 1},  // 1
        coordinate {2, 0},  // 2
        coordinate {2, -1}, // 3
        coordinate {3, 0},  // 4
        coordinate {4, -1}, // 5
        coordinate {5, 2}   // 6
    };

    coordinate a {0.5, 0.5};
    coordinate b {5, 0.5};
    coordinate c {3.75, -0.5};
    coordinate point_on_line {3.5, -0.5};

    sweepline_state state(coords, 0);

    // process 1
    state.move_sweepline(coords[1]);
    state.insert_edge(sweepline_state::edge {1, 2});

    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 1);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {1, 2}));

    // process a
    state.move_sweepline(a);
    BOOST_CHECK(state.get_first_intersecting(a) != state.intersecting_edges.end());
    BOOST_CHECK_EQUAL(*state.get_first_intersecting(a), (sweepline_state::edge {1, 2}));

    // process 6
    state.move_sweepline(coords[6]);
    state.insert_edge(sweepline_state::edge {5, 6});

    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 2);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {1, 2}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {5, 6}));

    // process b
    state.move_sweepline(b);
    BOOST_CHECK(state.get_first_intersecting(b) == state.intersecting_edges.end());

    // process 2
    state.move_sweepline(coords[2]);
    state.remove_edge(sweepline_state::edge {1, 2});
    state.insert_edge(sweepline_state::edge {2, 3});

    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 2);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {2, 3}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {5, 6}));

    // process 4
    state.move_sweepline(coords[4]);
    state.insert_edge(sweepline_state::edge {3, 4});
    state.insert_edge(sweepline_state::edge {4, 5});

    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 4);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {2, 3}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {3, 4}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[2], (sweepline_state::edge {4, 5}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[3], (sweepline_state::edge {5, 6}));

    // process c
    state.move_sweepline(c);
    BOOST_CHECK(state.get_first_intersecting(c) != state.intersecting_edges.end());
    BOOST_CHECK_EQUAL(*state.get_first_intersecting(c), (sweepline_state::edge {5, 6}));

    // FIXME This fails because we assume that points don't lie on lines
    // BOOST_CHECK(state.get_first_intersecting(point_on_line) != state.intersecting_edges.end());
    // BOOST_CHECK_EQUAL(*state.get_first_intersecting(point_on_line), (sweepline_state::edge {5, 6}));

    // process 3
    state.move_sweepline(coords[3]);
    state.remove_edge(sweepline_state::edge {3, 4});
    state.remove_edge(sweepline_state::edge {2, 3});

    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 2);
    BOOST_CHECK_EQUAL(state.intersecting_edges[0], (sweepline_state::edge {4, 5}));
    BOOST_CHECK_EQUAL(state.intersecting_edges[1], (sweepline_state::edge {5, 6}));

    // process 5
    state.move_sweepline(coords[5]);
    state.remove_edge(sweepline_state::edge {5, 6});
    state.remove_edge(sweepline_state::edge {4, 5});

    BOOST_CHECK_EQUAL(state.intersecting_edges.size(), 0);
}

BOOST_AUTO_TEST_CASE(horizontal_segment)
{

    //              a
    //         2--------3
    //         |b
    //         |
    //  0------1
    //
    std::vector<coordinate> coords { coordinate {0, 0}, coordinate {1, 0}, coordinate {1, 1}, coordinate {2, 1} };

    coordinate a {1.5, 1.25};
    coordinate b {1.1, 0.9};

    sweepline_state state(coords, 0);

    state.move_sweepline(coords[2]);
    state.insert_edge(sweepline_state::edge {2, 3});
    state.insert_edge(sweepline_state::edge {1, 2});

    state.move_sweepline(a);
    BOOST_CHECK(state.get_first_intersecting(a) == state.intersecting_edges.end());

    state.move_sweepline(b);
    BOOST_CHECK(state.get_first_intersecting(b) != state.intersecting_edges.end());
    BOOST_CHECK_EQUAL(*state.get_first_intersecting(b), (sweepline_state::edge {2, 3}));

    state.move_sweepline(coords[3]);
    state.remove_edge(sweepline_state::edge {2, 3});

    state.move_sweepline(coords[1]);
    state.remove_edge(sweepline_state::edge {1, 2});
    state.insert_edge(sweepline_state::edge {0, 1});
}

BOOST_AUTO_TEST_SUITE_END()


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Shikhar Vashistha
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_LQ_INCLUDE
#define MTL_MATRIX_LQ_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {
    namespace mat {

        /// LQ-Factorization of matrix A(m x n)
        /** Return pair L lower triangle matrix and Q= orthogonal matrix. L and Q are always dense2D **/
        template <typename Matrix, typename MatrixQ, typename MatrixR>
        void lq(const Matrix& A, MatrixQ& Q, MatrixR& R)
        {
            vampir_trace<4013> tracer;
            typedef typename Collection<Matrix>::value_type   		    value_type;
            typedef typename Collection<Matrix>::size_type    		    size_type;
            typedef typename Magnitude<value_type>::type      		    magnitude_type;
            typedef mtl::dense_vector<value_type, vec::parameters<> >       vector_type;

            size_type        ncols = num_cols(trans(A)), nrows = num_rows(trans(A)),
                mini = ncols == nrows ? ncols - 1 : (nrows >= ncols ? ncols : nrows);
            magnitude_type   factor = magnitude_type(2);

            Q = 1;
            for (size_type i = 0; i < mini; i++) {
                irange r(i, imax); // Intervals [i, n-1]
                vector_type   w(R[r][i]), v(householder_s(w));

                // R-= 2*v*(v'*R)
                // L will be same if we find R of transpose of A
                MatrixR Rsub(R[r][r]);
                vector_type tmp(-factor * trans(Rsub) * v);
                rank_one_update(Rsub, v, tmp);

                //update Q: Q-= 2*(v*Q)*v'
                MatrixQ Qsub(Q[iall][r]);
                vector_type qtmp(-factor * Qsub * v);
                rank_one_update(Qsub, qtmp, v);
            } //end for
        }

        /// QR-Factorization of matrix A(m x n)
        template <typename Matrix>
        std::pair<mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> >,
            mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> > >
            inline lq(const Matrix& A)
        {
            mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> >  R(A), Q(num_rows(trans(A)), num_rows(trans(A)));
            lq(trans(A), Q, R);
            return std::make_pair(Q, R);
        }

        // LQ-Factorization of matrix A
        // Return Q and L with A = L*Q   L lower triangle and Q othogonal
        template <typename Matrix>
        std::pair<typename mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> >,
            typename mtl::mat::dense2D<typename Collection<Matrix>::value_type, mat::parameters<> > >
            inline lq_factors(const Matrix& A)
        {
            vampir_trace<4014> tracer;
            using std::abs;
            typedef typename Collection<Matrix>::value_type   value_type;
            // typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
            typedef typename Collection<Matrix>::size_type    size_type;
            size_type        ncols = num_cols(trans(A)), nrows = num_rows(trans(A));
            value_type       zero = math::zero(A[0][0]), one = math::one(A[0][0]);

            //evaluation of Q
            Matrix  Q(nrows, nrows), Qk(nrows, nrows), HEL(nrows, ncols), R(nrows, ncols), R_tmp(nrows, ncols);
            Q = one; R = zero; HEL = zero;

            boost::tie(Q, R_tmp) = lq(trans(A));
            R = lower(R_tmp);
			//R will be a lower triangular matrix in lq decomposition
            return std::make_pair(Q, R);
        }

    }
} // namespace mtl::matrix

#endif // MTL_MATRIX_QR_INCLUDE


/******************************************************************************\
* 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 "error-model.h"

// Standard C++ library headers:
#include <cmath>
#include <functional>

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
using std::exp;
using std::function;
using std::log;
const double TOL = 0.000000001;
}  // namespace

BOOST_AUTO_TEST_SUITE(common_suite)
BOOST_AUTO_TEST_SUITE(error_model_suite)

BOOST_AUTO_TEST_CASE(constructor_test) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    BOOST_TEST(em.p_edman_failure == p_edman_failure);
    BOOST_TEST(em.p_detach == p_detach);
    BOOST_TEST(em.p_bleach == p_bleach);
    BOOST_TEST(em.p_dud == p_dud);
    BOOST_TEST(em.distribution_type == dist_type);
    BOOST_TEST(em.mu == mu);
    BOOST_TEST(em.sigma == sigma);
    BOOST_TEST(em.stuck_dye_ratio == stuck_dye_ratio);
    BOOST_TEST(em.p_stuck_dye_loss == p_stuck_dye_loss);
}

BOOST_AUTO_TEST_CASE(pdf_lognormal_state_zero_obs_zero_test) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 0.0;
    int state = 0;
    BOOST_TEST(pdf(observed, state) == 1.0);
}

BOOST_AUTO_TEST_CASE(pdf_lognormal_state_zero_obs_one_test) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 1.0;
    int state = 0;
    BOOST_TEST(pdf(observed, state) == 0.0);
}

BOOST_AUTO_TEST_CASE(pdf_lognormal_state_one_obs_zero_test) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 0.0;
    int state = 1;
    BOOST_TEST(pdf(observed, state) == 0.0);
}

BOOST_AUTO_TEST_CASE(pdf_lognormal_state_one_obs_one_test, *tolerance(TOL)) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 1.0;
    int state = 1;
    // The test value was found using an online lognormal distribution pdf
    // calculator.
    BOOST_TEST(pdf(observed, state) == 2.4933892525089547);
}

BOOST_AUTO_TEST_CASE(pdf_lognormal_state_eq_obs_ne_one_test, *tolerance(TOL)) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.3);
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 1.3;
    int state = 1;
    // The test value was found using an online lognormal distribution pdf
    // calculator.
    BOOST_TEST(pdf(observed, state) == 2.4933892525089547 / 1.3);
}

BOOST_AUTO_TEST_CASE(pdf_override_state_zero_obs_zero_test) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::OVERRIDE;
    double mu = 1.0;
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 0.0;
    int state = 0;
    BOOST_TEST(pdf(observed, state) == 1.0);
}

BOOST_AUTO_TEST_CASE(pdf_override_state_zero_obs_one_test) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::OVERRIDE;
    double mu = 1.0;
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 1.0;
    int state = 0;
    BOOST_TEST(pdf(observed, state) == 1.0);
}

BOOST_AUTO_TEST_CASE(pdf_override_state_one_obs_zero_test) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::OVERRIDE;
    double mu = 1.0;
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 0.0;
    int state = 1;
    BOOST_TEST(pdf(observed, state) == 1.0);
}

BOOST_AUTO_TEST_CASE(pdf_override_state_one_obs_one_test, *tolerance(TOL)) {
    double p_edman_failure = .07;
    double p_detach = .04;
    double p_bleach = .05;
    double p_dud = .10;
    DistributionType dist_type = DistributionType::OVERRIDE;
    double mu = 1.0;
    double sigma = .16;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    function<double(double, int)> pdf = em.pdf();
    double observed = 1.0;
    int state = 1;
    BOOST_TEST(pdf(observed, state) == 1.0);
}

BOOST_AUTO_TEST_CASE(relative_distance_p_edman_failure_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.66,
                   0.5,
                   0.5,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.5,
                   0.5,
                   0.5,
                   0.5);
    BOOST_TEST(em1.relative_distance(em2) == (0.66 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.66 - 0.5) / 0.66);
}

BOOST_AUTO_TEST_CASE(relative_distance_p_detach_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.5,
                   0.66,
                   0.5,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.5,
                   0.5,
                   0.5,
                   0.5);
    BOOST_TEST(em1.relative_distance(em2) == (0.66 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.66 - 0.5) / 0.66);
}

BOOST_AUTO_TEST_CASE(relative_distance_p_bleach_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.5,
                   0.5,
                   0.66,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.5,
                   0.5,
                   0.5,
                   0.5);
    BOOST_TEST(em1.relative_distance(em2) == (0.66 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.66 - 0.5) / 0.66);
}

BOOST_AUTO_TEST_CASE(relative_distance_p_dud_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.5,
                   0.5,
                   0.5,
                   0.66,
                   DistributionType::OVERRIDE,
                   0.5,
                   0.5,
                   0.5,
                   0.5);
    BOOST_TEST(em1.relative_distance(em2) == (0.66 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.66 - 0.5) / 0.66);
}

BOOST_AUTO_TEST_CASE(relative_distance_mu_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.5,
                   0.5,
                   0.5,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.66,
                   0.5,
                   0.5,
                   0.5);
    BOOST_TEST(em1.relative_distance(em2) == (exp(0.66) - exp(0.5)) / exp(0.5));
    BOOST_TEST(em2.relative_distance(em1)
               == (exp(0.66) - exp(0.5)) / exp(0.66));
}

BOOST_AUTO_TEST_CASE(relative_distance_sigma_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.5,
                   0.5,
                   0.5,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.5,
                   0.66,
                   0.5,
                   0.5);
    BOOST_TEST(em1.relative_distance(em2) == (0.66 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.66 - 0.5) / 0.66);
}

BOOST_AUTO_TEST_CASE(relative_distance_stuck_dye_ratio_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.5,
                   0.5,
                   0.5,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.5,
                   0.5,
                   0.66,
                   0.5);
    BOOST_TEST(em1.relative_distance(em2) == (0.66 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.66 - 0.5) / 0.66);
}

BOOST_AUTO_TEST_CASE(relative_distance_p_stuck_dye_loss_test, *tolerance(TOL)) {
    ErrorModel em1(
            0.5, 0.5, 0.5, 0.5, DistributionType::OVERRIDE, 0.5, 0.5, 0.5, 0.5);
    ErrorModel em2(0.5,
                   0.5,
                   0.5,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.5,
                   0.5,
                   0.5,
                   0.66);
    BOOST_TEST(em1.relative_distance(em2) == (0.66 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.66 - 0.5) / 0.66);
}

BOOST_AUTO_TEST_CASE(relative_distance_max_no_sum_test, *tolerance(TOL)) {
    ErrorModel em1(0.5,
                   0.5,
                   0.5,
                   0.5,
                   DistributionType::OVERRIDE,
                   0.66,
                   0.5,
                   0.5,
                   0.5);
    ErrorModel em2(0.7,
                   0.7,
                   0.7,
                   0.7,
                   DistributionType::OVERRIDE,
                   0.66,
                   0.7,
                   0.7,
                   0.7);
    BOOST_TEST(em1.relative_distance(em2) == (0.7 - 0.5) / 0.5);
    BOOST_TEST(em2.relative_distance(em1) == (0.7 - 0.5) / 0.7);
}

BOOST_AUTO_TEST_SUITE_END()  // error_model_suite
BOOST_AUTO_TEST_SUITE_END()  // common_suite

}  // namespace whatprot


#include <math.h>
#include <EigenUnsupported/Eigen/KroneckerProduct>
#include "Core/Utilities/QProgInfo/QCircuitInfo.h"
#include "Core/Utilities/Tools/MatrixDecomposition.h"
USING_QPANDA
using namespace std;

using MatrixSequence = std::vector<MatrixUnit>;
using DecomposeEntry = std::pair<int, MatrixSequence>;

using ColumnOperator = std::vector<DecomposeEntry>;
using MatrixOperator = std::vector<ColumnOperator>;

using SingleGateUnit = std::pair<MatrixSequence, QStat>;

static void upper_partition(int order, MatrixOperator &entries)
{
	auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

	for (auto cdx = 0; cdx < order - 1; ++cdx)
	{
		for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
		{
			auto entry = entries[cdx][rdx];

			entry.first += order;
			entry.second[index] = MatrixUnit::SINGLE_P1;

			entries[cdx + order].emplace_back(entry);
		}
	}

    return;
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int lj = ((cdx - 1) >> (udx - 1)) & 1;

	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if 1 ≤ j ≤ m and cj = lj' = 1 , return true
	auto mat = units[units.size() - udx];
	return udx >= 1
		&& udx <= M
		&& lj
		&& mat == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if j = n and none of cn...cm+1 is 1 , return true
	if (units.size() != udx)
	{
		return false;
	}
	else
	{
		auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
		return (units.end() - M) == iter;
	}
}

static void under_partition(int order, MatrixOperator& entries)
{
	auto qubits = (int)std::log2(entries.size() + 1);

	for (auto cdx = 1; cdx < order; ++cdx)
	{
		if (cdx & 1)
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto value = entries[0][rdx + order - 1].first ^ cdx;
				auto entry = make_pair(value, entries[cdx - 1][rdx + order - cdx].second);

				entries[cdx].emplace_back(entry);
			}

			auto &units = entries[cdx].back().second;
			for (auto idx = 0; idx < (int)std::log2(order); ++idx)
			{
				units[qubits - idx - 1] = ((cdx >> idx) & 1) ?
					MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;
			}
		}
		else
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto range = (int)std::log2(order) + 1;
				auto refer = entries[0][rdx + order - 1].second;
				auto entry = entries[0][rdx + order - 1].first ^ cdx;

				MatrixSequence units(refer.begin() + qubits - range, refer.end());

				for (auto udx = 1; udx <= range; ++udx)  /*udx = j , cdx = L*/
				{
					bool steps_accord = steps_requirement(units, udx, cdx + 1);
					bool entry_accord = entry_requirement(units, udx, cdx + 1);

					units[range - udx] = steps_accord ? MatrixUnit::SINGLE_P1 :
						entry_accord ? MatrixUnit::SINGLE_P0 : units[range - udx];
				}

				for (auto idx = 0; idx < qubits - range; ++idx)
				{
					units.insert(units.begin(), MatrixUnit::SINGLE_I2);
				}

				entries[cdx].emplace_back(make_pair(entry, units));
			}

			auto refer_opt = entries[0][2 * order - 2].second;
			for (auto idx = 0; idx < qubits; ++idx)
			{
				if ((cdx >> idx) & 1)
				{
					refer_opt[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
				}
			}

			entries[cdx].back().second = refer_opt;
		}
	}

    return;
}

static void controller(MatrixSequence &sequence, const EigenMatrix2c U2, EigenMatrixXc &matrix)
{
	EigenMatrix2c P0;
	EigenMatrix2c P1;
	EigenMatrix2c I2;

	P0 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0);
	P1 << Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);
	I2 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);

	std::map<MatrixUnit, std::function<EigenMatrix2c()>> mapping =
	{
		{ MatrixUnit::SINGLE_P0, [&]() {return P0; } },
		{ MatrixUnit::SINGLE_P1, [&]() {return P1; } },
		{ MatrixUnit::SINGLE_I2, [&]() {return I2; } },
		{ MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
	};

	auto order = sequence.size();
	EigenMatrixXc Un = EigenMatrixXc::Identity(1, 1);
	EigenMatrixXc In = EigenMatrixXc::Identity(1ull << order, 1ull << order);

	for (const auto &val : sequence)
	{
		EigenMatrix2c M2 = mapping.find(val)->second();
		Un = Eigen::kroneckerProduct(Un, M2).eval();
	}

	matrix = In + Un;
    return;
}

static void recursive_partition(const EigenMatrixXc& sub_matrix, MatrixOperator &entries)
{
    Eigen::Index order = sub_matrix.rows();
    if (1 == order)
    {
        return;
    }
    else
    {
        EigenMatrixXc corner = sub_matrix.topLeftCorner(order / 2, order / 2);

        recursive_partition(corner, entries);

        upper_partition(order / 2, entries);
        under_partition(order / 2, entries);
    }

    return;
}

static void decomposition(EigenMatrixXc& matrix, MatrixOperator& entries, std::vector<SingleGateUnit>& cir_units)
{
	for (auto cdx = 0; cdx < entries.size(); ++cdx)
	{
		auto opts = entries[cdx].size();
		for (auto idx = 0; idx < opts; ++idx)
		{
			auto rdx = entries[cdx][idx].first;
			auto opt = entries[cdx][idx].second;

			if ((EigenComplexT(0, 0) == matrix(rdx, cdx) && (idx != opts - 1)) ||
				(EigenComplexT(1, 0) == matrix(cdx + 1, cdx) && (idx == opts - 1)))
			{
				continue;
			}
			else
			{
				EigenMatrix2c C2; /*placeholder*/
				C2 << EigenComplexT(0, 1), EigenComplexT(0, 1),
					EigenComplexT(0, 1), EigenComplexT(0, 1);

				EigenMatrixXc Cn;
				controller(opt, C2, Cn);

				Qnum indices(2);
				for (Eigen::Index index = 0; index < (1ull << opt.size()); ++index)
				{
					if (Cn(rdx, index) != EigenComplexT(0, 0))
					{
						indices[index == rdx] = index;
					}
				}

				EigenComplexT C0 = matrix(indices[0], cdx);  /*The entry to be eliminated */
				EigenComplexT C1 = matrix(indices[1], cdx);  /*The corresponding entry */

				EigenComplexT V11, V12, V21, V22;

				if (indices[0] < indices[1])
				{
					V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
				}
				else
				{
					V11 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
				}

				EigenMatrix2c V2;
				V2 << V11, V12, V21, V22;

				EigenMatrixXc Un;
				controller(opt, V2, Un);

				matrix = Un * matrix;

				QStat M2 = { (qcomplex_t)V11 ,(qcomplex_t)V12 ,(qcomplex_t)V21 ,(qcomplex_t)V22 };
				cir_units.insert(cir_units.begin(), std::make_pair(opt, M2));
			}
		}
	}

	EigenMatrix2c V2 = matrix.bottomRightCorner(2, 2);
	if (EigenMatrixXc::Identity(2, 2) != V2)
	{
		QStat M2 = { (qcomplex_t)((EigenComplexT)1.0 / V2(0,0)), (qcomplex_t)(V2(0,1)),
					 (qcomplex_t)(V2(1,0)) , (qcomplex_t)((EigenComplexT)1.0 / V2(1,1))};

		auto entry = entries.back().back().second;
		cir_units.insert(cir_units.begin(), std::make_pair(entry, M2));
	}
}

static void initialize(EigenMatrixXc& matrix, MatrixOperator& entries)
{
    auto qubits = (int)std::log2(matrix.rows());

    MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
    Cns.back() = MatrixUnit::SINGLE_V2;
    entries.front().emplace_back(make_pair(1, Cns));

    ColumnOperator& column = entries.front();
    for (auto idx = 1; idx < qubits; ++idx)
    {
        size_t path = 1ull << idx;
        for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
        {
            auto entry = column[opt].first;
            auto units = column[opt].second;

            // 1 : none of cn−1, . . . , c1 equals 1
            // * : otherwise
            auto iter = std::find(units.end() - idx, units.end(), MatrixUnit::SINGLE_P1);

            units[units.size() - 1 - idx] = (units.end() == iter) ?
                MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;

            column.emplace_back(make_pair(entry + path, units));
        }

        MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
        Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

        column.emplace_back(make_pair((1ull << idx), Lns));
    }

    return;
}

static void general_scheme(EigenMatrixXc& matrix, std::vector<SingleGateUnit>& cir_units)
{
	MatrixOperator entries;
	for (auto idx = 1; idx < matrix.cols(); ++idx)
	{
		ColumnOperator Co;
		entries.emplace_back(Co);
	}

	initialize(matrix, entries);
 	recursive_partition(matrix, entries);
	decomposition(matrix, entries, cir_units);

    return;
}

static void circuit_insert(QVec& qubits, std::vector<SingleGateUnit>& cir_units, QCircuit &circuit)
{
	std::sort(qubits.begin(), qubits.end(), [&](Qubit *a, Qubit *b)
	{
		return a->getPhysicalQubitPtr()->getQubitAddr()
			 < b->getPhysicalQubitPtr()->getQubitAddr();
	});

	auto rank = qubits.size();
	for (auto &val : cir_units)
	{
		QVec control;
		QCircuit cir;

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_P0 == val.first[qdx])
			{
				cir << X(qubits[qdx]);
				control.emplace_back(qubits[qdx]);
			}
			else if (MatrixUnit::SINGLE_P1 == val.first[qdx])
			{
				control.emplace_back(qubits[qdx]);
			}
			else
			{}
		}

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_V2 == val.first[qdx])
			{
				circuit << cir
					    << U4(val.second, qubits[qdx]).control(control).dagger()
						<< cir;

				break;
			}
		}
	}
}

QCircuit QPanda::matrix_decompose(QVec qubits, const QStat& src_mat)
{
	auto order = std::sqrt(src_mat.size());

	EigenMatrixXc matrix = EigenMatrixXc::Zero(order, order);
	for (auto rdx = 0; rdx < order; ++rdx)
	{
		for (auto cdx = 0; cdx < order; ++cdx)
		{
			matrix(rdx, cdx) = src_mat[rdx*order + cdx];
		}
	}

	return matrix_decompose(qubits, matrix);
}

QCircuit QPanda::matrix_decompose(QVec qubits, EigenMatrixXc& src_mat)
{
	if (!src_mat.isUnitary(1e-6))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "Non-unitary matrix.");
	}

	if (qubits.size() != log2(src_mat.cols()))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "The qubits number is error.");
	}

	std::vector<SingleGateUnit> cir_units;
	general_scheme(src_mat, cir_units);

	QCircuit output_circuit;
	circuit_insert(qubits, cir_units, output_circuit);

	return output_circuit;
}


//
// author: Ed Valeev (eduard@valeyev.net)
// date  : July 8, 2014
// the use of this software is permitted under the conditions GNU General Public License (GPL) version 2
//

#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
        Matrix;  // import dense, dynamically sized Matrix type from Eigen;
                 // this is row-major to conform with C/C++
                 // Eigen also supports statically-sized matrices, sparse matrices, etc.

#define INDEX(i,j) ((i>j) ? (((i)*((i)+1)/2)+(j)) : (((j)*((j)+1)/2)+(i)))

struct Atom {
    double zval;
    double x, y, z;
};

void read_geometry(const std::string& filename, std::vector<Atom>& atoms);
void read_1e_ints(Matrix& A, const std::string& filename);
double* read_2e_ints(const std::string& filename, size_t nao);

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

  using std::cout;
  using std::cerr;
  using std::endl;

  try {

    /*** =========================== ***/
    /*** initialize integrals, etc.  ***/
    /*** =========================== ***/

    // read geometry from xyz file
    std::vector<Atom> atoms;
    read_geometry("geom.dat", atoms);

    // count the number of electrons
    auto nelectron = 0;
    for (auto i = 0; i < atoms.size(); ++i)
      nelectron += atoms[i].zval;
    const auto ndocc = nelectron / 2;

    // compute the nuclear repulsion energy
    auto enuc = 0.0;
    for (auto i = 0; i < atoms.size(); i++)
      for (auto j = i + 1; j < atoms.size(); j++) {
        auto xij = atoms[i].x - atoms[j].x;
        auto yij = atoms[i].y - atoms[j].y;
        auto zij = atoms[i].z - atoms[j].z;
        auto r2 = xij*xij + yij*yij + zij*zij;
        auto r = sqrt(r2);
        enuc += atoms[i].zval * atoms[j].zval / r;
      }
    cout << "\tNuclear repulsion energy = " << enuc << endl;

    // ask the user for # of AOs
    cout << "\nEnter the number of AOs (7 unless U have updated the links): ";
    size_t nao;
    std::cin >> nao;

    // compute overlap integrals
    Matrix S(nao, nao);      // this creates an nao by nao matrix (contents are not initialized!)
                             // to make a matrix of zeroes do this:
                             // auto S = Matrix::Zero(nao, nao);
    read_1e_ints(S, "s.dat");
    cout << "\n\tOverlap Integrals:\n";
    cout << S << endl;

    // compute kinetic-energy integrals
    Matrix T(nao, nao);
    read_1e_ints(T, "t.dat");
    cout << "\n\tKinetic-Energy Integrals:\n";
    cout << T << endl;

    // compute nuclear-attraction integrals
    Matrix V(nao, nao);
    read_1e_ints(V, "v.dat");
    cout << "\n\tNuclear Attraction Integrals:\n";
    cout << V << endl;

    // Core Hamiltonian = T + V
    Matrix H = T + V;
    cout << "\n\tCore Hamiltonian:\n";
    cout << H << endl;

    // T and V no longer needed, free up the memory
    T.resize(0,0);
    V.resize(0,0);

    /* read two-electron integrals */
    auto TEI = read_2e_ints("eri.dat", nao);

    /*** =========================== ***/
    /*** build initial-guess density ***/
    /*** =========================== ***/

    // solve H C = e S C
    Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(H, S);
    auto eps = gen_eig_solver.eigenvalues();
    auto C = gen_eig_solver.eigenvectors();
    cout << "\n\tInitial C Matrix:\n";
    cout << C << endl;

    // compute density, D = C(occ) . C(occ)T
    auto C_occ = C.leftCols(ndocc);
    Matrix D = C_occ * C_occ.transpose();
    cout << "\n\tInitial Density Matrix:\n";
    cout << D << endl;

    // compute HF energy
    auto ehf = 0.0;
    for (auto i = 0; i < nao; i++)
      for (auto j = 0; j < nao; j++)
        ehf += 2.0 * D(i,j) * H(i,j);

    std::cout <<
        "\n\n Iter        E(elec)              E(tot)               Delta(E)             RMS(D)\n";
    printf(" %02d %20.12f %20.12f\n", 0, ehf, ehf + enuc);

    /*** =========================== ***/
    /*** main iterative loop ***/
    /*** =========================== ***/

    const auto maxiter = 100;
    const auto conv = 1e-12;
    auto iter = 0;
    auto rmsd = 0.0;
    auto ediff = 0.0;
    do {
      ++iter;

      // Save a copy of the energy and the density
      auto ehf_last = ehf;
      auto D_last = D;

      // build a new Fock matrix
      auto F = H;
      for (auto i = 0; i < nao; i++)
        for (auto j = 0; j < nao; j++) {
          for (auto k = 0; k < nao; k++)
            for (auto l = 0; l < nao; l++) {
              auto ij = INDEX(i, j);
              auto kl = INDEX(k, l);
              auto ijkl = INDEX(ij, kl);
              auto ik = INDEX(i, k);
              auto jl = INDEX(j, l);
              auto ikjl = INDEX(ik, jl);

              F(i,j) += D(k,l) * (2.0 * TEI[ijkl] - TEI[ikjl]);
            }
        }

      if (iter == 1) {
        cout << "\n\tFock Matrix:\n";
        cout << F << endl;
      }

      // solve F C = e S C
      Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(F, S);
      auto eps = gen_eig_solver.eigenvalues();
      auto C = gen_eig_solver.eigenvectors();

      // compute density, D = C(occ) . C(occ)T
      auto C_occ = C.leftCols(ndocc);
      D = C_occ * C_occ.transpose();

      // compute HF energy
      ehf = 0.0;
      for (auto i = 0; i < nao; i++)
        for (auto j = 0; j < nao; j++)
          ehf += D(i,j) * (H(i,j) + F(i,j));

      // compute difference with last iteration
      ediff = ehf - ehf_last;
      rmsd = (D - D_last).norm();

      printf(" %02d %20.12f %20.12f %20.12f %20.12f\n", iter, ehf, ehf + enuc,
             ediff, rmsd);

    } while (((fabs(ediff) > conv) || (fabs(rmsd) > conv)) && (iter < maxiter));

    delete[] TEI;
  } // end of try block

  catch (const char* ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::string& ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::exception& ex) {
    cerr << ex.what() << endl;
    return 1;
  }
  catch (...) {
    cerr << "caught unknown exception\n";
    return 1;
  }

  return 0;
}

void read_geometry(const std::string& filename, std::vector<Atom>& atoms) {

  std::ifstream is(filename);
  assert(is.good());
  size_t natom;
  is >> natom;

  atoms.resize(natom);
  for (int i = 0; i < natom; i++)
    is >> atoms[i].zval >> atoms[i].x >> atoms[i].y >> atoms[i].z;
}

void read_1e_ints(Matrix& A, const std::string& filename) {
  std::ifstream is(filename);
  assert(is.good());

  while (is) {
    int i, j;
    double val;
    is >> i >> j >> val;
    --i; --j;
    A(i,j) = A(j,i) = val;
  }
}

double* read_2e_ints(const std::string& filename, size_t nao) {
  auto nints = ((nao * (nao + 1) / 2) * ((nao * (nao + 1) / 2) + 1) / 2);
  auto result = new double[nints];
  std::fill(result, result+nints, 0);

  std::ifstream is(filename);
  assert(is.good());

  while (is) {
    size_t i, j, k, l;
    double val;
    is >> i >> j >> k >> l >> val;
    auto ij = INDEX(i - 1, j - 1);
    auto kl = INDEX(k - 1, l - 1);
    auto ijkl = INDEX(ij, kl);

    result[ijkl] = val;
  }

  return result;
}


// Copyright 2020 Erik Kouters (Falcons)
// SPDX-License-Identifier: Apache-2.0
#ifndef GEOMETRY_INTERSECT_HPP
#define GEOMETRY_INTERSECT_HPP

#include "vector2d.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace geometry
{

class PolynomialEquation
{
public:
    PolynomialEquation(double x0, double x1, double x2);

    void solve();

    boost::optional<double> getMinNonNegative() const;

private:
    std::vector<double> polynome_;
    boost::optional<std::vector<double>> roots_;

    void solveOrder2();
};

class KinematicIntersect
{
public:
    static boost::optional<Vector2D> intersect(
        const Vector2D &actor_position, double max_actor_speed,
        const Vector2D &target_position, const Vector2D &target_velocity);
};

} // namespace geometry

#endif // GEOMETRY_INTERSECT_HPP


#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_COM_DIMAP
#include "com_dimap.h"
#define INCLUDED_COM_DIMAP
#endif

#ifndef INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#include <boost/math/special_functions/round.hpp>
#define INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#endif

//------------------------------------------------------------------------------
// DEFINITION OF STATIC CLASS MEMBERS
//------------------------------------------------------------------------------

const double com::DiMap::logMin = 1.0e-150;

const double com::DiMap::logMax = 1.0e150;

//------------------------------------------------------------------------------
// DEFINITION OF CLASS MEMBERS
//------------------------------------------------------------------------------

/*!
  The double and integer intervals are both set to [0,1].
*/
com::DiMap::DiMap()

  : d_x1(0.0), d_x2(1.0), d_y1(0), d_y2(1), d_cnv(1.0)

{
}

/*!
  \param   i1 First border of integer interval.
  \param   i2 Second border of integer interval.
  \param   d1 First border of double interval.
  \param   d2 Second border of double interval.
  \param   logarithmic Logarithmic mapping, true or false. Defaults to false.

  Constructs a com::DiMap instance with initial integer and double intervals.
*/
com::DiMap::DiMap(int i1, int i2, double d1, double d2, bool logarithmic)

  : d_x1(0.0), d_x2(1.0), d_y1(0), d_y2(1), d_cnv(1.0)

{
  d_log = logarithmic;
  setIntRange(i1,i2);
  setDblRange(d1, d2);
}

com::DiMap::~DiMap()
{
}

/*!
  \param   x Value.
  \return  If \a x lies inside or at the border of the double range.
*/
bool com::DiMap::contains(double x) const
{
  return ( (x >= MIN(d_x1, d_x1)) && (x <= MAX(d_x1, d_x2)));
}

/*!
  \param   x Value.
  \return  If \a x lies inside or at the border of the integer range.
*/
bool com::DiMap::contains(int x) const
{
  return (x >= MIN(d_y1, d_y1)) && (x <= MAX(d_y1, d_y2));
}

/*!
  \param   d1 First border.
  \param   d2 Second border.
  \param   lg Logarithmic (true) or linear (false) scaling. Defaults to false.
*/
void com::DiMap::setDblRange(double d1, double d2, bool lg)
{
  if(lg)
  {
    d_log = true;
    if(d1 < logMin)
      d1 = logMin;
    else if(d1 > logMax)
      d1 = logMax;

    if(d2 < logMin)
      d2 = logMin;
    else if (d2 > logMax)
      d2 = logMax;

    d_x1 = std::log(d1);
    d_x2 = std::log(d2);
  }
  else
  {
    d_log = false;
    d_x1  = d1;
    d_x2  = d2;
  }

  newFactor();
}

/*!
  \param   i1 First border.
  \param   i2 Second border.
*/
void com::DiMap::setIntRange(int i1, int i2)
{
  d_y1 = i1;
  d_y2 = i2;
  newFactor();
}

/*!
  \param  x Value in double interval.
  \return Transformed value in int interval.
  \sa     invTransform(), limTransform()

  linear mapping: round(i1 + (i2 - i1) / (d2 - d1) * (x - d1))

  logarithmic mapping: round(i1 + (i2 - i1) / log(d2 / d1) * log(x / d1))

  The specified value is allowed to lie outside the intervals. If you want to
  limit the returned value, use limTransform().
*/
int com::DiMap::transform(double x) const
{
  if(d_log)
    return (d_y1 + boost::math::iround((std::log(x) - d_x1) * d_cnv));
  else
    return (d_y1 + boost::math::iround((x - d_x1) * d_cnv));
}

/*!
  \param   y Integer value to be transformed.
  \return  Transformed value in double interval.
  \sa      transform(), limTransform()

  linear mapping: d1 + (d2 - d1) / (i2 - i1) * (y - i1)

  logarithmic mapping: d1 + (d2 - d1) / log(i2 / i1) * log(y / i1)
*/
double com::DiMap::invTransform(int y) const
{
  if(d_cnv == 0.0)
  {
    return 0.0;
  }
  else
  {
    if(d_log)
      return std::exp(d_x1 + double(y - d_y1) / d_cnv);
    else
      return (d_x1 + double(y - d_y1) / d_cnv);
  }
}

/*!
  \param   x Value to be transformed.
  \return  Transformed value.
  \sa      transform(), invTransform()

  The function is similar to transform, but limits the input value to the
  nearest border of the map's double interval if it lies outside that interval.
*/
int com::DiMap::limTransform(double x) const
{
  if(x > MAX(d_x1, d_x2))
    x = MAX(d_x1, d_x2);
  else if(x < MIN(d_x1, d_x2))
    x = MIN(d_x1, d_x2);

  return transform(x);
}

/*!
  \param   x Value to be transformed.
  \return  Transformed value.
  \sa

  linear mapping: i1 + (i2 - i1) / (d2 - d1) * (x - d1)
  logarithmic mapping: i1 + (i2 - i1) / log(d2 / d1) * log(x / d1)

  This function is similar to transform(), but makes the integer interval
  appear to be double.
*/
//------------------------------------------------------------
double com::DiMap::xTransform(double x) const
{
  double rv;

  if(d_log) {
    rv = static_cast<double>(d_y1) + (std::log(x) - d_x1) * d_cnv;
  }
  else {
    rv = static_cast<double>(d_y1) + (x - d_x1) * d_cnv;
  }

  return rv;
}

void com::DiMap::newFactor()
{
  if(d_x2 != d_x1)
    d_cnv = static_cast<double>(d_y2 - d_y1) / (d_x2 - d_x1);
  else
    d_cnv = 0.0;
}

double com::DiMap::d1() const
{
  return d_x1;
}

double com::DiMap::d2() const
{
  return d_x2;
}

int com::DiMap::i1() const
{
  return d_y1;
}

int com::DiMap::i2() const
{
  return d_y2;
}

double com::DiMap::scale() const
{
  return d_cnv;
}

//------------------------------------------------------------------------------
// DOCUMENTATION OF ENUMERATIONS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DOCUMENTATION OF INLINE FUNCTIONS
//------------------------------------------------------------------------------

/*!
  \fn      bool com::DiMap::logarithmic() const
  \return  True if the double interval is scaled logarithmically.
*/


#ifndef _POSE_ESTIMATION_ORIENTATION_UKF_HPP
#define _POSE_ESTIMATION_ORIENTATION_UKF_HPP

#include "OrientationState.hpp"
#include "OrientationUKFConfig.hpp"
#include <pose_estimation/Measurement.hpp>
#include <pose_estimation/UnscentedKalmanFilter.hpp>
#include <Eigen/Core>

namespace pose_estimation
{

/**
 * This filter estimates the orientation of an IMU in the NWU navigation frame.
 * It integrates rotation rates, accelerations and linear velocities.
 * The linear velocities help to constrain the acceleration and to estimate the biases.
 * Given gyroscopes capable of sensing the rotation of the earth (e.g. a fibre optic gyro)
 * this filter is able to estimate it's true heading.
 */
class OrientationUKF : public UnscentedKalmanFilter<OrientationState>
{
public:
    MEASUREMENT(RotationRate, 3)
    MEASUREMENT(Acceleration, 3)
    MEASUREMENT(VelocityMeasurement, 3)

public:
    OrientationUKF(const State& initial_state, const Covariance& state_cov,
                   double gyro_bias_tau, double acc_bias_tau, const LocationConfiguration& location);
    virtual ~OrientationUKF() {}

    /**
     * Sets the current rotation rate of the IMU in rad/s.
     */
    void integrateMeasurement(const RotationRate& measurement);

    /**
     * Sets the current acceleration of the IMU in m/s^2.
     */
    void integrateMeasurement(const Acceleration& measurement);

    /**
     * Integrate the linear velocitiy of the IMU in m/s.
     */
    void integrateMeasurement(const VelocityMeasurement& measurement);

    /* Returns unbiased rotation rate in IMU frame */
    RotationRate::Mu getRotationRate();

protected:
    void predictionStepImpl(double delta);

protected:
    RotationRate rotation_rate;
    Acceleration acceleration;
    Eigen::Vector3d earth_rotation;
    double gyro_bias_tau;
    double acc_bias_tau;
};

}

#endif


#include <Eigen/Core>
#include <gtest/gtest.h>

#include "test/utils.hpp"

#include "ldaplusplus/e_step_utils.hpp"
#include "ldaplusplus/utils.hpp"

using namespace Eigen;
using namespace ldaplusplus;

// T will be available as TypeParam in TYPED_TEST functions
template <typename T>
class TestApproximateSupervisedExpectationStep : public ParameterizedTest<T> {};

TYPED_TEST_CASE(TestApproximateSupervisedExpectationStep, ForFloatAndDouble);

TYPED_TEST(TestApproximateSupervisedExpectationStep, ComputeApproximateSupervisedPhi) {
    VectorXi X(10);
    X << 22, 49, 0, 2, 16, 35, 94, 3, 25, 10;
    VectorX<TypeParam> X_ratio = X.cast<TypeParam>() / X.sum();
    int  y = 0;

    MatrixX<TypeParam> phi = MatrixX<TypeParam>::Random(5, 10);
    VectorX<TypeParam> alpha = VectorX<TypeParam>::Constant(5, 0.2);
    MatrixX<TypeParam> beta = MatrixX<TypeParam>::Random(5, 10);
    MatrixX<TypeParam> eta = MatrixX<TypeParam>::Random(5, 3);
    VectorX<TypeParam> gamma(beta.rows());

    // Normalize beta and phi
    beta.array() -= beta.minCoeff() - 0.001;
    beta.array().rowwise() /= beta.colwise().sum().array();

    phi.array() -= phi.minCoeff() - 0.001;
    phi.array().rowwise() /= phi.colwise().sum().array();

    // Compute gamma according to the random phi
    e_step_utils::compute_gamma<TypeParam>(
        X,
        alpha,
        phi,
        gamma
    );

    // Make copies of phi
    MatrixX<TypeParam> phi_unsupervised = phi;
    MatrixX<TypeParam> phi_supervised = phi;
    MatrixX<TypeParam> phi_supervised_approximation = phi;

    // Make copies of gamma
    VectorX<TypeParam> gamma_unsupervised = gamma;
    VectorX<TypeParam> gamma_supervised = gamma;
    VectorX<TypeParam> gamma_supervised_approximation = gamma;

    VectorX<TypeParam> h(5);

    TypeParam likelihood_baseline = e_step_utils::compute_supervised_likelihood(
        X,
        y,
        alpha,
        beta,
        eta,
        phi,
        gamma
    );

    size_t fixed_point_iterations = 5;

    for (int i=0; i<50; i++) {
        // Compute phi with unsupervised method
        e_step_utils::compute_unsupervised_phi<TypeParam> (
            beta,
            gamma_unsupervised,
            phi_unsupervised
        );
        // Compute gamma
        e_step_utils::compute_gamma <TypeParam> (
            X,
            alpha,
            phi_unsupervised,
            gamma_unsupervised
        );
    }

    // Compute new likelihood
    TypeParam likelihood_unsupervised = e_step_utils::compute_supervised_likelihood(
        X,
        y,
        alpha,
        beta,
        eta,
        phi_unsupervised,
        gamma_unsupervised
    );

    for (int i=0; i<50; i++) {
        // Compute phi with the supervised method
        e_step_utils::compute_supervised_phi_gamma<TypeParam> (
            X,
            X_ratio,
            y,
            beta,
            eta,
            fixed_point_iterations,
            phi_supervised,
            gamma_supervised,
            h
        );
    }

    // Compute new likelihood
    TypeParam likelihood_supervised = e_step_utils::compute_supervised_likelihood(
        X,
        y,
        alpha,
        beta,
        eta,
        phi_supervised,
        gamma_supervised
    );

    TypeParam C = 1.0;
    for (int i=0; i<50; i++) {
        // Compute phi with the supervised approximation
        e_step_utils::compute_supervised_approximate_phi<TypeParam> (
            X_ratio,
            X.sum(),
            y,
            beta,
            eta,
            gamma_supervised_approximation,
            C,
            phi_supervised_approximation
        );

        // Compute gamma
        e_step_utils::compute_gamma<TypeParam> (
            X,
            alpha,
            phi_supervised_approximation,
            gamma_supervised_approximation
        );
    }
    // Compute new likelihood
    TypeParam likelihood_supervised_approximation = e_step_utils::compute_supervised_likelihood(
        X,
        y,
        alpha,
        beta,
        eta,
        phi_supervised_approximation,
        gamma_supervised_approximation
    );

    EXPECT_GT(likelihood_supervised_approximation, likelihood_baseline);
    // EXPECT_GT(likelihood_supervised_approximation, likelihood_unsupervised);
    // EXPECT_GT(likelihood_supervised, likelihood_unsupervised);
    //
    // What should the following be
    //
    // EXPECT_GT(likelihood_supervised_approximation, likelihood_supervised);
}

TYPED_TEST(TestApproximateSupervisedExpectationStep, ComputeApproximatePhiWithEmptyDocument) {
    VectorX<TypeParam> X_ratio = VectorX<TypeParam>::Zero(10);
    int  y = 0;
    TypeParam C = 1.0;

    MatrixX<TypeParam> phi = MatrixX<TypeParam>::Random(5, 10);
    VectorX<TypeParam> alpha = VectorX<TypeParam>::Constant(5, 0.2);
    MatrixX<TypeParam> beta = MatrixX<TypeParam>::Random(5, 10);
    MatrixX<TypeParam> eta = MatrixX<TypeParam>::Random(5, 3);
    VectorX<TypeParam> gamma(beta.rows());

    // Normalize beta and phi
    beta.array() -= beta.minCoeff() - 0.001;
    beta.array().rowwise() /= beta.colwise().sum().array();

    phi.array() -= phi.minCoeff() - 0.001;
    phi.array().rowwise() /= phi.colwise().sum().array();

    // Compute gamma according to the random phi
    e_step_utils::compute_gamma<TypeParam>(
        VectorXi::Zero(10),
        alpha,
        phi,
        gamma
    );

    // Compute phi and gamma
    e_step_utils::compute_supervised_approximate_phi<TypeParam> (
        X_ratio,
        0,
        y,
        beta,
        eta,
        gamma,
        C,
        phi
    );
    e_step_utils::compute_gamma<TypeParam>(
        VectorXi::Zero(10),
        alpha,
        phi,
        gamma
    );

    auto cwise_isnan = math_utils::CwiseIsNaN<TypeParam>();
    EXPECT_FALSE(phi.unaryExpr(cwise_isnan).any());
    EXPECT_FALSE(gamma.unaryExpr(cwise_isnan).any());
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE bilinearity_algebra_test

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/edwards.hpp>
#include <nil/crypto3/algebra/curves/bn128.hpp>
#include <nil/crypto3/algebra/curves/alt_bn128.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>

using namespace nil::crypto3::algebra;

template<typename CurveType>
void pairing_test() {
    GT<CurveType> GT_one = GT<CurveType>::one();

    printf("Running bilinearity tests:\n");
    G1<CurveType> P = (Fr<CurveType>::random_element()) * G1<CurveType>::one();
    // G1<CurveType> P = Fr<CurveType>("2") * G1<CurveType>::one();
    G2<CurveType> Q = (Fr<CurveType>::random_element()) * G2<CurveType>::one();
    // G2<CurveType> Q = Fr<CurveType>("3") * G2<CurveType>::one();

    printf("P:\n");
    P.print();
    P.print_coordinates();
    printf("Q:\n");
    Q.print();
    Q.print_coordinates();
    printf("\n\n");

    Fr<CurveType> s = Fr<CurveType>::random_element();
    // Fr<CurveType> s = Fr<CurveType>("2");
    G1<CurveType> sP = s * P;
    G2<CurveType> sQ = s * Q;

    printf("Pairing bilinearity tests (three must match):\n");
    GT<CurveType> ans1 = CurveType::pair_reduced(sP, Q);
    GT<CurveType> ans2 = CurveType::pair_reduced(P, sQ);
    GT<CurveType> ans3 = CurveType::pair_reduced(P, Q) ^ s;
    ans1.print();
    ans2.print();
    ans3.print();
    assert(ans1 == ans2);
    assert(ans2 == ans3);

    assert(ans1 != GT_one);
    assert((ans1 ^ Fr<CurveType>::field_char()) == GT_one);
    printf("\n\n");
}

template<typename CurveType>
void double_miller_loop_test() {
    const G1<CurveType> P1 = (Fr<CurveType>::random_element()) * G1<CurveType>::one();
    const G1<CurveType> P2 = (Fr<CurveType>::random_element()) * G1<CurveType>::one();
    const G2<CurveType> Q1 = (Fr<CurveType>::random_element()) * G2<CurveType>::one();
    const G2<CurveType> Q2 = (Fr<CurveType>::random_element()) * G2<CurveType>::one();

    const typename CurveType::pairing::g1_precomp prec_P1 = CurveType::precompute_G1(P1);
    const typename CurveType::pairing::g1_precomp prec_P2 = CurveType::precompute_G1(P2);
    const typename CurveType::pairing::g2_precomp prec_Q1 = CurveType::precompute_G2(Q1);
    const typename CurveType::pairing::g2_precomp prec_Q2 = CurveType::precompute_G2(Q2);

    const typename CurveType::pairing::fqk_type ans_1 = CurveType::miller_loop(prec_P1, prec_Q1);
    const typename CurveType::pairing::fqk_type ans_2 = CurveType::miller_loop(prec_P2, prec_Q2);
    const typename CurveType::pairing::fqk_type ans_12 =
        CurveType::double_miller_loop(prec_P1, prec_Q1, prec_P2, prec_Q2);
    assert(ans_1 * ans_2 == ans_12);
}

template<typename CurveType>
void affine_pairing_test() {
    GT<CurveType> GT_one = GT<CurveType>::one();

    printf("Running bilinearity tests:\n");
    G1<CurveType> P = (Fr<CurveType>::random_element()) * G1<CurveType>::one();
    G2<CurveType> Q = (Fr<CurveType>::random_element()) * G2<CurveType>::one();

    printf("P:\n");
    P.print();
    printf("Q:\n");
    Q.print();
    printf("\n\n");

    Fr<CurveType> s = Fr<CurveType>::random_element();
    G1<CurveType> sP = s * P;
    G2<CurveType> sQ = s * Q;

    printf("Pairing bilinearity tests (three must match):\n");
    GT<CurveType> ans1 = CurveType::affine_pair_reduced(sP, Q);
    GT<CurveType> ans2 = CurveType::affine_pair_reduced(P, sQ);
    GT<CurveType> ans3 = CurveType::affine_pair_reduced(P, Q) ^ s;
    ans1.print();
    ans2.print();
    ans3.print();
    assert(ans1 == ans2);
    assert(ans2 == ans3);

    assert(ans1 != GT_one);
    assert((ans1 ^ Fr<CurveType>::field_char()) == GT_one);
    printf("\n\n");
}

int main(void) {
    pairing_test<edwards_pp>();
    double_miller_loop_test<edwards_pp>();

    pairing_test<mnt6_pp>();
    double_miller_loop_test<mnt6_pp>();
    affine_pairing_test<mnt6_pp>();

    pairing_test<mnt4_pp>();
    double_miller_loop_test<mnt4_pp>();
    affine_pairing_test<mnt4_pp>();

    pairing_test<alt_bn128_pp>();
    double_miller_loop_test<alt_bn128_pp>();

    pairing_test<bn128_pp>();
    double_miller_loop_test<bn128_pp>();
}


#ifndef __UTIL_H__
#define __UTIL_H__

#include <Eigen/Geometry>
#include <vector>

#include <opencv2/opencv.hpp>
#include <cvx/util/camera/camera.hpp>
#include <cvx/util/geometry/kdtree.hpp>
#include <cvx/util/math/rng.hpp>

// perform mean shift clustering of the points and choose the mode with larger number of votes

void mean_shift(cvx::util::RNG &g, const std::vector<Eigen::Vector3f> &pts, Eigen::Vector3f &mode, float &weight, float sigma,
                    unsigned int maxIter, float seed_perc, uint min_seeds) ;

Eigen::Vector3f back_project(const cv::Mat &depth, const cvx::util::PinholeCamera &cam, const cv::Point &pt) ;

// Kabsch algorithm for rigid pose estimation between two point clouds ( finds T to minimize sum_i ||P_i * T - Q_i|| )

Eigen::Isometry3f find_rigid(const Eigen::Matrix3Xf &P, const Eigen::Matrix3Xf &Q) ;
Eigen::Isometry3f find_rigid(const std::vector<Eigen::Vector3f> &P, const std::vector<Eigen::Vector3f> &Q) ;

void save_cloud_obj(const std::string &file_name, const std::vector<Eigen::Vector3f> &cloud) ;

#endif


#ifndef SQUAREDFUNCTIONINFO_H
#define SQUAREDFUNCTIONINFO_H

#include <opengv/optimization_tools/objective_function_tools/ObjectiveFunctionInfo.hpp>
#include <Eigen/Dense>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>

class SquaredFunctionInfo : public ObjectiveFunctionInfo {
  Eigen::MatrixXd M;

 public:
  SquaredFunctionInfo(const opengv::relative_pose::RelativeAdapterBase & adapter );
  ~SquaredFunctionInfo();

  Eigen::MatrixXd get_M();
  double objective_function_value(const opengv::rotation_t & rotation, const opengv::translation_t & translation);
  opengv::rotation_t rotation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation);
  opengv::translation_t translation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation);
};

#endif


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

// Copyright Christopher Kormanyos 2016.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing a bare metal real-time performance measurement of sqrt(negatable), 16-bit.

#include <cmath>
#include <mcal_benchmark.h>
#include <mcal_cpu.h>
#include <mcal_irq.h>
#include <mcal_port.h>

#define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION // Do not use Boost.Multiprecision.
#define BOOST_FIXED_POINT_DISABLE_IOSTREAM       // Do not use I/O streaming.

#include <boost/fixed_point/fixed_point.hpp>

typedef boost::fixed_point::negatable<4, -11> numeric_type;
//typedef float numeric_type;

namespace app
{
  namespace benchmark
  {
    void task_init();
    void task_func();
  }

  typedef mcal::benchmark::benchmark_port_type port_type;
}

numeric_type x = numeric_type(6) / 10;
numeric_type y;

void app::benchmark::task_init()
{
  port_type::set_direction_output();
}

void app::benchmark::task_func()
{
  using std::sqrt;

  mcal::irq::disable_all();
  port_type::set_pin_high();

  y = sqrt(x);

  port_type::set_pin_low();
  mcal::irq::enable_all();

  // sqrt(6/10) = approx. 0.7745966692414834
  const bool value_is_ok =    (y > (numeric_type(7) / 10))
                           && (y < (numeric_type(8) / 10));

  if(value_is_ok)
  {
    // The benchmark is OK.
    // Perform one nop and leave.

    mcal::cpu::nop();
  }
  else
  {
    // The benchmark result is not OK!
    // Remain in a blocking loop and crash the system.

    for(;;) { mcal::cpu::nop(); }
  }
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/range.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTANT_ASSERT(
        sum(range(int_<1>, int_<6>)) == int_<1 + 2 + 3 + 4 + 5>
    );

    BOOST_HANA_CONSTEXPR_ASSERT(
        sum(list(1, int_<3>, long_<-5>, 9)) == 1 + 3 - 5 + 9
    );
    //! [main]
}


///////////////////////////////////////////////////////////////////////////////
// ars::test::standard_distribution.hpp                                      //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_TEST_STANDARD_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_TEST_STANDARD_DISTRIBUTION_HPP_ER_2009

#include <iostream>
#include <list>
#include <string>
#include <iterator>
#include <vector>
#include <algorithm>

#include <boost/format.hpp>
#include <boost/range.hpp>
#include <boost/accumulators/accumulators.hpp>

#include <boost/statistics/detail/distribution_common/meta/random/generator.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/non_parametric/kolmogorov_smirnov/statistic.hpp>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/ref_distribution.hpp>

#include <boost/ars/function/adaptor.hpp>
#include <boost/ars/constant.hpp>
#include <boost/ars/proposal_sampler.hpp>
#include <boost/ars/sampler.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{
namespace test{

struct standard_distribution{

	static void header(std::ostream& os){
    	os << "This test outputs a sequence {(a,b,c)} where" << std::endl;
    	os 	<< "a : sample size" << std::endl
     		<< "b : kolmogorov-smirnov statistic" << std::endl
     		<< "c : avg number of rejections" << std::endl;
    }

	// Samples from distribution D using adaptive rejection sampling and outputs
    // convergence statistics
	template<typename D,typename U>
	static void call(
    	const D& mdist, //e.g. D == math::normal_distribution<T>
    	typename D::value_type x_min,
    	typename D::value_type x_max,
    	typename D::value_type init_0,
    	typename D::value_type init_1,
    	U& urng,
    	unsigned n1,   // # loops
    	unsigned n2,   // # subsamples per loop
    	unsigned n3,   // size of subsample
    	unsigned n4,   // At each loop, n2 *= n4
    	unsigned n_max_reject,
    	std::ostream& os
	){

    	// The ars is re-initialized after each n3 sample.
    	// n3 * n4 is the total size of the sample over which a KS is computed

    	namespace dist = boost::statistics::detail::distribution;
    	namespace ks = boost::statistics::detail::kolmogorov_smirnov;
    	typedef std::string                                     str_;
    	typedef std::runtime_error                              err_;
        typedef typename D::value_type							val_;
    	typedef std::vector<val_>                               vals_;

    	typedef ars::function::adaptor<D>                       fun_t;
    	typedef ars::proposal_sampler<val_,std::vector>         ps_;
    	typedef ars::sampler<ps_>                               ars_;
    	typedef random::ref_distribution<ars_&>                 ref_ars_;
    	typedef variate_generator<U&,ref_ars_>                  vg_ars_;

    	typedef ks::tag::statistic<val_> 						tag_ks_;
    	typedef boost::accumulators::stats<tag_ks_> 			acc_features_;
    	typedef boost::accumulators::accumulator_set<val_,acc_features_> acc_;

    	ars_ ars;
    	ars.set_function(x_min, x_max, fun_t(mdist));
    	{
    		static const str_ str
        		= "Initialized every %1% draw(s) with x1 = %2% and x2 = %3%";
        	boost::format f(str);
        	f%n3%init_0%init_1;
        	os << f.str() << std::endl;
        	os << description(mdist) << std::endl;
    	}
    	long unsigned n_reject;

    	try{
        	for(unsigned i1 = 0; i1<n1; i1++){
            	n_reject = 0;
            	acc_ acc;
            	for(unsigned i2 = 0; i2<n2; i2++){
                	try{
                    	ars.initialize(init_0,init_1);
                    	vg_ars_ vg_ars(urng,ref_ars_(ars));
                    	for(unsigned i = 0; i<n3; i++)
                    	{
							val_ x = vg_ars();
							acc(x);
                    	}
                    	// Without ref_ars_, n would be reset to 0
                    	n_reject += (
                        	(vg_ars.distribution()).distribution()
                    	).n_reject();
                	}catch(std::exception& e){
                    	boost::format f("at i1 = %1%, i2 = %2% : %3%");
                    	f % i1 % i2 % e.what();
                    	throw std::runtime_error(f.str());
                	}
            	}
            	long int n = boost::accumulators::extract::count(acc);
            	val_ st = ks::extract::statistic<val_>(acc,mdist);
            	val_ rate = static_cast<val_>(n_reject)/static_cast<val_>(n3*n2);
            	os
                	<< '('
                	<< n
                	<< ','
                	<< st
                	<< ','
                	<< rate
                	<< ')'
                	<< std::endl;
            	n2 *= n4;
        	}
    	}catch(std::exception& e)
    	{
        	std::cerr << e.what() << std::endl;
    	}
    	os << std::endl;
	}
};

}//test
}// ars
}// detail
}// statistics
}// boost

#endif


#ifdef MEX

#include <igl/AABB.h>
#include <igl/in_element.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>

#include <cstring>
#include <iostream>
void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & Ele,
  Eigen::MatrixXd & Q,
  Eigen::MatrixXd & bb_mins,
  Eigen::MatrixXd & bb_maxs,
  Eigen::VectorXi & elements)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");

  const int dim = mxGetN(prhs[0]);
  mexErrMsgTxt(dim == 3 || dim == 2,
    "Mesh vertex list must be #V by 2 or 3 list of vertex positions");

  mexErrMsgTxt(dim+1 == mxGetN(prhs[1]),
    "Mesh \"face\" simplex size must equal dimension+1");

  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,Ele);
  parse_rhs_double(prhs+2,Q);
  mexErrMsgTxt(Q.cols() == dim,"Dimension of Q should match V");
  if(nrhs > 3)
  {
    mexErrMsgTxt(nrhs >= 6, "The number of input arguments must be 3 or >=6.");
    parse_rhs_double(prhs+3,bb_mins);
    if(bb_mins.size()>0)
    {
      mexErrMsgTxt(bb_mins.cols() == dim,"Dimension of bb_mins should match V");
      mexErrMsgTxt(bb_mins.rows() >= Ele.rows(),"|bb_mins| should be > |Ele|");
    }
    parse_rhs_double(prhs+4,bb_maxs);
    mexErrMsgTxt(bb_maxs.cols() == bb_mins.cols(),
      "|bb_maxs| should match |bb_mins|");
    mexErrMsgTxt(bb_mins.rows() == bb_maxs.rows(),
      "|bb_mins| should match |bb_maxs|");
    parse_rhs_index(prhs+5,elements);
    mexErrMsgTxt(elements.cols() == 1,"Elements should be column vector");
    mexErrMsgTxt(bb_mins.rows() == elements.rows(),
      "|bb_mins| should match |elements|");
  }else
  {
    // Defaults
    bb_mins.resize(0,dim);
    bb_maxs.resize(0,dim);
    elements.resize(0,1);
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  MatrixXd V,Q,bb_mins,bb_maxs;
  MatrixXi Ele;
  VectorXi elements;
  VectorXi I;
  parse_rhs(nrhs,prhs,V,Ele,Q,bb_mins,bb_maxs,elements);
  bool was_serialized = bb_mins.size()>0;

  switch(V.cols())
  {
    default:
      mexErrMsgTxt(false,"Un-supported dimension.");
      break;
    case 3:
    {
      AABB<MatrixXd,3> aabb;
      aabb.init(V,Ele,bb_mins,bb_maxs,elements);
      in_element(V,Ele,Q,aabb,I);
      if(nlhs>1 && !was_serialized)
      {
        aabb.serialize(bb_mins,bb_maxs,elements);
      }
      break;
    }
    case 2:
    {
      AABB<MatrixXd,2> aabb;
      aabb.init(V,Ele,bb_mins,bb_maxs,elements);
      in_element(V,Ele,Q,aabb,I);
      if(nlhs>1 && !was_serialized)
      {
        aabb.serialize(bb_mins,bb_maxs,elements);
      }
      break;
    }
  }

  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 4:
    {
      prepare_lhs_index(elements,plhs+3);
      //fallthrough
    }
    case 3:
    {
      prepare_lhs_double(bb_maxs,plhs+2);
      //fallthrough
    }
    case 2:
    {
      prepare_lhs_double(bb_mins,plhs+1);
      //fallthrough
    }
    case 1:
    {
      prepare_lhs_index(I,plhs+0);
      //fallthrough
    }
    case 0: break;
  }

  std::cout.rdbuf(outbuf);
}

#endif


// discrete_distribution.hpp
//
// Copyright (c) 2009
// Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_RANDOM_DISCRETE_DISTRIBUTION_HPP_INCLUDED
#define BOOST_RANDOM_DISCRETE_DISTRIBUTION_HPP_INCLUDED

#include <vector>
#include <cassert>
#include <limits>
#include <numeric>
#include <utility>

#include <boost/range.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>

namespace boost {
namespace random {

template<class IntType, class WeightType>
class discrete_distribution {
public:
    typedef WeightType input_type;
    typedef IntType result_type;

    template<class Iter>
    discrete_distribution(Iter begin, Iter end) : weights(begin, end), data(weights.size()) {
        std::size_t size = weights.size();
        //assert(size <= (std::numeric_limits<IntType>::max)());
        std::vector<std::pair<WeightType, IntType> > below_average;
        std::vector<std::pair<WeightType, IntType> > above_average;
        WeightType weight_sum = std::accumulate(weights.begin(), weights.end(), static_cast<WeightType>(0));
        WeightType weight_average = weight_sum / size;
        for(std::size_t i = 0; i < size; ++i) {
            if(weights[i] < weight_average) {
                below_average.push_back(std::make_pair(weights[i] / weight_average, static_cast<IntType>(i)));
            } else {
                above_average.push_back(std::make_pair(weights[i] / weight_average, static_cast<IntType>(i)));
            }
        }
        typedef typename range_iterator<
            std::vector<
                std::pair<WeightType, IntType>
            >
        >::type iter_;
            iter_ b_iter = below_average.begin();
            iter_ b_end = below_average.end();
            iter_ a_iter = above_average.begin();
            iter_ a_end = above_average.end();
        while(b_iter != b_end && a_iter != a_end) {
            data[b_iter->second] = std::make_pair(b_iter->first, a_iter->second);
            a_iter->first -= (1 - b_iter->first);
            if(a_iter->first < 1) {
                *b_iter = *a_iter++;
            } else {
                ++b_iter;
            }
        }
        for(; b_iter != b_end; ++b_iter) {
            data[b_iter->second].first = 1;
        }
        for(; a_iter != a_end; ++a_iter) {
            data[a_iter->second].first = 1;
        }
    }
    template<class Engine>
    IntType operator()(Engine& eng) const {
        assert(!data.empty());
        boost::variate_generator<Engine&, boost::uniform_01<WeightType> > real_gen(eng, boost::uniform_01<WeightType>());
        WeightType test = real_gen() * data.size();
        IntType result = static_cast<IntType>(test);
        if(test - result < data[result].first) {
            return result;
        } else {
            return(data[result].second);
        }
    }

    result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
    result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return static_cast<result_type>(weights.size() - 1); }
private:
    std::vector<WeightType> weights;
    std::vector<std::pair<WeightType, IntType> > data;
};

}
}

#endif


#include "common/common.hpp"

#include "geodb/hilbert.hpp"

#include <boost/program_options.hpp>
#include <fmt/format.h>
#include <fmt/ostream.h>

#include <iostream>

namespace po = boost::program_options;

using std::cout;
using std::cerr;

void parse_options(int argc, char** argv);

template<u32 Dimension, u32 Precision>
json curve_json();

int main(int argc, char** argv) {
    return tpie_main([&]{
        parse_options(argc, argv);

        json curves = json::array({
            curve_json<2, 1>(),
            curve_json<2, 2>(),
            curve_json<2, 3>(),
            curve_json<3, 1>(),
            curve_json<3, 2>(),
            curve_json<3, 3>(),
        });

        fmt::print(cout, "{}\n", curves.dump(4));
        return 0;
    });
}

void parse_options(int argc, char** argv) {
    po::options_description options("Options");
    options.add_options()
            ("help,h", "Show this message.");

    po::variables_map vm;
    try {
        po::command_line_parser p(argc, argv);
        p.options(options);
        po::store(p.run(), vm);

        if (vm.count("help")) {
            fmt::print(cerr, "Usage: {0}\n"
                             "\n"
                             "Outputs hilbert curve points for certain dimensions and precisions.\n"
                             "\n"
                             "{1}",
                       argv[0], options);
            throw exit_main(0);
        }

        po::notify(vm);
    } catch (const po::error& e) {
        fmt::print(cerr, "Failed to parse arguments: {}.\n", e.what());
        throw exit_main(1);
    }
}

/// Creates a vector of points that represents a walk over
/// the hilbert curve with the given Dimension and Precision.
template<u32 Dimension, u32 Precision>
auto curve_points()
{
    using curve_t = geodb::hilbert_curve<Dimension, Precision>;
    using point_t = typename curve_t::point_t;
    using index_t = typename curve_t::index_t;

    const index_t count = curve_t::index_count;

    std::vector<point_t> result(count);
    for (index_t i = 0; i < count; ++i) {
        result[i] = curve_t::hilbert_index_inverse(i);
    }
    return result;
}

template<u32 Dimension, u32 Precision>
json curve_json() {
    const auto points = curve_points<Dimension, Precision>();

    json json_data = json::object();
    json_data["dimension"] = Dimension;
    json_data["precision"] = Precision;

    json& json_points = json_data["points"] = json::array();
    for (const auto& point : points) {
        json jp = json::array();
        for (const auto& coord : point) {
            jp.push_back(coord.to_ullong());
        }
        json_points.push_back(jp);
    }
    return json_data;
}


#include "edlib/Solver/ArpackSolver.hpp"

#include <Eigen/Sparse>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/SymEigsSolver.h>

#include <catch2/catch.hpp>

#include <iostream>
#include <random>

constexpr uint32_t max_iter = 1000;
constexpr double tol = 1e-10;
constexpr uint32_t n_evals = 3;

template<typename MatProd>
auto eigenvectorSpectra(MatProd& prod) -> std::pair<Eigen::VectorXd, Eigen::MatrixXd>
{
    using Spectra::SortRule;
    Spectra::SymEigsSolver<MatProd> eigs(prod, n_evals, 2 * n_evals + 1);
    eigs.init();
    eigs.compute(SortRule::SmallestAlge, max_iter, tol, SortRule::SmallestAlge);
    return std::pair{eigs.eigenvalues(), eigs.eigenvectors()};
}

template<typename MatProd>
auto eigenvectorArpack(MatProd& prod) -> std::pair<Eigen::VectorXd, Eigen::MatrixXd>
{
    using namespace edlib;
    ArpackSolver solver(prod);
    solver.solve(n_evals, max_iter, tol);
    return std::pair{solver.eigenvalues(), solver.eigenvectors()};
}

TEST_CASE("Test arpack solver", "[arpack_solver]")
{
    using Catch::Matchers::WithinAbs;
    using std::abs;

    constexpr uint32_t dim = 1024;
    constexpr uint32_t n_elts = 128;

    std::mt19937 re{1337};
    std::uniform_int_distribution<uint32_t> index_dist(0, dim - 1);
    std::normal_distribution<double> elt_dist;

    Eigen::SparseMatrix<double> m(dim, dim);

    for(size_t idx = 0; idx < n_elts; ++idx)
    {
        const auto row = index_dist(re);
        const auto col = index_dist(re);
        const auto val = elt_dist(re);
        m.coeffRef(row, col) = val; // NOLINT(readability-suspicious-call-argument)
        m.coeffRef(col, row) = val; // NOLINT(readability-suspicious-call-argument)
    }
    m.makeCompressed();

    Spectra::SparseSymMatProd<double> prod(m);

    const auto [evals_spectra, evecs_spectra] = eigenvectorSpectra(prod);
    const auto [evals_arpack, evecs_arpack] = eigenvectorArpack(prod);

    REQUIRE((evals_spectra - evals_arpack).lpNorm<1>() < 1e-8);

    for(uint32_t i = 0; i < n_evals; i++)
    {
        double inner_prod = evecs_spectra.col(i).transpose() * evecs_arpack.col(i);
        REQUIRE_THAT(abs(inner_prod), WithinAbs(1.0, 1e-8));
    }
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2014 Erik Erlandson
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <sstream>

#include <boost/units/quantity.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/io.hpp>

#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/time.hpp>

// All information systems definitions
#include <boost/units/systems/information.hpp>

using std::cout;
using std::cerr;
using std::endl;
using std::stringstream;

namespace bu = boost::units;
namespace si = boost::units::si;

using bu::quantity;

using bu::information::bit_base_unit;
using bu::information::byte_base_unit;
using bu::information::nat_base_unit;
using bu::information::hartley_base_unit;
using bu::information::shannon_base_unit;

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/multiprecision/cpp_int.hpp>

const double close_fraction = 0.0000001;

// checks that cf(u2,u1) == expected
// also checks invariant property that cf(u2,u1) * cf(u1,u2) == 1
#define CHECK_DIRECT_CF(u1, u2, expected) \
    BOOST_CHECK_CLOSE_FRACTION(bu::conversion_factor((u2), (u1)), (expected), close_fraction); \
    BOOST_CHECK_CLOSE_FRACTION(bu::conversion_factor((u2), (u1)) * bu::conversion_factor((u1), (u2)), 1.0, close_fraction);

// check transitive conversion factors
// invariant:  cf(u1,u3) = cf(u1,u2)*cf(u2,u3)
#define CHECK_TRANSITIVE_CF(u1, u2, u3) { \
    double cf12 = bu::conversion_factor((u2), (u1)) ; \
    double cf23 = bu::conversion_factor((u3), (u2)) ; \
    double cf13 = bu::conversion_factor((u3), (u1)) ; \
    BOOST_CHECK_CLOSE_FRACTION(cf13, cf12*cf23, close_fraction); \
    double cf32 = bu::conversion_factor((u2), (u3)) ; \
    double cf21 = bu::conversion_factor((u1), (u2)) ; \
    double cf31 = bu::conversion_factor((u1), (u3)) ; \
    BOOST_CHECK_CLOSE_FRACTION(cf31, cf32*cf21, close_fraction); \
}

BOOST_AUTO_TEST_CASE(test_cf_bit_byte) {
    CHECK_DIRECT_CF(bit_base_unit::unit_type(), byte_base_unit::unit_type(), 8.0);
}

BOOST_AUTO_TEST_CASE(test_cf_bit_nat) {
    CHECK_DIRECT_CF(bit_base_unit::unit_type(), nat_base_unit::unit_type(), 1.442695040888964);
}

BOOST_AUTO_TEST_CASE(test_cf_bit_hartley) {
    CHECK_DIRECT_CF(bit_base_unit::unit_type(), hartley_base_unit::unit_type(), 3.321928094887363);
}

BOOST_AUTO_TEST_CASE(test_cf_bit_shannon) {
    CHECK_DIRECT_CF(bit_base_unit::unit_type(), shannon_base_unit::unit_type(), 1.0);
}

/////////////////////////////////////////////////////////////////////////////////////
// spot-check that these are automatically transitive, thru central "hub unit" bit:
// basic pattern is to test invariant property:  cf(c,a) = cf(c,b)*cf(b,a)

BOOST_AUTO_TEST_CASE(test_transitive_byte_nat) {
    CHECK_TRANSITIVE_CF(byte_base_unit::unit_type(), bit_base_unit::unit_type(), nat_base_unit::unit_type());
}
BOOST_AUTO_TEST_CASE(test_transitive_nat_hartley) {
    CHECK_TRANSITIVE_CF(nat_base_unit::unit_type(), bit_base_unit::unit_type(), hartley_base_unit::unit_type());
}
BOOST_AUTO_TEST_CASE(test_transitive_hartley_shannon) {
    CHECK_TRANSITIVE_CF(hartley_base_unit::unit_type(), bit_base_unit::unit_type(), shannon_base_unit::unit_type());
}
BOOST_AUTO_TEST_CASE(test_transitive_shannon_byte) {
    CHECK_TRANSITIVE_CF(shannon_base_unit::unit_type(), bit_base_unit::unit_type(), byte_base_unit::unit_type());
}

// test transitive factors, none of which are bit, just for good measure
BOOST_AUTO_TEST_CASE(test_transitive_byte_nat_hartley) {
    CHECK_TRANSITIVE_CF(byte_base_unit::unit_type(), nat_base_unit::unit_type(), hartley_base_unit::unit_type());
}

BOOST_AUTO_TEST_CASE(test_byte_quantity_is_default) {
    using namespace bu::information;
    quantity<info, double> qd(2 * byte);
    BOOST_CHECK_EQUAL(qd.value(), double(2));
    quantity<info, long> ql(2 * byte);
    BOOST_CHECK_EQUAL(ql.value(), long(2));
}

BOOST_AUTO_TEST_CASE(test_byte_quantity_explicit) {
    using namespace bu::information;
    quantity<hu::byte::info, double> qd(2 * byte);
    BOOST_CHECK_EQUAL(qd.value(), double(2));
    quantity<hu::byte::info, long> ql(2 * byte);
    BOOST_CHECK_EQUAL(ql.value(), long(2));
}

BOOST_AUTO_TEST_CASE(test_bit_quantity) {
    using namespace bu::information;
    quantity<hu::bit::info, double> qd(2 * bit);
    BOOST_CHECK_EQUAL(qd.value(), double(2));
    quantity<hu::bit::info, long> ql(2 * bit);
    BOOST_CHECK_EQUAL(ql.value(), long(2));
}

BOOST_AUTO_TEST_CASE(test_nat_quantity) {
    using namespace bu::information;
    quantity<hu::nat::info, double> qd(2 * nat);
    BOOST_CHECK_EQUAL(qd.value(), double(2));
    quantity<hu::nat::info, long> ql(2 * nat);
    BOOST_CHECK_EQUAL(ql.value(), long(2));
}

BOOST_AUTO_TEST_CASE(test_hartley_quantity) {
    using namespace bu::information;
    quantity<hu::hartley::info, double> qd(2 * hartley);
    BOOST_CHECK_EQUAL(qd.value(), double(2));
    quantity<hu::hartley::info, long> ql(2 * hartley);
    BOOST_CHECK_EQUAL(ql.value(), long(2));
}

BOOST_AUTO_TEST_CASE(test_shannon_quantity) {
    using namespace bu::information;
    quantity<hu::shannon::info, double> qd(2 * shannon);
    BOOST_CHECK_EQUAL(qd.value(), double(2));
    quantity<hu::shannon::info, long> ql(2 * shannon);
    BOOST_CHECK_EQUAL(ql.value(), long(2));
}

BOOST_AUTO_TEST_CASE(test_mixed_hu) {
    using namespace bu::information;
    const double cf = 0.001;
    BOOST_CHECK_CLOSE_FRACTION((quantity<hu::bit::info>(1.0 * bits)).value(), 1.0, cf);
    BOOST_CHECK_CLOSE_FRACTION((quantity<hu::byte::info>(1.0 * bits)).value(), 1.0/8.0, cf);
    BOOST_CHECK_CLOSE_FRACTION((quantity<hu::nat::info>(1.0 * bits)).value(), 0.69315, cf);
    BOOST_CHECK_CLOSE_FRACTION((quantity<hu::hartley::info>(1.0 * bits)).value(), 0.30102, cf);
    BOOST_CHECK_CLOSE_FRACTION((quantity<hu::shannon::info>(1.0 * bits)).value(), 1.0, cf);
}

BOOST_AUTO_TEST_CASE(test_info_prefixes) {
    using namespace bu::information;
    quantity<info, long long> q10(1LL * kibi * byte);
    BOOST_CHECK_EQUAL(q10.value(), 1024LL);

    quantity<info, long long> q20(1LL * mebi * byte);
    BOOST_CHECK_EQUAL(q20.value(), 1048576LL);

    quantity<info, long long> q30(1LL * gibi * byte);
    BOOST_CHECK_EQUAL(q30.value(), 1073741824LL);

    quantity<info, long long> q40(1LL * tebi * byte);
    BOOST_CHECK_EQUAL(q40.value(), 1099511627776LL);

    quantity<info, long long> q50(1LL * pebi * byte);
    BOOST_CHECK_EQUAL(q50.value(), 1125899906842624LL);

    quantity<info, long long> q60(1LL * exbi * byte);
    BOOST_CHECK_EQUAL(q60.value(), 1152921504606846976LL);

    using boost::multiprecision::int128_t;

    quantity<info, int128_t> q70(1LL * zebi * byte);
    BOOST_CHECK_EQUAL(q70.value(), int128_t("1180591620717411303424"));

    quantity<info, int128_t> q80(1LL * yobi * byte);
    BOOST_CHECK_EQUAL(q80.value(), int128_t("1208925819614629174706176"));

    // sanity check: si prefixes should also operate
    quantity<info, long long> q1e3(1LL * si::kilo * byte);
    BOOST_CHECK_EQUAL(q1e3.value(), 1000LL);

    quantity<info, long long> q1e6(1LL * si::mega * byte);
    BOOST_CHECK_EQUAL(q1e6.value(), 1000000LL);
}

BOOST_AUTO_TEST_CASE(test_unit_constant_io) {
    using namespace bu::information;

    std::stringstream ss;
    ss << bu::symbol_format << bytes;
    BOOST_CHECK_EQUAL(ss.str(), "B");

    ss.str("");
    ss << bu::name_format << bytes;
    BOOST_CHECK_EQUAL(ss.str(), "byte");

    ss.str("");
    ss << bu::symbol_format << bits;
    BOOST_CHECK_EQUAL(ss.str(), "b");

    ss.str("");
    ss << bu::name_format << bits;
    BOOST_CHECK_EQUAL(ss.str(), "bit");

    ss.str("");
    ss << bu::symbol_format << nats;
    BOOST_CHECK_EQUAL(ss.str(), "nat");

    ss.str("");
    ss << bu::name_format << nats;
    BOOST_CHECK_EQUAL(ss.str(), "nat");

    ss.str("");
    ss << bu::symbol_format << hartleys;
    BOOST_CHECK_EQUAL(ss.str(), "Hart");

    ss.str("");
    ss << bu::name_format << hartleys;
    BOOST_CHECK_EQUAL(ss.str(), "hartley");

    ss.str("");
    ss << bu::symbol_format << shannons;
    BOOST_CHECK_EQUAL(ss.str(), "Sh");

    ss.str("");
    ss << bu::name_format << shannons;
    BOOST_CHECK_EQUAL(ss.str(), "shannon");
}