// Copyright Nick Thompson, 2017
// 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)

#define BOOST_TEST_MODULE sinh_sinh_quadrature_test

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/quadrature/sinh_sinh.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#if !BOOST_WORKAROUND(BOOST_MSVC, < 1900)
// MSVC-12 has problems if we include 2 different multiprecision types in the same program,
// basically random things stop compiling, even though they work fine in isolation :(
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif

using std::expm1;
using std::exp;
using std::sin;
using std::cos;
using std::atan;
using std::tan;
using std::log;
using std::log1p;
using std::asinh;
using std::atanh;
using std::sqrt;
using std::isnormal;
using std::abs;
using std::sinh;
using std::tanh;
using std::cosh;
using std::pow;
using std::string;
using boost::multiprecision::cpp_bin_float_quad;
using boost::math::quadrature::sinh_sinh;
using boost::math::constants::pi;
using boost::math::constants::pi_sqr;
using boost::math::constants::half_pi;
using boost::math::constants::two_div_pi;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::catalan;
using boost::math::constants::ln_two;
using boost::math::constants::root_two;
using boost::math::constants::root_two_pi;
using boost::math::constants::root_pi;
//
// Code for generating the coefficients:
//
template <class T>
void print_levels(const T& v, const char* suffix)
{
   std::cout << "{\n";
   for (unsigned i = 0; i < v.size(); ++i)
   {
      std::cout << "      { ";
      for (unsigned j = 0; j < v[i].size(); ++j)
      {
         std::cout << v[i][j] << suffix << ", ";
      }
      std::cout << "},\n";
   }
   std::cout << "   };\n";
}

template <class T>
void print_levels(const std::pair<T, T>& p, const char* suffix = "")
{
   std::cout << "   static const std::vector<std::vector<Real> > abscissa = ";
   print_levels(p.first, suffix);
   std::cout << "   static const std::vector<std::vector<Real> > weights = ";
   print_levels(p.second, suffix);
}

template <class Real, class TargetType>
std::pair<std::vector<std::vector<Real>>, std::vector<std::vector<Real>> > generate_constants(unsigned max_rows)
{
   using boost::math::constants::half_pi;
   using boost::math::constants::two_div_pi;
   using boost::math::constants::pi;
   auto g = [](Real t) { return sinh(half_pi<Real>()*sinh(t)); };
   auto w = [](Real t) { return cosh(t)*half_pi<Real>()*cosh(half_pi<Real>()*sinh(t)); };

   std::vector<std::vector<Real>> abscissa, weights;

   std::vector<Real> temp;

   Real t_max = log(2 * two_div_pi<Real>()*log(2 * two_div_pi<Real>()*sqrt(boost::math::tools::max_value<TargetType>())));

   std::cout << "m_t_max = " << t_max << ";\n";

   Real h = 1;
   for (Real t = 1; t < t_max; t += h)
   {
      temp.push_back(g(t));
   }
   abscissa.push_back(temp);
   temp.clear();

   for (Real t = 1; t < t_max; t += h)
   {
      temp.push_back(w(t * h));
   }
   weights.push_back(temp);
   temp.clear();

   for (unsigned row = 1; row < max_rows; ++row)
   {
      h /= 2;
      for (Real t = h; t < t_max; t += 2 * h)
         temp.push_back(g(t));
      abscissa.push_back(temp);
      temp.clear();
   }
   h = 1;
   for (unsigned row = 1; row < max_rows; ++row)
   {
      h /= 2;
      for (Real t = h; t < t_max; t += 2 * h)
         temp.push_back(w(t));
      weights.push_back(temp);
      temp.clear();
   }

   return std::make_pair(abscissa, weights);
}

template<class Real>
void test_nr_examples()
{
    std::cout << "Testing type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real integration_limit = sqrt(boost::math::tools::epsilon<Real>());
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    Real Q;
    Real Q_expected;
    Real L1;
    Real error;
    sinh_sinh<Real> integrator(10);

    auto f0 = [](Real)->Real { return (Real) 0; };
    Q = integrator.integrate(f0, integration_limit, &error, &L1);
    Q_expected = 0;
    BOOST_CHECK_SMALL(Q, tol);
    BOOST_CHECK_SMALL(L1, tol);

    // In spite of the poles at \pm i, we still get a doubling of the correct digits at each level of refinement.
    auto f1 = [](const Real& t) { return 1/(1+t*t); };
    Q = integrator.integrate(f1, integration_limit, &error, &L1);
    Q_expected = pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
#if defined(BOOST_MSVC) && (BOOST_MSVC < 1900)
    auto f2 = [](const Real& x) { return fabs(x) > boost::math::tools::log_max_value<Real>() ? 0 : exp(-x*x); };
#else
    auto f2 = [](const Real& x) { return exp(-x*x); };
#endif
    Q = integrator.integrate(f2, integration_limit, &error, &L1);
    Q_expected = root_pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    auto f5 = [](const Real& t) { return 1/cosh(t);};
    Q = integrator.integrate(f5, integration_limit, &error, &L1);
    Q_expected = pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    // This oscillatory integral has rapid convergence because the oscillations get swamped by the exponential growth of the denominator,
    // none the less the error is slightly higher than for the other cases:
    tol *= 10;
    auto f8 = [](const Real& t) { return cos(t)/cosh(t);};
    Q = integrator.integrate(f8, integration_limit, &error, &L1);
    Q_expected = pi<Real>()/cosh(half_pi<Real>());
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    // Try again with progressively fewer arguments:
    Q = integrator.integrate(f8, integration_limit);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    Q = integrator.integrate(f8);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
}

// Test formulas for in the CRC Handbook of Mathematical functions, 32nd edition.
template<class Real>
void test_crc()
{
    std::cout << "Testing CRC formulas on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real integration_limit = sqrt(boost::math::tools::epsilon<Real>());
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    Real Q;
    Real Q_expected;
    Real L1;
    Real error;
    sinh_sinh<Real> integrator(10);

    // CRC Definite integral 698:
    auto f0 = [](Real x)->Real {
      using std::sinh;
      if(x == 0) {
        return (Real) 1;
      }
      return x/sinh(x);
    };
    Q = integrator.integrate(f0, integration_limit, &error, &L1);
    Q_expected = pi_sqr<Real>()/2;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    // CRC Definite integral 695:
    auto f1 = [](Real x)->Real {
      using std::sin; using std::sinh;
      if(x == 0) {
        return (Real) 1;
      }
      return (Real) sin(x)/sinh(x);
    };
    Q = integrator.integrate(f1, integration_limit, &error, &L1);
    Q_expected = pi<Real>()*tanh(half_pi<Real>());
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
}

BOOST_AUTO_TEST_CASE(sinh_sinh_quadrature_test)
{
    //
    // Uncomment the following to print out the coefficients:
    //
    /*
    std::cout << std::scientific << std::setprecision(8);
    print_levels(generate_constants<cpp_bin_float_100, float>(8), "f");
    std::cout << std::setprecision(18);
    print_levels(generate_constants<cpp_bin_float_100, double>(8), "");
    std::cout << std::setprecision(35);
    print_levels(generate_constants<cpp_bin_float_100, cpp_bin_float_quad>(8), "L");
    */
    test_nr_examples<float>();
    test_nr_examples<double>();
    test_nr_examples<long double>();
    test_nr_examples<cpp_bin_float_quad>();
    test_nr_examples<boost::math::concepts::real_concept>();
#if !BOOST_WORKAROUND(BOOST_MSVC, < 1900)
    test_nr_examples<boost::multiprecision::cpp_dec_float_50>();
#endif

    test_crc<float>();
    test_crc<double>();
    test_crc<long double>();
    test_crc<cpp_bin_float_quad>();
    test_crc<boost::math::concepts::real_concept>();
#if !BOOST_WORKAROUND(BOOST_MSVC, < 1900)
    test_crc<boost::multiprecision::cpp_dec_float_50>();
#endif
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Filename    : algorithms.cpp                                                                        *
 * Project     : Planewalker - Schnorr-Euchner sphere decoder simulation for space-time lattice codes  *
 * Authors     : Pasi Pyrrö, Oliver Gnilke                                                             *
 * Version     : 1.0                                                                                *
 * Copyright   : Aalto University ~ School of Science ~ Department of Mathematics and Systems Analysis *
 * Date        : 9.1.2017                                                                              *
 * Language    : C++ (2011 or newer standard)                                                          *
 * Description : All custom mathematical algorithms not found in Armadillo or STL are collected here   *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define ARMA_NO_DEBUG /* disable Armadillo bound checks for addiotional speed */
#define FPLLL_WITH_ZDOUBLE
#define FPLLL_WITH_LONG_DOUBLE

#include <iostream>
#include <armadillo> /* linear algebra library */
#include <algorithm>
#include <complex>
#include <vector>
#include <string>
#include <set>
#include <random>
#include <omp.h>
#include <tuple>

/* library for LLL reduction (optional) */
#ifdef USE_LLL
#include <fplll.h>
#endif
/* required packages to install:
 *   libmpfr-dev
 *   libgmp-dev
 */

#include "algorithms.hpp"
#include "misc.hpp"

using namespace std;
using namespace arma;

/* Signum function for schorr-euchnerr sphere decoder (sesd)
 * ---------------------------------------------------------
 * Note that sesd_sign(0) = -1
 */
int sesd_sign(double x) {
    return (x <= 0) ? -1 : 1;
}

/* compute greatest common divisor of a and b
 * credits go to: https://codereview.stackexchange.com/a/66735
 */
int gcd(int a, int b) {
    return b == 0 ? a : gcd(b, a % b);
}

/* Rounds x to nearest integer in S
 * --------------------------------
 * E.g. let S = 4-PAM and x = 1.8 then
 * nearest_symbol(1.8) = 3
 * cf. round(1.8) = 2
 */
double nearest_symbol(double x, const vector<int> &S){
    double min = 10e6;
    double d = 0.0;
    double nearest = 0.0;

    for (const int symbol : S) {
        d = fabs(x - symbol);
        if (d < min) {
            nearest = symbol;
            min = d;
        }
    }
    return nearest;
}

double nearest_symbol_floor(double x, const vector<int> &S){

    // for (const int symbol : S) {
    //     if (symbol < x) {
    //         return symbol;
    //     }
    // }
    for (int i = S.size()-1; i >= 0; i--) {
        if (S[i] <= x) {
            return S[i];
        }
    }
    // return S[S.size()-1];
    return S[0];
}

double nearest_symbol_ceil(double x, const vector<int> &S){

    for (auto i = 0u; i < S.size()-1; i++) {
        if (S[i] >= x) {
            return S[i];
        }
    }
    return S[S.size()-1];
}

/* A simple heuristic function based on the idea that the volume of the hypersphere divided by the volume of the fundamental region of the lattice
 * should roughly equal to the number of lattice points inside the hypersphere
 * i.e. this function estimates the squared radius required for the hypersphere in order to have 2^s codewords (lattice points) inside it
 * (for q-PAM we need to multiply the generator matrix by 2 to get the correct volume for the fundamental region)
 */
double estimate_squared_radius(const mat &G, int s){
    int n = params["no_of_matrices"];
    double pi = 3.1415926535897;
    // cout << "lattice constant: " << to_string(sqrt(det(4*G.t()*G))) << endl;
    // cout << "volume of the sphere: " << to_string(pow(pi, n/2.0)*pow(dparams["spherical_shaping_max_power"], n/2)/tgamma(n/2.0 + 1.0)) << endl;

    /* The basic idea: 2^s = vol(Sphere(R))/vol(Lambda)
     * --> Solve for R when s and Lambda (G) are given
     */

    /* This algorithm uses this equation for the volume of the n-ball:
     * https://en.wikipedia.org/wiki/Volume_of_an_n-ball
     */
    return pow(pow(2.0, s)*tgamma(n/2.0 + 1.0)*sqrt(det(4*G.t()*G))*pow(pi, n/-2.0), 2.0/n);
}

/* Generates a new symbolset where elements are between given lower and upper bounds */
vector<int> slice_symbset(const vector<int> &S, double lb, double ub){
    vector<int> retval;
    for (const int q : S){
        if (lb <= (double)q && (double)q <= ub) {
            retval.push_back(q);
        }
    }
    return retval;
}

/* Takes the squared Frobenius norm from a complex matrix A */
double frob_norm_squared(const cx_mat &A){
    double sum = 0;
    for (auto i = 0u; i < A.n_rows; i++)
        for (auto j = 0u; j < A.n_cols; j++)
            sum += norm(A(i,j));
    return sum;
}

/* Makes sure the upper triangular matrix R has only positive diagonal elements */
void process_qr(mat &Q, mat &R){
    for (auto i = 0u; i < R.n_cols; i++){
        if (R(i,i) < 0.0){
            // R.col(i) *= -1;
            // Q.row(i) *= -1;
            R.row(i) *= -1;
            Q.col(i) *= -1;
        }
    }
}

/* Create the lattice generator matrix G out of basis matrices B_i
 * i.e. G = vec(B_i), i = 1,...,k
 */
cx_mat create_generator_matrix(const vector<cx_mat> &bases){
    int m = params["no_of_transmit_antennas"];
    int t = params["time_slots"];
    int k = params["no_of_matrices"];
    cx_mat G(k, m*t);
    for(int i = 0; i < k; i++){
        G.row(i) = vectorise(bases[i], 1);
    }
    return G.st();
}

/***** DEPRECATED *****
 * Use
 * G_real = to_real_matrix(create_generator_matrix(bases));
 * instead
 **********************/

/* Create the real valued lattice generator matrix G out of basis matrices */
// mat create_real_generator_matrix(const vector<cx_mat> &bases){
//     int m = params["no_of_transmit_antennas"];
//     int t = params["time_slots"];
//     int k = params["no_of_matrices"];
//     mat G(2*m*t,k);
//     for(int i = 0; i < k; i++){
//         G.col(i) = to_real_vector(bases[i]);
//     }
//     return G;
// }

/* Inverse of the create_generator_matrix() function
 * Useful for generating basis matrix files in conjunction with
 * output_complex_matrix()
 * See main.cpp for an example of this use case
 */
vector<cx_mat> generator_to_bases(const cx_mat &G){
    int m = params["no_of_transmit_antennas"];
    int t = params["time_slots"];
    int idx = 0;

    vector<cx_mat> bases;
    cx_mat X(m, t);
    cx_vec col(m*t);

    for (auto i = 0u; i < G.n_cols; i++){
        X.zeros();
        col = G.col(i);
        for (int j = 0; j < m; j++){
            for (int s = 0; s < t; s++){
                X(j,s) = col[idx];
                idx++;
            }
        }
        idx = 0;
        bases.push_back(X);
    }
    return bases;
}

/* Vectorizes a complex matrix A to real vector (each row is concatenated) */
vec to_real_vector(const cx_mat &A, bool row_wise){
    vec a(2*A.n_elem);
    // as the imaginary parts are now independent real elements in the vector,
    // we need to double the number of elements in the returned vector
    int index = 0;
    if (row_wise){
        for (auto i = 0u; i < A.n_rows; i++){
            for (auto j = 0u; j < A.n_cols; j++){
                auto z = A(i,j);
                a[index++] = z.real();
                a[index++] = z.imag();
            }
        }
    } else { /* concatenate columns instead */
        for (auto i = 0u; i < A.n_cols; i++){
            for (auto j = 0u; j < A.n_rows; j++){
                auto z = A(j,i);
                a[index++] = z.real();
                a[index++] = z.imag();
            }
        }
    }
    return a;
}

/* Converts a complex matrix A to its real representation */
mat to_real_matrix(const cx_mat &A){
    mat B(2*A.n_rows, A.n_cols);
    for(auto i = 0u; i < A.n_cols; i++){
        B.col(i) = to_real_vector(A.col(i));
    }
    return B;
}

/* Converts a real matrix constructed out of a complex matrix back to complex */
cx_mat to_complex_matrix(const mat &A){
    if (A.n_rows % 2 != 0){
        log_msg("to_complex_matrix: the input matrix needs to have an even number of rows!", "Error");
        exit(1);
    }

    cx_mat B(A.n_rows/2, A.n_cols);
    // int index = 0;
    for (auto j = 0u; j < A.n_cols; j++){
        for (auto i = 0u; i < B.n_rows; i++){
            B(i,j) = complex<double>(A(2*i,j), A(2*i+1,j));
        }
        // index = 0;
    }
    return B;
}

/* generates a random n x m 'full' complex matrix from normal distribution */
cx_mat create_random_matrix(int n, int m, double mean, double variance){
    normal_distribution<double> distr(mean, sqrt(variance));
    cx_mat A(n,m);
    return A.imbue([&]() { /* Armadillo magic with lambda function */
        return complex<double>(distr(mersenne_twister), distr(mersenne_twister));
    });
}

/* generates a random n x n diagonal complex matrix from normal distribution
 * Used for generating the SISO channel matrix
 */
cx_mat create_random_diag_matrix(int n, double mean, double variance){
    normal_distribution<double> distr(mean, sqrt(variance));
    cx_vec diag(n);
    cx_mat A(n,n);

    A.zeros();
    diag.imbue([&]() { /* Armadillo magic with lambda function */
        return complex<double>(distr(mersenne_twister), 0.0);
    });
    A.diag() = diag;
    return A;
}

/* Creates q-PAM symbol set (integer vector)
 * q is usually some power of two, i.e. q = 2, 4, 8, 16...
 * Example: 4-PAM = {-3, -1, 1, 3}
 */
vector<int> create_symbolset(int q){
    vector<int> symbset(q);
    for (int u = 0; u < q; u++) {
        symbset[u] = 2*u - q + 1;
    }
    return symbset;
}

/* Calculates all possible combinations of elements for code vector _a_
   given the set of feasible symbols (element values)
   Used for generating the whole codebook, rarely needed though */
void combinations(parallel_set< vector<int> > &comblist, const vector<int> &symbset, vector<int> comb, int dim){
    comblist.par_insert(comb);
    if (dim >= 0){
        #pragma omp parallel
        {
            vector<int> new_comb = comb;
            #pragma omp for
            for (size_t i = 0; i < symbset.size(); i++){
                new_comb[dim] = symbset[i];
                combinations(comblist, symbset, new_comb, dim-1);
            }
        }
    }
}

/* Helper function for above combinations algorithm */
set< vector<int> > comb_wrapper(const vector<int> &symbset, int vector_len){
    parallel_set< vector<int> > comblist;
    vector<int> init(vector_len);
    for (int i = 0; i < vector_len; i++)
        init[i] = symbset[0];
    combinations(comblist, symbset, init, vector_len-1);
    return comblist;
}

/* Creates a random codeword from basis matrices B_i and symbolset x-PAM */
pair<vector<int>, cx_mat> create_random_codeword(const vector<cx_mat> &bases, const vector<int> &symbolset){
    int m = params["no_of_transmit_antennas"];
    int t = params["time_slots"];
    int k = params["no_of_matrices"];
    int q = params["x-PAM"];
    int random_index = 0;

    cx_mat X(m, t);
    vector<int> coeffs(k);
    X.zeros();

    uniform_int_distribution<int> dist(0, q-1);

    for (int i = 0; i < k; i++) {
        random_index = dist(mersenne_twister);
        coeffs[i] = symbolset[random_index];
        X = X + symbolset[random_index]*bases[i];
    }

    return make_pair(coeffs, X);
}

/* Attempts to take account the probability bias when picking vector coefficients uniform randomly
   from hyperplane intervals within a hypersphere */
// vector<int> create_unbiased_subset(const mat &R, const vector<int> &subset, vec xt,
//                                    vec ener, vec curr, double radius, int i){
//     if (i <= 0) return subset; /* can't perform lookahead for the lowest dimension */

//     vector<int> unbiased_subset; //, delta_vec;
//     unbiased_subset.reserve(4*subset.size());

//     double xiener = 0.0;
//     int ub = 0, lb = 0, delta = 0; //, delta_sum = 0, bias_correction_amount = 0;
//     int k = params["no_of_matrices"];

//     // cout << i << endl;
//     // cout << vec2str(xt, xt.size()) << endl;
//     // cout << vec2str(ener, ener.size()) << endl;
//     // cout << vec2str(curr, curr.size()) << endl << endl;

//     for (const int elem : subset) {
//         xt[i] = elem;
//         xiener = pow(R(i,i)*xt[i] + curr[i], 2);

//         for (int j = i; j < k; j++)
//             curr[i-1] += xt[j]*R(i-1,j);
//         ener[i-1] = ener[i] + xiener;

//         lb = nearest_symbol_ceil(-(sqrt(radius - ener[i-1]) + curr[i-1])/R(i-1,i-1), subset);
//         ub = nearest_symbol_floor((sqrt(radius - ener[i-1]) - curr[i-1])/R(i-1,i-1), subset);
//         delta = (ub - lb)/2 + 1;

//         // cout << lb << ", " << ub << ", " << delta << endl;

//         for (int d = 0; d < delta; d++)
//             unbiased_subset.push_back(elem);
//         // delta_vec.push_back(delta);
//         // delta_sum += delta;
//     }
//     // cout << vec2str(xt, xt.size()) << endl;
//     // cout << vec2str(ener, ener.size()) << endl;
//     // cout << vec2str(curr, curr.size()) << endl << endl;
//     // cout << endl;

//     // for (auto j = 0u; j < subset.size(); j++){
//     //     bias_correction_amount = delta_sum/gcd(delta_sum, delta_vec[j]);
//     //     for (int s = 0; s < bias_correction_amount; s++)
//     //         unbiased_subset.push_back(subset[j]);
//     // }
//     // cout << vec2str(unbiased_subset, unbiased_subset.size()) << endl << endl;
//     return unbiased_subset;
// }

/* Attempts to pick q-PAM coefficients which correspond to large subhyperspheres more often
   This should eliminate some the bias present (picking border points too often) in naive uniform random picking */
int unbiased_random_select(const mat &R, const vector<int> &subset, vec xt,
                                   vec ener, vec curr, double radius, int i){
    if (i <= 0) return 0; /* can't perform lookahead for the lowest dimension */

    vector<int> unbiased_subset, feasible_coeffs;
    vector<double> radiuses;
    vector<pair<double, double>> intervals;
    unbiased_subset.reserve(4*subset.size());
    radiuses.reserve(subset.size());
    intervals.reserve(subset.size());
    feasible_coeffs.reserve(subset.size());

    double xiener = 0.0, radius_sum = 0.0, r = 0.0, numer = 0.0, ub = 0, lb = 0;
    int k = params["no_of_matrices"];

    /* calculate subsphere radiuses for each coefficient */
    for (auto j = 0u; j < subset.size(); j++) {
        xt[i] = subset[j];
        xiener = pow(R(i,i)*xt[i] + curr[i], 2);
        r = radius - xiener - ener[i];
        if (r > 0){ /* We don't want negative radiuses, do we? */
            r = pow(r, k-1);
            radiuses.push_back(r);
            radius_sum += r;
            feasible_coeffs.push_back(subset[j]);
        }
    }

    /* Calculate intervals from [0,1] for each coefficient */
    for (auto j = 0u; j < radiuses.size(); j++) {
        lb = numer/radius_sum;
        ub = (numer+radiuses[j])/radius_sum;
        intervals.push_back(make_pair(lb, ub));
        numer += radiuses[j];
    }

    // intervals.back().second = 1.0;

    // cout << vec2str(radiuses, radiuses.size()) << endl;
    // cout << vec2str(feasible_coeffs, feasible_coeffs.size()) << endl;
    // cout << vec2str(intervals, intervals.size()) << endl << endl;
    // cout << endl;

    // uniform_real_distribution<double> dist(0.0, 1.0);
    double X = uniform_real_dist(mersenne_twister);
    // cout << X << endl << endl;

    /* Select the coefficient from distribution created earlier (weights large radiuses) */
    for (auto j = 0u; j < feasible_coeffs.size(); j++) {
        // cout << "[" << intervals[j].first << ", " << intervals[j].second << "]" << endl;
        if (intervals[j].first <= X && X <= intervals[j].second)
            return feasible_coeffs[j];
    }
    // cout << "------" << endl;

    return 0;
}

/* Creates a random codeword from basis matrices B_i and symbolset x-PAM within given radius (spherical shaping) */
pair<vector<int>, cx_mat> create_random_spherical_codeword(const vector<cx_mat> &bases, const mat &R, const vector<int> &S, double radius){
    int m = params["no_of_transmit_antennas"];
    int t = params["time_slots"];
    int k = params["no_of_matrices"];

    cx_mat X(m, t);
    vector<int> coeffs(k), subset;
    X.zeros();

    vec xt(k), curr(k), ener(k);
    xt.zeros(); curr.zeros(); ener.zeros();
    int i = k-1;
    double xiener = 0.0;
    double lb = 0.0; // lower bound
    double ub = 0.0; // upper bound

    // radius = sqrt(radius);

    /* Randomly pick each component of the codeword and check if it's still within the energy bound in each dimension */
    while (i >= 0){
        lb = -(sqrt(radius - ener[i]) + curr[i])/R(i,i);
        ub = (sqrt(radius - ener[i]) - curr[i])/R(i,i);
        // uniform_real_distribution<double> xirange(lb, ub);
        // xt[i] = nearest_symbol(xirange(mersenne_twister), S); // probability bias fix this
        subset = slice_symbset(S, lb, ub);
        // if (i >= k-10 /*max(k-13, 3)*/ && !subset.empty())
        //     subset = create_unbiased_subset(R, subset, xt, ener, curr, radius, i);

        if (subset.empty()) { /* No lattice points are within energy bounds in this dimension */
            i = k-1;
            xt.zeros(); curr.zeros(); ener.zeros();
            continue;
        }

        // if (i > 0)
        //     xt[i] = unbiased_random_select(R, subset, xt, ener, curr, radius, i);
        // else
        xt[i] = pick_uniform(subset);

        if (xt[i] == 0) {
            // cout << "something went wrong" << endl;
            i = k-1;
            xt.zeros(); curr.zeros(); ener.zeros();
            continue;
        }

        xiener = pow(R(i,i)*xt[i] + curr[i], 2);

        // if (xiener + ener[i] < radius + 1e-6) {
        if (i > 0) {
            for (int j = i; j < k; j++)
                curr[i-1] += xt[j]*R(i-1,j);
            ener[i-1] = ener[i] + xiener;
        }
        i--;
        // } else { /* we're outside the energy bound, start over */
        //     log_msg("test!", "Error");
        //     i = k-1;
        //     xt.zeros(); curr.zeros(); ener.zeros();
        // }
    }
    coeffs = conv_to<vector<int>>::from(xt);
    for (int i = 0; i < k; i++) {
        X = X + coeffs[i]*bases[i];
    }
    return make_pair(coeffs, X);
}

/* Creates a codebook from basis matrices B_i and symbolset x-PAM within given radius (spherical shaping) */
// vector<pair<vector<int>, cx_mat>> create_spherical_codebook(const vector<cx_mat> &bases, const mat &R, const vector<int> &S, double radius){
vector<pair<vector<int>, cx_mat>> create_spherical_codebook(const vector<cx_mat> &bases, const mat &R, const vector<int> &S,
                                                            double radius, vec xt, int dim, double dist) {
    int m = params["no_of_transmit_antennas"];
    int t = params["time_slots"];
    int k = params["no_of_matrices"];

    cx_mat X(m, t, fill::zeros);
    vector<int> coeffs(k); //, subset;
    vector<pair<vector<int>,cx_mat>> codebook, part;

    // vec xt(k), curr(k), ener(k), indices(k);
    // xt.zeros(); curr.zeros(); ener.zeros(); indices.zeros();
    // int i = k-1;
    int i = dim-1;
    double xidist = 0;
    // double xiener = 0.0;
    // double lb = 0.0; // lower bound
    // double ub = 0.0; // upper bound
    // bool nextlevel = true;

    // while (true) {
    //  if (nextlevel) {
       //      lb = -(sqrt(radius - ener[i]) + curr[i])/R(i,i);
       //      ub = (sqrt(radius - ener[i]) - curr[i])/R(i,i);
       //      subset = slice_symbset(S, lb, ub);
       //      if (subset.empty()) { /* No lattice points are within energy bounds in this dimension */
       //       cout << "empty interval!" << endl;
       //       if (i == k - 1) break;
       //       else {
       //           indices[i] = 0;
       //           // indices[i+1]++;
          //           i++;
          //           continue;
          //       }
       //      }
       //      nextlevel = false;
       //  }
    //      xt[i] = subset[indices[i]];

    //     xiener = pow(R(i,i)*xt[i] + curr[i], 2);

    //     cout << i << ": " << vec2str(xt, k) << ", xiener = " << xiener + ener[i] << " interval: [" << lb << ", " << ub << "], xt[i] = " << xt[i] << endl;
    //     cout << "subset: " << vec2str(subset, subset.size()) << endl;

    //     // if (xiener + ener[i] < radius + 1e-6) {
    //     if (i > 0) {
    //      curr[i-1] = 0;
    //         for (int j = i; j < k; j++)
    //             curr[i-1] += xt[j]*R(i-1,j);
    //         ener[i-1] = ener[i] + xiener;
    //         i--;
    //         nextlevel = true;
    //         cout << "lowering dimension!" << endl;
    //         // continue;
    //     } else {
    //      coeffs = conv_to<vector<int>>::from(xt);
    //      X.zeros();
    //      for (int i = 0; i < k; i++) {
          //       X = X + coeffs[i]*bases[i];
          //   }
          //   cout << "found point!" << endl;
          //   codebook.push_back(make_pair(coeffs, X));
    //     }
    //     // }
    //     if (indices[i] >= subset.size() - 1){
    //      cout << "subset clear!" << endl;
    //      if (i == k - 1) break;
       //   else {
       //       indices[i] = 0;
       //       // indices[i+1]++;
       //       i++;
       //       nextlevel = true;
       //      }
    //     } else indices[i]++;
    // }
    // return codebook;

    // #pragma omp parallel for
    for (auto j = 0u; j < S.size(); j++) {
        vec tmp = xt;
        tmp[i] = S[j];
        xidist = pow(dot(R.row(i).subvec(i, k-1), tmp.subvec(i, k-1)), 2) + dist;
        if (xidist <= radius) {
            if (i > 0) {
                part = create_spherical_codebook(bases, R, S, radius, tmp, dim-1, xidist);
                codebook.insert(codebook.end(), part.begin(), part.end());
            } else {
                coeffs = conv_to<vector<int>>::from(tmp);
                X.zeros();
                for (int s = 0; s < k; s++) {
                    X = X + coeffs[s]*bases[s];
                }
                // cout << vec2str(coeffs, k) << endl;
                codebook.push_back(make_pair(coeffs, X));
            }
        }
    }
    return codebook;
}

/* Creates a codebook (set of X matrices) from basis matrices B_i and symbolset q-PAM */
vector<pair<vector<int>,cx_mat>> create_codebook(const vector<cx_mat> &bases, const mat &R, const vector<int> &symbolset){
    int m = params["no_of_transmit_antennas"];
    int t = params["time_slots"];
    int k = params["no_of_matrices"];
    int cs = dparams["codebook_size"];
    int samples = params["energy_estimation_samples"];
    double P = dparams["spherical_shaping_max_power"];

    vector<pair<vector<int>,cx_mat>> codebook;
    pair<vector<int>,cx_mat> code;
    vector<int> tmp;
    cx_mat X(m, t);
    X.zeros();

    if(samples > 0){
        for (int j = 0; j < samples; j++){
            if (P > 0)
                code = create_random_spherical_codeword(bases, R, symbolset, P);
            else
                code = create_random_codeword(bases, symbolset);
            codebook.push_back(code);
        }
    } else {
        if (P > 0)
            return create_spherical_codebook(bases, R, symbolset, P, vec(k, fill::zeros), k, 0);

        if (cs > 1e6)
            log_msg("create_codebook: Generating a codebook of size " + to_string(cs) + ", this can be really slow!", "Warning");

        /* all possible combinations of code words */
        auto c = comb_wrapper(symbolset, k);

        // log_msg("All possible data vector combinations:");
        for (const auto &symbols : c){
            X.zeros();
            for (int i = 0; i < k; i++)
                X = X + symbols[i]*bases[i];
            // log_msg(vec2str(symbols, symbols.size()));
            // if (frob_norm_squared(X) > P + 1e-6 && P > 0) continue;
            codebook.push_back(make_pair(symbols, X));
        }
    }
    return codebook;
}

/* Computes the average and maximum energy of given codebook X */
pair<double,double> code_energy(const vector<pair<vector<int>,cx_mat>> &X){
    double sum = 0, max = 0, tmp = 0, average = 0;
    int cs = (int) X.size();

    for (int i = 0; i < cs; i++){
        tmp = frob_norm_squared(X[i].second);
        sum += tmp;
        if (tmp > max)
            max = tmp;
    }
    average = sum / cs;
    return make_pair(average, max);
}

/* Returns the shortest lattice basis vector from the lattice generator matrix G*/
cx_vec shortest_basis_vector(const cx_mat &G){
    cx_vec shortest(G.n_rows, fill::zeros);
    double len = 10e9;
    for (auto i = 0u; i < G.n_cols; i++){
        if (len > norm(G.col(i), 2)) {
            shortest = G.col(i);
            len = norm(shortest, 2);
        }
    }
    return shortest;
}

/* Check if x belongs to the right coset (when using coset encoding)
 * i.e. check if the difference vector sent_x - decoded_x belongs in
 * the given sublattice of the codebook carved out of lattice G_b
 */
bool coset_check(const cx_mat &Gb, const cx_mat &invGe, const Col<int> diff){
    cx_vec x = Gb*diff;
    cx_vec lambda = invGe*x;

    vec lambda_real = to_real_vector(lambda);

    /* check if lambda_real has only integer entries */
    for (const auto &l : lambda_real)
        if (fabs(l - round(l)) > 10e-5) /* tolerance */
            return false;

    return true;
}

/* calculates different rates related to the coset encoded block code */
tuple<double, double, double> code_rates(const cx_mat &Gb, const cx_mat &Ge){
    mat RGb = to_real_matrix(Gb);
    mat RGe = to_real_matrix(Ge);
    // cout << "Vol(Ge): " << sqrt(det(RGe.t()*RGe)) << endl;
    // cout << "Vol(Gb): " << sqrt(det(RGb.t()*RGb)) << endl;
    double r = log2(dparams["codebook_size"]);                          // overall rate
    double rt = log2(sqrt(det(RGe.t()*RGe))/sqrt(det(RGb.t()*RGb)));   // transmission rate
    double rc = r - rt;                                                // confusion rate
    return make_tuple(r, rt, rc);
}

/* algorithm for counting the lattice points inside a _dim_ dimensional hypersphere of radius _radius_
 * considers only a single radius, but probably faster than the function below thanks to parallelisation
 */
int count_points(const mat &R, const vector<int> &S, double radius, vec xt, int dim, double dist){

    int k = params["no_of_matrices"];
    int i = dim - 1;

    vector<int> counters(omp_get_max_threads());

    #pragma omp parallel for
    for (auto j = 0u; j < S.size(); j++) {
        vec tmp = xt;
        tmp[i] = S[j];
        double xidist = pow(dot(R.row(i).subvec(i, k-1), tmp.subvec(i, k-1)), 2) + dist;
        if (xidist <= radius) {
            if (i > 0) {
                counters[omp_get_thread_num()] += count_points(R, S, radius, tmp, dim-1, xidist);
            } else {
                counters[omp_get_thread_num()]++;
            }
        }
    }
    return accumulate(counters.begin(), counters.end(), 0);
}

/* algorithm for counting the lattice points inside a _dim_ dimensional hypersphere of radiuses r_1, r_2, ..., r_n */
vector<int> count_points_many_radiuses(const mat &R, const vector<int> &S, vector<double> radiuses, vec xt, int dim, double dist){

    int k = params["no_of_matrices"];
    int i = dim - 1;

    vector<int> counters(radiuses.size());

    for (auto j = 0u; j < S.size(); j++){
        vec tmp = xt;
        tmp[i] = S[j];
        double xidist = pow(dot(R.row(i).subvec(i, k-1), tmp.subvec(i, k-1)), 2) + dist;

        if (xidist <= radiuses[0]){
            if (i > 0){
                vector<int> counts = count_points_many_radiuses(R, S, radiuses, tmp, dim-1, xidist);
                for (auto s = 0u; s < radiuses.size(); s++)
                    counters[s] += counts[s];
            } else {
                for (auto k = 0u; k < radiuses.size(); k++){
                    if (xidist <= radiuses[k])
                        counters[k]++;
                }
            }
        }
    }
    return counters;
}

/*
 * Should return the LLL reduced (as close to orthogonal as possible) basis for lattice generated by G
 */
cx_mat LLL_reduction(const cx_mat &G) {
    #ifdef USE_LLL

        mat G_real = to_real_matrix(G);

        // ZZ_mat<double> fpG(G_real.n_rows, G_real.n_cols);
        // ZZ_mat<double> fpG(G_real.n_cols, G_real.n_rows);
        FP_mat<double> fpG(G_real.n_cols, G_real.n_rows);
        for (auto i = 0u; i < G_real.n_rows; i++) {
            for (auto j = 0u; j < G_real.n_cols; j++) {
                // fpG(i, j) = G_real(i, j);
                fpG(j, i) = G_real(i, j);
            }
        }
        cout << fpG << endl << endl;
        int status = lll_reduction(fpG, LLL_DEF_DELTA, LLL_DEF_ETA, LM_PROVED, FT_DEFAULT, 0, LLL_DEFAULT);
        if (status != RED_SUCCESS) {
            log_msg("LLL reduction failed!", "Error"); //with error '" + to_string(get_red_status_str(status)) + "'", "Error");
            exit(1);
        }
        cout << fpG << endl << endl;
        for (auto i = 0u; i < G_real.n_rows; i++) {
            for (auto j = 0u; j < G_real.n_cols; j++) {
                // G_real(i, j) = fpG(i, j).get_d();
                G_real(i, j) = fpG(j, i).get_d();
            }
        }
        cout << G_real << endl;
        return to_complex_matrix(G_real);

    #else /* Do nothing */

        return G;

    #endif
}


#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <vector>
#include <list>

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/lapack/geqrf.hpp>
#include <boost/numeric/bindings/lapack/orgqr.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/vector_of_vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

#ifdef OPENMP
#include <omp.h>
#endif

void Perform_Alpert_transform(boost::numeric::ublas::vector<double> f, std::vector<int> ki, int J,int dir, boost::numeric::ublas::vector<double>& w, std::list<std::list<boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> > > Uj, std::list<std::vector<int> > part, std::vector<int> G);
void Compute_Uj(std::vector<std::vector<double> >p, std::vector<int> ki, int &J, std::list<std::list<boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> > > &Uj, std::list<std::vector<int> > &part, std::vector<int> &G);
void Optimal_Alpert_transform(boost::numeric::ublas::vector<double> f, std::vector<std::vector<double> >p, std::vector<int> ki, int J, boost::numeric::ublas::vector<double>& w, std::list<std::list<boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> > > Uj, std::list<std::vector<int> > part, std::vector<int> G, double NRMSE, double &R, double mx, double mn, double mxg, double mng, double alpha);


//
// Created by jachu on 27.02.18.
//

#include <iostream>

#include <Eigen/Dense>

#include <g2o/types/slam3d/se3quat.h>

#include "PlaneEstimator.hpp"

using namespace std;

PlaneEstimator::PlaneEstimator() {

}

PlaneEstimator::PlaneEstimator(const Eigen::MatrixXd &pts) {
    init(pts);
}

PlaneEstimator::PlaneEstimator(const Eigen::Vector3d &icentroid,
                               const Eigen::Matrix3d &icovar,
                               int inpts)
{
    init(icentroid,
         icovar,
         inpts);
}

void PlaneEstimator::init(const Eigen::MatrixXd &pts) {
    Eigen::Vector3d icentroid;
    Eigen::Matrix3d icovar;
    compCentroidAndCovar(pts, icentroid, icovar);
    init(icentroid, icovar, pts.cols());
}

void
PlaneEstimator::init(const Eigen::Vector3d &icentroid, const Eigen::Matrix3d &icovar, int inpts) {
    centroid = icentroid;
    covar = icovar;
    npts = inpts;
    compPlaneParams(centroid,
                    covar,
                    evecs,
                    evals,
                    planeEq);
}

void
PlaneEstimator::update(const Eigen::Vector3d &ucentroid, const Eigen::Matrix3d &ucovar, int unpts) {
//    cout << "centroid = " << centroid.transpose() << endl;
//    cout << "covar = " << covar << endl;
//    cout << "npts = " << npts << endl;
//    cout << "ucentroid = " << ucentroid.transpose() << endl;
//    cout << "ucovar = " << ucovar << endl;
//    cout << "unpts = " << unpts << endl;

    updateCentroidAndCovar(centroid,
                           covar,
                           npts,
                           ucentroid,
                           ucovar,
                           unpts,
                           centroid,
                           covar,
                           npts);

//    cout << "centroid = " << centroid.transpose() << endl;
//    cout << "covar = " << covar << endl;
//    cout << "npts = " << npts << endl;

    static constexpr int ptsLimit = 500000;
    if(npts > ptsLimit){
        double scale = (double)npts/ptsLimit;
        covar /= scale;
        npts = ptsLimit;
    }
//    cout << "centroid = " << centroid.transpose() << endl;
//    cout << "covar = " << covar << endl;
//    cout << "npts = " << npts << endl;

    compPlaneParams(centroid,
                    covar,
                    evecs,
                    evals,
                    planeEq);
}

double PlaneEstimator::distance(const PlaneEstimator &other) const {
    const Eigen::Vector3d &centroid1 = centroid;
    const Eigen::Vector3d &centroid2 = other.centroid;
    Eigen::Matrix3d covar1 = covar / npts;
    Eigen::Matrix3d covar2 = other.covar / other.npts;
    int npts1 = npts;
    int npts2 = other.npts;

//    cout << "centroid1 = " << centroid1.transpose() << endl;
//    cout << "covar1 = " << covar1 << endl;
//    cout << "centroid2 = " << centroid2.transpose() << endl;
//    cout << "covar2 = " << covar2 << endl;
//
//    Eigen::Vector3d centrDiff = centroid1 - centroid2;
//    Eigen::Matrix3d infComb = (covar1 + covar2).inverse();
//    Eigen::Matrix3d covarProd = covar1 * infComb * covar2;
//    Eigen::Vector3d meanProd = covar2 * infComb * centroid1 +
//                               covar1 * infComb * centroid2;
//    double detVal = (2*M_PI*(covar1 + covar2)).determinant();
//    double expVal = -0.5 * centrDiff.transpose() * infComb * centrDiff;
//    double normFactor = 1.0 / sqrt(detVal) * exp(expVal);
//    cout << "detVal = " << detVal << endl;
//    cout << "expVal = " << -2.0*expVal << endl;
//    cout << "normFactor = " << normFactor << endl;
//    cout << "dist = " << 1.0/normFactor << endl;

    Eigen::Vector3d centrDiff = centroid1 - centroid2;
    // covariance of the second plane relative to the centroid of the first plane
    Eigen::Matrix3d relCovar = covar2 + (centrDiff * centrDiff.transpose());

    const Eigen::Vector3d &normal = evecs.col(2);
    double varNorm = normal.transpose() * relCovar * normal;
//    cout << "covar2 = " << covar2 << endl;
//    cout << "relCovar = " << relCovar << endl;
//    cout << "normal = " << normal.transpose() << endl;
//    cout << "varNorm = " << varNorm << endl;
    return varNorm;
}

void PlaneEstimator::compCentroidAndCovar(const Eigen::MatrixXd &pts,
                                         Eigen::Vector3d &centroid,
                                         Eigen::Matrix3d &covar)
{
    Eigen::Vector4d mean = Eigen::Vector4d::Zero();
    for(int i = 0; i < pts.cols(); ++i){
        mean += pts.col(i);
    }
    mean /= pts.cols();

    centroid = mean.head<3>();

    Eigen::MatrixXd demeanPts = pts;
    for(int i = 0; i < demeanPts.cols(); ++i){
        demeanPts.col(i) -= mean;
    }

    covar = demeanPts.topRows<3>() * demeanPts.topRows<3>().transpose();
}

void PlaneEstimator::compPlaneParams(const Eigen::Vector3d centroid,
                                     const Eigen::Matrix3d &covar,
                                     Eigen::Matrix3d &evecs,
                                     Eigen::Vector3d &evals,
                                     Eigen::Vector4d &planeEq)
{
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> evd(covar);

    for(int i = 0; i < 3; ++i){
        evecs.col(i) = evd.eigenvectors().col(2 - i);
        evals(i) = evd.eigenvalues()(2 - i);
    }

    planeEq.head<3>() = evecs.col(2).cast<double>();
    // distance is the dot product of normal and point lying on the plane
    planeEq(3) = -planeEq.head<3>().dot(centroid);
}

void PlaneEstimator::transform(const Vector7d &transform) {
    g2o::SE3Quat transformSE3Quat(transform);
    Eigen::Matrix4d transformMat = transformSE3Quat.to_homogeneous_matrix();
    Eigen::Matrix3d R = transformMat.block<3, 3>(0, 0);
    Eigen::Vector3d t = transformMat.block<3, 1>(0, 3);
    Eigen::Matrix4d Tinvt = transformMat.inverse();
    Tinvt.transposeInPlace();

    // Eigen::Vector3d centroid;
    centroid = R * centroid + t;

    // Eigen::Matrix3d covar;
    covar = R * covar * R.transpose();

    // Eigen::Matrix3d evecs;
    evecs = R * evecs;

    // Eigen::Vector3d evals;
    // no need to transform

    // Eigen::Vector4d planeEq;
    planeEq = Tinvt * planeEq;

    // int npts;
    // no need to transform
}

void PlaneEstimator::updateCentroidAndCovar(const Eigen::Vector3d &centroid1,
                                            const Eigen::Matrix3d &covar1,
                                            const int &npts1,
                                            const Eigen::Vector3d &centroid2,
                                            const Eigen::Matrix3d &covar2,
                                            const int &npts2,
                                            Eigen::Vector3d &ocentroid,
                                            Eigen::Matrix3d &ocovar,
                                            int &onpts)
{
    ocentroid = (npts1 * centroid1 + npts2 * centroid2)/(npts1 + npts2);
    Eigen::Vector3d centrDiff = centroid1 - centroid2;
    double fact = ((double)npts1 * npts2)/(npts1 + npts2);
//    cout << "(npts1 * npts2)/(npts1 + npts2) = " << fact << endl;
//    cout << "(centrDiff * centrDiff.transpose()) = " << (centrDiff * centrDiff.transpose()) << endl;
    ocovar = covar1 + covar2 + fact*(centrDiff * centrDiff.transpose());
    onpts = npts1 + npts2;
}


#include <algorithm>
#include <csv_reader/csv_reader.h>
#include <iostream>
#include <string>

#include <dlib/matrix.h>

namespace my_dlib {
template <typename T, size_t num_rows> struct MatrixData {
  MatrixData() = default;
  MatrixData(const std::string &csv_line) {
    std::istringstream iss(csv_line);
    for (size_t idx_element = 0; idx_element < num_rows; idx_element++) {
      data(idx_element, 0) = parse_token<T>(iss);
    }
  }
  dlib::matrix<T, num_rows, 1> data;
};

} // namespace my_dlib

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

  /*
  auto sensor_data = load_csv_file<my_dlib::MatrixData<double, 13>>(argv[1]);
  std::cout << sensor_data[0].data << std::endl;
  std::cout << sensor_data[1].data << std::endl;
  */

  std::ifstream ifs(argv[1]);
  dlib::matrix<double> my_matrix;
  dlib::set_all_elements(my_matrix, 0);
  std::cout << "test matrix: " << my_matrix << std::endl;
  ifs >> my_matrix;
  std::cout << "test matrix: " << my_matrix << std::endl;

  return 0;
}


// 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: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_REDUCTION_FUNCTORS_INCLUDE
#define MTL_REDUCTION_FUNCTORS_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/operation/squared_abs.hpp>

namespace mtl { namespace vector {

struct one_norm_functor
{
    template <typename Value>
    static inline void init(Value& value)
    {
	using math::zero;
	value= zero(value);
    }

    template <typename Value, typename Element>
    static inline void update(Value& value, const Element& x)
    {
	using std::abs;
	value+= abs(x);
    }

    template <typename Value>
    static inline void finish(Value& value, const Value& value2)
    {
	value+= value2;
    }

    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	return value;
    }
};

// sub-optimal if abs is not needed
struct two_norm_functor
{
    template <typename Value>
    static inline void init(Value& value)
    {
	using math::zero;
	value= zero(value);
    }

    template <typename Value, typename Element>
    static inline void update(Value& value, const Element& x)
    {
	using mtl::squared_abs;
	value+= squared_abs(x);
    }

    template <typename Value>
    static inline void finish(Value& value, const Value& value2)
    {
	value+= value2;
    }

    // After reduction compute square root
    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	using std::sqrt;
	return sqrt(value);
    }
};

// same as two-norm without the root at the end
struct unary_dot_functor
  : two_norm_functor
{
    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	return value;
    }
};

struct infinity_norm_functor
{
    template <typename Value>
    static inline void init(Value& value)
    {
	using math::zero;
	value= zero(value);
    }

    template <typename Value, typename Element>
    static inline void update(Value& value, const Element& x)
    {
	using std::abs; using std::max;
	value= max(value, Value(abs(x)));
    }

    template <typename Value>
    static inline void finish(Value& value, const Value& value2)
    {
	using std::abs; using std::max;
	value= max(value, Value(abs(value2)));
    }

    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	return value;
    }
};

struct sum_functor
{
    template <typename Value>
    static inline void init(Value& value)
    {
	using math::zero;
	value= zero(value);
    }

    template <typename Value, typename Element>
    static inline void update(Value& value, const Element& x)
    {
	value+= x;
    }

    template <typename Value>
    static inline void finish(Value& value, const Value& value2)
    {
	value+= value2;
    }

    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	return value;
    }
};

struct product_functor
{
    template <typename Value>
    static inline void init(Value& value)
    {
	using math::one;
	value= one(value);
    }

    template <typename Value, typename Element>
    static inline void update(Value& value, const Element& x)
    {
	value*= x;
    }

    template <typename Value>
    static inline void finish(Value& value, const Value& value2)
    {
	value*= value2;
    }

    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	return value;
    }
};

struct max_functor
{
    template <typename Value>
    static inline void init(Value& value)
    {
	using math::identity;
	value= math::identity(math::max<Value>(), value); // ADL doesn't work here in g++ 4.4
    }

    template <typename Value, typename Element>
    static inline void update(Value& value, const Element& x)
    {
	value= math::max<Value>()(value, x);
    }

    template <typename Value>
    static inline void finish(Value& value, const Value& value2)
    {
	value= math::max<Value>()(value, value2);
    }

    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	return value;
    }
};

struct min_functor
{
    template <typename Value>
    static inline void init(Value& value)
    {
	using math::identity;
	value= math::identity(math::min<Value>(), value); // ADL doesn't work here in g++ 4.4
    }

    template <typename Value, typename Element>
    static inline void update(Value& value, const Element& x)
    {
	value= math::min<Value>()(value, x);
    }

    template <typename Value>
    static inline void finish(Value& value, const Value& value2)
    {
	value= math::min<Value>()(value, value2);
    }

    template <typename Value>
    static inline Value post_reduction(const Value& value)
    {
	return value;
    }
};

}} // namespace mtl::vector

#endif // MTL_REDUCTION_FUNCTORS_INCLUDE


#pragma once
#include <manipulate_topics/msg_filter_base.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <numeric>
#include <dynamic_reconfigure/server.h>
#include<manipulate_topics/MeanConfig.h>

/**
 * @brief Class that implements a mean filter for a given Topic type.
 *
 * |Ros-Parameter | Desciption|
 * |---- | -----|
 * |~sample_number | Number of samples the mean is calculated from|
 *
 * @tparam T Type of the topic to be filtered. Operators + and / must be implemented within the msg_operators namespace.
 */

template<class T>
class MessageMeanFilter: public MessageFilterBase<T>{
    public:
        MessageMeanFilter(ros::NodeHandle &nh);

    private:
        dynamic_reconfigure::Server<manipulate_topics::MeanConfig> server_;
        void dynConfigcallback(manipulate_topics::MeanConfig &config, uint32_t level); ///<Callback for the dynamic reconfigure server
        int samples_num_;
        T filter() override;

};
#include "../src/msg_mean_filter.cpp"


/**
 * @file nca_test.cpp
 * @author Ryan Curtin
 *
 * Unit tests for Neighborhood Components Analysis and related code (including
 * the softmax error function).
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <mlpack/methods/nca/nca.hpp>
#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::metric;
using namespace mlpack::nca;
using namespace ens;

//
// Tests for the SoftmaxErrorFunction
//

BOOST_AUTO_TEST_SUITE(NCATest);

/**
 * The Softmax error function should return the identity matrix as its initial
 * point.
 */
BOOST_AUTO_TEST_CASE(SoftmaxInitialPoint)
{
  // Cheap fake dataset.
  arma::mat data;
  data.randu(5, 5);
  arma::Row<size_t> labels;
  labels.zeros(5);

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  // Verify the initial point is the identity matrix.
  arma::mat initialPoint = sef.GetInitialPoint();
  for (int row = 0; row < 5; row++)
  {
    for (int col = 0; col < 5; col++)
    {
      if (row == col)
        BOOST_REQUIRE_CLOSE(initialPoint(row, col), 1.0, 1e-5);
      else
        BOOST_REQUIRE_SMALL(initialPoint(row, col), 1e-5);
    }
  }
}

/***
 * On a simple fake dataset, ensure that the initial function evaluation is
 * correct.
 */
BOOST_AUTO_TEST_CASE(SoftmaxInitialEvaluation)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat data           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  double objective = sef.Evaluate(arma::eye<arma::mat>(2, 2));

  // Result painstakingly calculated by hand by rcurtin (recorded forever in his
  // notebook).  As a result of lack of precision of the by-hand result, the
  // tolerance is fairly high.
  BOOST_REQUIRE_CLOSE(objective, -1.5115, 0.01);
}

/**
 * On a simple fake dataset, ensure that the initial gradient evaluation is
 * correct.
 */
BOOST_AUTO_TEST_CASE(SoftmaxInitialGradient)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat data           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  arma::mat gradient;
  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  sef.Gradient(coordinates, gradient);

  // Results painstakingly calculated by hand by rcurtin (recorded forever in
  // his notebook).  As a result of lack of precision of the by-hand result, the
  // tolerance is fairly high.
  BOOST_REQUIRE_CLOSE(gradient(0, 0), -0.089766, 0.05);
  BOOST_REQUIRE_SMALL(gradient(1, 0), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(0, 1), 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), 1.63823, 0.01);
}

/**
 * On optimally separated datasets, ensure that the objective function is
 * optimal (equal to the negative number of points).
 */
BOOST_AUTO_TEST_CASE(SoftmaxOptimalEvaluation)
{
  // Simple optimal dataset.
  arma::mat data           = " 500  500 -500 -500;"
                             "   1    0    1    0 ";
  arma::Row<size_t> labels = "   0    0    1    1 ";

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  double objective = sef.Evaluate(arma::eye<arma::mat>(2, 2));

  // Use a very close tolerance for optimality; we need to be sure this function
  // gives optimal results correctly.
  BOOST_REQUIRE_CLOSE(objective, -4.0, 1e-10);
}

/**
 * On optimally separated datasets, ensure that the gradient is zero.
 */
BOOST_AUTO_TEST_CASE(SoftmaxOptimalGradient)
{
  // Simple optimal dataset.
  arma::mat data           = " 500  500 -500 -500;"
                             "   1    0    1    0 ";
  arma::Row<size_t> labels = "   0    0    1    1 ";

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  arma::mat gradient;
  sef.Gradient(arma::eye<arma::mat>(2, 2), gradient);

  BOOST_REQUIRE_SMALL(gradient(0, 0), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(0, 1), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(1, 0), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(1, 1), 1e-5);
}

/**
 * Ensure the separable objective function is right.
 */
BOOST_AUTO_TEST_CASE(SoftmaxSeparableObjective)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat data           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  // Results painstakingly calculated by hand by rcurtin (recorded forever in
  // his notebook).  As a result of lack of precision of the by-hand result, the
  // tolerance is fairly high.
  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 0, 1), -0.22480, 0.01);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 1, 1), -0.30613, 0.01);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 2, 1), -0.22480, 0.01);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 3, 1), -0.22480, 0.01);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 4, 1), -0.30613, 0.01);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 5, 1), -0.22480, 0.01);
}

/**
 * Ensure the optimal separable objective function is right.
 */
BOOST_AUTO_TEST_CASE(OptimalSoftmaxSeparableObjective)
{
  // Simple optimal dataset.
  arma::mat data           = " 500  500 -500 -500;"
                             "   1    0    1    0 ";
  arma::Row<size_t> labels = "   0    0    1    1 ";

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  arma::mat coordinates = arma::eye<arma::mat>(2, 2);

  // Use a very close tolerance for optimality; we need to be sure this function
  // gives optimal results correctly.
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 0, 1), -1.0, 1e-10);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 1, 1), -1.0, 1e-10);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 2, 1), -1.0, 1e-10);
  BOOST_REQUIRE_CLOSE(sef.Evaluate(coordinates, 3, 1), -1.0, 1e-10);
}

/**
 * Ensure the separable gradient is right.
 */
BOOST_AUTO_TEST_CASE(SoftmaxSeparableGradient)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat data           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  arma::mat coordinates = arma::eye<arma::mat>(2, 2);
  arma::mat gradient(2, 2);

  sef.Gradient(coordinates, 0, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -2.0 * 0.0069708, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), -2.0 * -0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), -2.0 * -0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), -2.0 * -0.14359, 0.01);

  sef.Gradient(coordinates, 1, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -2.0 * 0.008496, 0.01);
  BOOST_REQUIRE_SMALL(gradient(0, 1), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(1, 0), 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), -2.0 * -0.12238, 0.01);

  sef.Gradient(coordinates, 2, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -2.0 * 0.0069708, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), -2.0 * 0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), -2.0 * 0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), -2.0 * -0.1435886, 0.01);

  sef.Gradient(coordinates, 3, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -2.0 * 0.0069708, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), -2.0 * 0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), -2.0 * 0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), -2.0 * -0.1435886, 0.01);

  sef.Gradient(coordinates, 4, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -2.0 * 0.008496, 0.01);
  BOOST_REQUIRE_SMALL(gradient(0, 1), 1e-5);
  BOOST_REQUIRE_SMALL(gradient(1, 0), 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), -2.0 * -0.12238, 0.01);

  sef.Gradient(coordinates, 5, gradient, 1);

  BOOST_REQUIRE_CLOSE(gradient(0, 0), -2.0 * 0.0069708, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(0, 1), -2.0 * -0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 0), -2.0 * -0.0101707, 0.01);
  BOOST_REQUIRE_CLOSE(gradient(1, 1), -2.0 * -0.1435886, 0.01);
}

//
// Tests for the NCA algorithm.
//

/**
 * On our simple dataset, ensure that the NCA algorithm fully separates the
 * points.
 */
BOOST_AUTO_TEST_CASE(NCASGDSimpleDataset)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat data           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  // Huge learning rate because this is so simple.
  NCA<SquaredEuclideanDistance> nca(data, labels);
  nca.Optimizer().StepSize() = 1.2;
  nca.Optimizer().MaxIterations() = 300000;
  nca.Optimizer().Tolerance() = 0;
  nca.Optimizer().Shuffle() = true;

  arma::mat outputMatrix;
  nca.LearnDistance(outputMatrix);

  // Ensure that the objective function is better now.
  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  double initObj = sef.Evaluate(arma::eye<arma::mat>(2, 2));
  double finalObj = sef.Evaluate(outputMatrix);
  arma::mat finalGradient;
  sef.Gradient(outputMatrix, finalGradient);

  // finalObj must be less than initObj.
  BOOST_REQUIRE_LT(finalObj, initObj);
  // Verify that final objective is optimal.
  BOOST_REQUIRE_CLOSE(finalObj, -6.0, 0.005);
  // The solution is not unique, so the best we can do is ensure the gradient
  // norm is close to 0.
  BOOST_REQUIRE_LT(arma::norm(finalGradient, 2), 1e-4);
}

BOOST_AUTO_TEST_CASE(NCALBFGSSimpleDataset)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat data           = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                             " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  // Huge learning rate because this is so simple.
  NCA<SquaredEuclideanDistance, L_BFGS> nca(data, labels);
  nca.Optimizer().NumBasis() = 5;

  arma::mat outputMatrix;
  nca.LearnDistance(outputMatrix);

  // Ensure that the objective function is better now.
  SoftmaxErrorFunction<SquaredEuclideanDistance> sef(data, labels);

  double initObj = sef.Evaluate(arma::eye<arma::mat>(2, 2));
  double finalObj = sef.Evaluate(outputMatrix);
  arma::mat finalGradient;
  sef.Gradient(outputMatrix, finalGradient);

  // finalObj must be less than initObj.
  BOOST_REQUIRE_LT(finalObj, initObj);
  // Verify that final objective is optimal.
  BOOST_REQUIRE_CLOSE(finalObj, -6.0, 1e-5);
  // The solution is not unique, so the best we can do is ensure the gradient
  // norm is close to 0.
  BOOST_REQUIRE_LT(arma::norm(finalGradient, 2), 1e-6);
}

BOOST_AUTO_TEST_SUITE_END();


// Copyright (C) 2017 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (sweeney.chris.m@gmail.com)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/rotation.h>

#include "gtest/gtest.h"

#include "theia/math/rotation.h"
#include "theia/math/util.h"

namespace theia {

void TestTwoRotations(const Eigen::Vector3d& rotation1,
                      const Eigen::Vector3d& rotation2,
                      const double tolerance_degrees) {
  // Convert to rotation matrices.
  Eigen::Matrix3d rotation1_matrix, rotation2_matrix;
  ceres::AngleAxisToRotationMatrix(
      rotation1.data(), ceres::ColumnMajorAdapter3x3(rotation1_matrix.data()));
  ceres::AngleAxisToRotationMatrix(
      rotation2.data(), ceres::ColumnMajorAdapter3x3(rotation2_matrix.data()));

  // Compose the rotations to obtain R3 = R1 * R2;
  const Eigen::Matrix3d rotation_matrix = rotation1_matrix * rotation2_matrix;

  // Get the estimated rotation.
  const Eigen::Vector3d estimated_rotation =
      MultiplyRotations(rotation1, rotation2);

  // Convert the estimated rotation to a rotation matrix.
  Eigen::Matrix3d estimated_rotation_matrix;
  ceres::AngleAxisToRotationMatrix(
      estimated_rotation.data(),
      ceres::ColumnMajorAdapter3x3(estimated_rotation_matrix.data()));

  // Determine the angle between the two rotations.
  const Eigen::AngleAxisd rotation_difference(rotation_matrix.transpose() *
                                              estimated_rotation_matrix);
  EXPECT_LT(RadToDeg(rotation_difference.angle()), tolerance_degrees);
}

TEST(MultiplyRotations, BasicTest) {
  static const double kToleranceDegrees = 1e-8;
  const Eigen::Vector3d rotation1(-0.3, -0.2, 0.1);
  const Eigen::Vector3d rotation2(0.13, 0.06, -0.4);
  TestTwoRotations(rotation1, rotation2, kToleranceDegrees);
}

TEST(MultiplyRotations, SmallFirstRotation) {
  static const double kToleranceDegrees = 1e-8;
  const Eigen::Vector3d rotation1(1e-12, 4e-10, 1e-16);
  const Eigen::Vector3d rotation2(0.13, 0.06, -0.4);
  TestTwoRotations(rotation1, rotation2, kToleranceDegrees);
}

TEST(MultiplyRotations, SmallSecondRotation) {
  static const double kToleranceDegrees = 1e-8;
  const Eigen::Vector3d rotation1(-0.3, -0.2, 0.1);
  const Eigen::Vector3d rotation2(1e-12, 4e-10, 1e-16);
  TestTwoRotations(rotation1, rotation2, kToleranceDegrees);
}

TEST(MultiplyRotations, BothSmallRotations) {
  static const double kToleranceDegrees = 1e-8;
  const Eigen::Vector3d rotation1(-4e-12, -2e-10, 1e-16);
  const Eigen::Vector3d rotation2(3e-12, 1e-10, 8e-16);
  TestTwoRotations(rotation1, rotation2, kToleranceDegrees);
}

TEST(MultiplyRotations, OppositeRotations) {
  static const double kToleranceDegrees = 1e-8;
  const Eigen::Vector3d rotation1(-0.3, -0.2, 0.1);
  const Eigen::Vector3d rotation2(0.3, 0.2, -0.1);
  TestTwoRotations(rotation1, rotation2, kToleranceDegrees);
}

TEST(MultiplyRotations, DifficultRotatations) {
  static const double kToleranceDegrees = 1e-8;
  const Eigen::Vector3d rotation1(0.00648212, -1.05947, 0.204346);
  const Eigen::Vector3d rotation2(0.0158538, -2.07541, 0.356276);
  TestTwoRotations(rotation1, rotation2, kToleranceDegrees);
}
}  // namespace theia


#include <boost/test/unit_test.hpp>
#include <shz/math/matrix.hpp>

extern bool is_aligned(void *p, intptr_t N);

BOOST_AUTO_TEST_CASE(matrix4fConstructors)
{
	shz::math::matrix4f m = shz::math::matrix4f::identity();
	shz::math::matrix4f n = shz::math::matrix4f::identity();
	shz::math::matrix4f nn(n);

	BOOST_CHECK(is_aligned(&m, 16));
	BOOST_CHECK(is_aligned(&n, 16));
	BOOST_CHECK(is_aligned(&nn, 16));

	BOOST_CHECK(true == m.is_identity());
	BOOST_CHECK(true == n.is_identity());
	BOOST_CHECK(true == nn.is_identity());

	BOOST_CHECK(1.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(0.f == m.data[3]);

	BOOST_CHECK(0.f == m.data[4]);
	BOOST_CHECK(1.f == m.data[5]);
	BOOST_CHECK(0.f == m.data[6]);
	BOOST_CHECK(0.f == m.data[7]);

	BOOST_CHECK(0.f == m.data[8]);
	BOOST_CHECK(0.f == m.data[9]);
	BOOST_CHECK(1.f == m.data[10]);
	BOOST_CHECK(0.f == m.data[11]);

	BOOST_CHECK(0.f == m.data[12]);
	BOOST_CHECK(0.f == m.data[13]);
	BOOST_CHECK(0.f == m.data[14]);
	BOOST_CHECK(1.f == m.data[15]);
}

BOOST_AUTO_TEST_CASE(matrix4fSizeConstructors)
{
	shz::math::matrix4f m = shz::math::matrix4f::identity();
	shz::math::matrix4f n = shz::math::matrix4f::identity();

	BOOST_CHECK(true == m.is_identity());
	BOOST_CHECK(true == n.is_identity());

	BOOST_CHECK(4*4 == m.size);
	BOOST_CHECK(4 == m.mindimension);
	BOOST_CHECK(4 == m.columns);
	BOOST_CHECK(4 == m.rows);
}

BOOST_AUTO_TEST_CASE(matrix4fAdds)
{
	shz::math::matrix4f m = shz::math::matrix4f::identity();
	shz::math::matrix4f n = shz::math::matrix4f::identity();

	shz::math::matrix4f j = m+n;
	BOOST_CHECK(true != j.is_identity());
	BOOST_CHECK(4*4 == j.size);
	BOOST_CHECK(2.f == j.data[0]);
	BOOST_CHECK(0.f == j.data[1]);
	BOOST_CHECK(0.f == j.data[2]);
	BOOST_CHECK(0.f == j.data[3]);

	BOOST_CHECK(0.f == j.data[4]);
	BOOST_CHECK(2.f == j.data[5]);
	BOOST_CHECK(0.f == j.data[6]);
	BOOST_CHECK(0.f == j.data[7]);

	BOOST_CHECK(0.f == j.data[8]);
	BOOST_CHECK(0.f == j.data[9]);
	BOOST_CHECK(2.f == j.data[10]);
	BOOST_CHECK(0.f == j.data[11]);

	BOOST_CHECK(0.f == j.data[12]);
	BOOST_CHECK(0.f == j.data[13]);
	BOOST_CHECK(0.f == j.data[14]);
	BOOST_CHECK(2.f == j.data[15]);

	m += n;
	BOOST_CHECK(2.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(0.f == m.data[3]);

	BOOST_CHECK(0.f == m.data[4]);
	BOOST_CHECK(2.f == m.data[5]);
	BOOST_CHECK(0.f == m.data[6]);
	BOOST_CHECK(0.f == m.data[7]);

	BOOST_CHECK(0.f == m.data[8]);
	BOOST_CHECK(0.f == m.data[9]);
	BOOST_CHECK(2.f == m.data[10]);
	BOOST_CHECK(0.f == m.data[11]);

	BOOST_CHECK(0.f == m.data[12]);
	BOOST_CHECK(0.f == m.data[13]);
	BOOST_CHECK(0.f == m.data[14]);
	BOOST_CHECK(2.f == m.data[15]);

	n += 2.f;
	BOOST_CHECK(3.f == n.data[0]);
	BOOST_CHECK(2.f == n.data[1]);
	BOOST_CHECK(2.f == n.data[2]);
	BOOST_CHECK(2.f == n.data[3]);

	BOOST_CHECK(2.f == n.data[4]);
	BOOST_CHECK(3.f == n.data[5]);
	BOOST_CHECK(2.f == n.data[6]);
	BOOST_CHECK(2.f == n.data[7]);

	BOOST_CHECK(2.f == n.data[8]);
	BOOST_CHECK(2.f == n.data[9]);
	BOOST_CHECK(3.f == n.data[10]);
	BOOST_CHECK(2.f == n.data[11]);

	BOOST_CHECK(2.f == n.data[12]);
	BOOST_CHECK(2.f == n.data[13]);
	BOOST_CHECK(2.f == n.data[14]);
	BOOST_CHECK(3.f == n.data[15]);
}

BOOST_AUTO_TEST_CASE(matrix4fSubs)
{
	shz::math::matrix4f m = shz::math::matrix4f::identity();
	shz::math::matrix4f n = shz::math::matrix4f::identity();

	shz::math::matrix4f j = m-n;
	BOOST_CHECK(true != j.is_identity());
	BOOST_CHECK(16 == j.size);
	BOOST_CHECK(0.f == j.data[0]);
	BOOST_CHECK(0.f == j.data[1]);
	BOOST_CHECK(0.f == j.data[2]);
	BOOST_CHECK(0.f == j.data[3]);

	m -= n;
	BOOST_CHECK(0.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(0.f == m.data[3]);

	n -= 2.f;
	BOOST_CHECK(-1.f == n.data[0]);
	BOOST_CHECK(-2.f == n.data[1]);
	BOOST_CHECK(-2.f == n.data[2]);
	BOOST_CHECK(-2.f == n.data[3]);

	BOOST_CHECK(-2.f == n.data[4]);
	BOOST_CHECK(-1.f == n.data[5]);
	BOOST_CHECK(-2.f == n.data[6]);
	BOOST_CHECK(-2.f == n.data[7]);

	BOOST_CHECK(-2.f == n.data[8]);
	BOOST_CHECK(-2.f == n.data[9]);
	BOOST_CHECK(-1.f == n.data[10]);
	BOOST_CHECK(-2.f == n.data[11]);

	BOOST_CHECK(-2.f == n.data[12]);
	BOOST_CHECK(-2.f == n.data[13]);
	BOOST_CHECK(-2.f == n.data[14]);
	BOOST_CHECK(-1.f == n.data[15]);
}

BOOST_AUTO_TEST_CASE(matrix4fMuls)
{
	shz::math::matrix4f m = shz::math::matrix4f::identity();
	shz::math::matrix4f n = shz::math::matrix4f::identity();

	shz::math::matrix4f j = m*n;
	BOOST_CHECK(true == j.is_identity());
	BOOST_CHECK(4*4 == j.size);

	shz::math::vector4f v = shz::math::vector4f();
	v.data[0] = 2.f;
	v.data[1] = 2.f;
	v.data[2] = 0.f;
	v.data[3] = 0.f;
	shz::math::vector4f b = m*v;
	BOOST_CHECK(2.f == b.data[0]);
	BOOST_CHECK(2.f == b.data[1]);
	BOOST_CHECK(0.f == b.data[2]);
	BOOST_CHECK(0.f == b.data[3]);

	m = n*2;
	BOOST_CHECK(true != m.is_identity());
	b = m*v;
	BOOST_CHECK(4.f == b.data[0]);
	BOOST_CHECK(4.f == b.data[1]);
	BOOST_CHECK(0.f == b.data[2]);
	BOOST_CHECK(0.f == b.data[3]);

	m *= n;
	BOOST_CHECK(true != m.is_identity());
	BOOST_CHECK(2.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(0.f == m.data[3]);

	BOOST_CHECK(0.f == m.data[4]);
	BOOST_CHECK(2.f == m.data[5]);
	BOOST_CHECK(0.f == m.data[6]);
	BOOST_CHECK(0.f == m.data[7]);

	BOOST_CHECK(0.f == m.data[8]);
	BOOST_CHECK(0.f == m.data[9]);
	BOOST_CHECK(2.f == m.data[10]);
	BOOST_CHECK(0.f == m.data[11]);

	BOOST_CHECK(0.f == m.data[12]);
	BOOST_CHECK(0.f == m.data[13]);
	BOOST_CHECK(0.f == m.data[14]);
	BOOST_CHECK(2.f == m.data[15]);

	m *= 2;
	BOOST_CHECK(true != m.is_identity());
	BOOST_CHECK(4.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(0.f == m.data[3]);

	BOOST_CHECK(0.f == m.data[4]);
	BOOST_CHECK(4.f == m.data[5]);
	BOOST_CHECK(0.f == m.data[6]);
	BOOST_CHECK(0.f == m.data[7]);

	BOOST_CHECK(0.f == m.data[8]);
	BOOST_CHECK(0.f == m.data[9]);
	BOOST_CHECK(4.f == m.data[10]);
	BOOST_CHECK(0.f == m.data[11]);

	BOOST_CHECK(0.f == m.data[12]);
	BOOST_CHECK(0.f == m.data[13]);
	BOOST_CHECK(0.f == m.data[14]);
	BOOST_CHECK(4.f == m.data[15]);

}

BOOST_AUTO_TEST_CASE(matrixRotation)
{
	//shz::math::vector3f axis(0.f, 1.f, 0.f);
	//auto rot = shz::math::matrix4f::from_rotation(axis, shz::math::HALF_PI);

}

/*
BOOST_AUTO_TEST_CASE(matrixPerformance)
{
	auto m = shz::math::matrix<shz::math::f32, 200, 200>::identity();
	auto n = shz::math::matrix<shz::math::f32, 200, 200>::identity();

	for(size_t i=0; i<10000; i++)
		auto r = m*n;
}*/


#include "convergence/moments/convergence_checker_l1_norm.hpp"

#include <deal.II/lac/vector.h>

#include "test_helpers/test_helper_functions.h"
#include "test_helpers/gmock_wrapper.h"

namespace {

using namespace bart;

class SingleMomentCheckerL1NormTest : public ::testing::Test {
 public:
  using Vector = dealii::Vector<double>;
  static constexpr double max_delta{ 1e-6 };
  static constexpr Vector::size_type moment_size{5 };
  bart::convergence::moments::ConvergenceCheckerL1Norm checker{ max_delta };
  Vector moment_one, moment_two;
  auto SetUp() -> void override;
};

auto SingleMomentCheckerL1NormTest::SetUp() -> void {
  auto set_up_moment = [=](Vector& moment) {
    moment.reinit(moment_size);
    const auto random_vector{ test_helpers::RandomVector(5, 0, 2) };
    for (Vector::size_type i = 0; i < moment_size; ++i)
      moment[i] = random_vector.at(i);
  };
  set_up_moment(moment_one);
  moment_two = moment_one;
}

// Max Delta getter should return the correct value
TEST_F(SingleMomentCheckerL1NormTest, Getters) {
  EXPECT_EQ(checker.max_delta(), this->max_delta);
}

// Max delta setter should set value correctly
TEST_F(SingleMomentCheckerL1NormTest, MaxDeltaSetter) {
  const double max_delta_to_set{ test_helpers::RandomDouble(1e-16, 1e-8) };
  EXPECT_EQ(checker.max_delta(), this->max_delta);
  checker.SetMaxDelta(max_delta_to_set);
  EXPECT_EQ(checker.max_delta(), max_delta_to_set);
}

// Setter should throw if provided a negative value
TEST_F(SingleMomentCheckerL1NormTest, SetterBadValue) {
  const double max_delta_to_set{ test_helpers::RandomDouble(-1e-8, -1e-16) };
  EXPECT_EQ(checker.max_delta(), this->max_delta);
  EXPECT_ANY_THROW({checker.SetMaxDelta(max_delta_to_set);});
}

// The same vector should return true
TEST_F(SingleMomentCheckerL1NormTest, SameVector) {
  EXPECT_TRUE(checker.IsConverged(moment_one, moment_one));
  EXPECT_TRUE(checker.is_converged());
}

// Being slightly less than one max delta away should return true
TEST_F(SingleMomentCheckerL1NormTest, OneThresholdAway) {
  double to_add = moment_one.l1_norm() * 0.99 * checker.max_delta();
  moment_two(2) += to_add;

  EXPECT_TRUE(checker.IsConverged(moment_one, moment_two));
  EXPECT_TRUE(checker.IsConverged(moment_two, moment_one));
  EXPECT_TRUE(checker.is_converged());
  EXPECT_NEAR(0.99 * checker.max_delta(), checker.delta().value(), 1e-6);
}

// Being greater than max delta away should return false
TEST_F(SingleMomentCheckerL1NormTest, TwoThresholdAway) {
  const double to_add = moment_one.l1_norm() * 2 * checker.max_delta();
  moment_two(2) += to_add;

  EXPECT_FALSE(checker.IsConverged(moment_one, moment_two));
  EXPECT_FALSE(checker.IsConverged(moment_two, moment_one));
  EXPECT_FALSE(checker.is_converged());
  EXPECT_NEAR(2 * checker.max_delta(), checker.delta().value(), 1e-6);
}

// Setting new max delta should still work for convergence
TEST_F(SingleMomentCheckerL1NormTest, SetMaxDelta) {
  const double to_set{ 1e-5 };
  checker.SetMaxDelta(to_set);
  EXPECT_EQ(checker.max_delta(), to_set);

  const double to_add = moment_one.l1_norm() * 0.99 * to_set;
  moment_two(2) += to_add;

  EXPECT_TRUE(checker.IsConverged(moment_one, moment_two));
  EXPECT_TRUE(checker.IsConverged(moment_two, moment_one));
}

} // namespace


/*
 * Copyright 2015 C. Brett Witherspoon
 */

#define BOOST_TEST_MODULE signum_tests
#include <boost/test/unit_test.hpp>

#include <cmath>
#include <cstdint>
#include <limits>
#include <type_traits>

#include "signum/utility/fixed.hpp"

namespace
{
template<typename Float, typename Fixed>
void _fixed_to_float_test()
{
  using signum::utility::fixed_to_float;

  const auto delta = 1.0 / (std::numeric_limits<Fixed>::max() + 1.0);

  const auto fixed_lower  = std::numeric_limits<Fixed>::lowest();
  const auto float_lower  = (std::is_signed<Fixed>::value) ? -1.0 : 0.0;

  const auto fixed_median = std::numeric_limits<Fixed>::max() / 2;
  const auto float_median = 0.5 - delta;

  const auto fixed_upper  = std::numeric_limits<Fixed>::max();
  const auto float_upper  = 1.0 - delta;

  const auto lower  = fixed_to_float<Float, Fixed>(fixed_lower);
  const auto median = fixed_to_float<Float, Fixed>(fixed_median);
  const auto upper  = fixed_to_float<Float, Fixed>(fixed_upper);

  BOOST_CHECK_CLOSE(lower,
                    static_cast<Float>(float_lower),
                    static_cast<Float>(100.0*delta/float_lower));
  BOOST_CHECK_CLOSE(median,
                    static_cast<Float>(float_median),
                    static_cast<Float>(100.0*delta/float_median));
  BOOST_CHECK_CLOSE(upper,
                    static_cast<Float>(float_upper),
                    static_cast<Float>(100.0*delta/float_upper));
}

template<typename Fixed, typename Float>
void _float_to_fixed_test()
{
  using signum::utility::float_to_fixed;

  const auto delta = 1.0 / (std::numeric_limits<Fixed>::max() + 1.0);
  const auto epsilon = std::numeric_limits<Float>::epsilon();

  const auto fixed_lower  = std::numeric_limits<Fixed>::lowest();
  const auto float_lower  = (std::is_signed<Fixed>::value) ? -1.0 : 0.0;

  const auto fixed_median = std::numeric_limits<Fixed>::max() / 2;
  const auto float_median = 0.5 - epsilon - delta;

  const auto fixed_upper  = std::numeric_limits<Fixed>::max();
  const auto float_upper  = 1.0 - epsilon - delta;

  const auto lower  = float_to_fixed<Fixed, Float>(float_lower);
  const auto median = float_to_fixed<Fixed, Float>(float_median);
  const auto upper  = float_to_fixed<Fixed, Float>(float_upper);

  BOOST_CHECK_EQUAL(lower, fixed_lower);
  BOOST_CHECK_EQUAL(median, fixed_median);
  BOOST_CHECK_EQUAL(upper, fixed_upper);
}
} // namespace (anonymous)

BOOST_AUTO_TEST_CASE(fixed_to_float_unsigned_single_test)
{
  _fixed_to_float_test<float, uint8_t>();
  _fixed_to_float_test<float, uint16_t>();
  _fixed_to_float_test<float, uint32_t>();
  _fixed_to_float_test<float, uint64_t>();
}

BOOST_AUTO_TEST_CASE(fixed_to_float_signed_single_test)
{
  _fixed_to_float_test<float, int8_t>();
  _fixed_to_float_test<float, int16_t>();
  _fixed_to_float_test<float, int32_t>();
  _fixed_to_float_test<float, int64_t>();
}

BOOST_AUTO_TEST_CASE(fixed_to_float_unsigned_double_test)
{
  _fixed_to_float_test<double, uint8_t>();
  _fixed_to_float_test<double, uint16_t>();
  _fixed_to_float_test<double, uint32_t>();
  _fixed_to_float_test<double, uint64_t>();
}

BOOST_AUTO_TEST_CASE(fixed_to_float_signed_double_test)
{
  _fixed_to_float_test<double, int8_t>();
  _fixed_to_float_test<double, int16_t>();
  _fixed_to_float_test<double, int32_t>();
  _fixed_to_float_test<double, int64_t>();
}

BOOST_AUTO_TEST_CASE(float_to_fixed_unsigned_single_test)
{
  _float_to_fixed_test<uint8_t, float>();
  _float_to_fixed_test<uint16_t, float>();
}

BOOST_AUTO_TEST_CASE(float_to_fixed_signed_single_test)
{
  _float_to_fixed_test<int8_t, float>();
  _float_to_fixed_test<int16_t, float>();
}

BOOST_AUTO_TEST_CASE(float_to_fixed_unsigned_double_test)
{
  _float_to_fixed_test<uint8_t, double>();
  _float_to_fixed_test<uint16_t, double>();
  _float_to_fixed_test<uint32_t, double>();
}

BOOST_AUTO_TEST_CASE(float_to_fixed_signed_double_test)
{
  _float_to_fixed_test<int8_t, double>();
  _float_to_fixed_test<int16_t, double>();
  _float_to_fixed_test<int32_t, double>();
}