#include "mode_calc_pca.hpp"

#include <mill/util/cmdarg.hpp>
#include <mill/math/Vector.hpp>
// #include <mill/math/EigenSolver.hpp>
#include <mill/traj.hpp>
#include <toml/toml.hpp>
#include <numeric>

#include <Eigen/Dense>

namespace mill
{

const char* mode_calc_pca_usage() noexcept
{
    return "usage: mill calc pca {trajectory} [--top=(3 by default)] [--top-contribution=(95% by default)] [--output=(\"{base name of trajectory}_pca.dat\" by default)]\n"
           "         determines principal component from traj file.\n"
           "         --top=N specifies the number of component will be written out.\n"
           "         --top-contribution=% specifies the number of component via the accumulated contribution rate.\n"
           "         if you specify `--top=3 --top-contribution=95` and it would be found that top 5 components are\n"
           "         needed to achieve 95% contribution, 5 components are written.\n"
           "     $ mill calc pca {input.toml}\n"
           "         determines principal component using customized input.\n"
           "         ```toml\n"
           "         input  = \"traj.dcd\"\n"
           "         output_basename = \"pca.dat\"\n"
           "         # (optional) how many components are reported.\n"
           "         # by default, 3.\n"
           "         top    = 10\n"
           "         # (optional) instead of `top`, output top N% contributing PCs.\n"
           "         top_contribution = 95 # %\n"
           "         # (optional) indices of particles to be used.\n"
           "         # by default, all the particles are used.\n"
           "         # {first, last} specifies the range. last is not included.\n"
           "         use    = [0, 2, 5, {first=10, last=20}]\n"
           "         ```";
}

int mode_calc_pca(std::deque<std::string_view> args)
{
    using namespace std::literals::string_literals;

    auto top_n_opt       = pop_argument<std::size_t>(args, "top");
    auto top_contrib_opt = pop_argument<double     >(args, "top-contribution");
    auto output_opt      = pop_argument<std::string>(args, "output");
    std::vector<std::size_t> particles_to_be_used;

    if(args.empty())
    {
        log::error("mill calc pca: too few arguments.");
        log::error(mode_calc_pca_usage());
        return 1;
    }

    for(const auto& arg : args)
    {
        if(arg.substr(0, 2) == "--")
        {
            log::error("unknown argument ", arg, " found. It will be ignored. please check it.");
        }
    }

    const std::string fname(args.front());
    if(fname == "help")
    {
        log::info(mode_calc_pca_usage());
        return 0;
    }

    std::string trajfile;
    if(extension_of(fname) == ".toml")
    {
        const auto data = toml::parse(fname);

        trajfile = toml::find<std::string>(data, "input");

        if(data.contains("top"))
        {
            top_n_opt = toml::find<std::size_t>(data, "top");
        }

        if(data.contains("top_contribution"))
        {
            top_contrib_opt = toml::find<double>(data, "top_contribution");
        }
        if(data.contains("top-contribution"))
        {
            top_contrib_opt = toml::find<double>(data, "top-contribution");
        }

        if(data.contains("output_basename"))
        {
            output_opt = toml::find<std::string>(data, "output_basename");
        }
        if(data.contains("use"))
        {
            for(auto&& elem : toml::find<toml::array>(data, "use"))
            {
                if(elem.is_integer())
                {
                    particles_to_be_used.push_back(elem.as_integer());
                }
                else
                {
                    for(std::size_t i=elem.at("first").as_integer(), e=elem.at("last").as_integer(); i<e; ++i)
                    {
                        particles_to_be_used.push_back(i);
                    }
                }
            }
        }
        else
        {
            particles_to_be_used.resize(reader(trajfile).read_frame()->size(), 0);
            std::iota(particles_to_be_used.begin(), particles_to_be_used.end(), 0);
        }
    }
    else
    {
        trajfile = std::string(fname);
        particles_to_be_used.resize(reader(trajfile).read_frame()->size(), 0);
        std::iota(particles_to_be_used.begin(), particles_to_be_used.end(), 0);
    }

    if(not output_opt.has_value())
    {
        output_opt = std::string(base_name_of(trajfile));
    }

    const std::string output_basename = output_opt.value();

    log::info("It will use ", particles_to_be_used.size(), " particles in total.");
    if(top_n_opt.has_value())
    {
        log::info("It will output ", top_n_opt.value(), " components");
    }
    if(top_contrib_opt.has_value())
    {
        log::info("It will output top ", top_contrib_opt.value(), "% contributing components.");
    }
    log::info("The results will be written in ", output_basename);

    // we will use this `traj` to write the movement corresponds to the principal components.
    Trajectory traj = reader(trajfile).read();

    log::debug("input files are loaded.");
    log::info("trajectory has ", traj.size(), " snapshots.");

    // -----------------------------------------------------------------------
    // constructing covariance matrix

    std::size_t num_frames = 0;
    std::vector<double> means(particles_to_be_used.size() * 3, 0.0);
    for(const auto frame : traj)
    {
#pragma omp parallel for
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            means[i*3+0] += frame.at(particles_to_be_used[i]).position()[0];
            means[i*3+1] += frame.at(particles_to_be_used[i]).position()[1];
            means[i*3+2] += frame.at(particles_to_be_used[i]).position()[2];
        }
        num_frames += 1;
    }

    const double normalize = 1.0 / num_frames;
#pragma omp parallel for
    for(std::size_t i=0; i<means.size(); ++i)
    {
        auto& mean = means[i];
        mean *= normalize;
    }

    log::info("mean positions are calculated");

    // {x1, y1, z1, x2, y2, z2, ...}
    Eigen::MatrixXd mat = Eigen::MatrixXd::Zero(particles_to_be_used.size() * 3,
                                                particles_to_be_used.size() * 3);

    for(const auto frame : traj)
    {
        // the matrix often will be large enough for this
#pragma omp parallel for
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            const auto pix = frame.at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            const auto piy = frame.at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            const auto piz = frame.at(particles_to_be_used[i]).position()[2] - means[i*3+2];

            for(std::size_t j=i; j<particles_to_be_used.size(); ++j)
            {
                const auto pjx = frame.at(particles_to_be_used[j]).position()[0] - means[j*3+0];
                const auto pjy = frame.at(particles_to_be_used[j]).position()[1] - means[j*3+1];
                const auto pjz = frame.at(particles_to_be_used[j]).position()[2] - means[j*3+2];

                mat(i*3+0, j*3+0) += pix * pjx;
                mat(i*3+0, j*3+1) += pix * pjy;
                mat(i*3+0, j*3+2) += pix * pjz;

                mat(i*3+1, j*3+0) += piy * pjx;
                mat(i*3+1, j*3+1) += piy * pjy;
                mat(i*3+1, j*3+2) += piy * pjz;

                mat(i*3+2, j*3+0) += piz * pjx;
                mat(i*3+2, j*3+1) += piz * pjy;
                mat(i*3+2, j*3+2) += piz * pjz;
            }
        }
    }

#pragma omp parallel for
    for(std::size_t i=0; i<particles_to_be_used.size() * 3; ++i)
    {
        mat(i, i) *= normalize;
        for(std::size_t j=i+1; j<particles_to_be_used.size() * 3; ++j)
        {
            mat(i, j) *= normalize;
            mat(j, i) = mat(i, j);
        }
    }

    log::info("co-variance matrix is constructed");

    // -----------------------------------------------------------------------
    // calculating eigenvalues of covariance matrix

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(mat);
    std::vector<std::pair<double, Eigen::VectorXd>> eigens(particles_to_be_used.size() * 3);

#pragma omp parallel for
    for(std::size_t i=0; i<particles_to_be_used.size() * 3; ++i)
    {
        eigens[i].first  = solver.eigenvalues()[i];
        eigens[i].second = solver.eigenvectors().col(i);
    }

    std::sort(eigens.begin(), eigens.end(), [](const auto& lhs, const auto& rhs) {
            return lhs.first > rhs.first;
        });

    log::info("eigenvalues are calculated");

    // -----------------------------------------------------------------------
    // determining contribution rate

    const auto sum_eigenvalues = std::accumulate(eigens.begin(), eigens.end(),
        0.0, [](const auto& acc, const auto& elem) {
            return acc + elem.first;
        }) * 0.01; // scale to the per-cent
    std::vector<double> contribution_rate(eigens.size());
    std::transform(eigens.begin(), eigens.end(), contribution_rate.begin(),
            [sum_eigenvalues](const auto& eigenpair) noexcept -> double {
                return eigenpair.first / sum_eigenvalues;
            });

    std::vector<double> accumulated_contribution_rate(eigens.size());
    const double contribution_threshold = top_contrib_opt.value_or(95.0);
    std::size_t top_contributions = 0;
    double accum = 0.0;
    std::transform(contribution_rate.begin(), contribution_rate.end(),
            accumulated_contribution_rate.begin(),
            [&accum, &top_contributions, contribution_threshold](const auto& rate) {
                if(accum <= contribution_threshold)
                {
                    top_contributions += 1;
                }
                accum += rate;
                return accum;
            });

    std::size_t num_components = std::max<std::size_t>(3, top_contributions);
    if(top_n_opt.has_value())
    {
        num_components = top_n_opt.value();
    }
    if(top_contrib_opt.has_value())
    {
        num_components = top_contributions;
    }
    if(top_n_opt.has_value() && top_contrib_opt.has_value())
    {
        num_components = std::max(top_contributions, top_n_opt.value());
    }

    if(num_components < eigens.size())
    {
        eigens.resize(num_components);
    }

    log::info("top ", num_components, " components (",
            accumulated_contribution_rate.at(num_components), "% contribution) will be written");

    // ------------------------------------------------------------------------
    // determine direction (sign) of eigenvector to make the trajectory (-)->(+)

    for(auto& [eigval, eigvec] : eigens)
    {
        Eigen::VectorXd first_frame = Eigen::VectorXd::Zero(particles_to_be_used.size() * 3);
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            first_frame[i*3+0] = traj.front().at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            first_frame[i*3+1] = traj.front().at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            first_frame[i*3+2] = traj.front().at(particles_to_be_used[i]).position()[2] - means[i*3+2];
        }
        Eigen::VectorXd  last_frame = Eigen::VectorXd::Zero(particles_to_be_used.size() * 3);
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            last_frame[i*3+0] = traj.back().at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            last_frame[i*3+1] = traj.back().at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            last_frame[i*3+2] = traj.back().at(particles_to_be_used[i]).position()[2] - means[i*3+2];
        }

        const auto first = eigvec.dot(first_frame);
        const auto  last = eigvec.dot( last_frame);
        if(last < first)
        {
            eigvec *= -1.0;
        }
    }

    // -----------------------------------------------------------------------
    // writing trajectory along the eigenvector

    std::vector<std::pair<double, double>> component_range(eigens.size(),
            std::make_pair(std::numeric_limits<double>::max(),
                          -std::numeric_limits<double>::max()));

    std::ofstream ofs(output_basename + "_PCA.dat"s);
    if(not ofs.good())
    {
        log::fatal("file open error: ", output_basename + "_PCA.dat"s);
    }

    ofs << '#';
    for(std::size_t i=0; i<eigens.size(); ++i)
    {
        ofs << " PC" << i;
    }
    ofs << '\n';

    ofs << '#';
    for(std::size_t i=0; i<eigens.size(); ++i)
    {
        ofs << " " << std::fixed << std::setprecision(5) << contribution_rate.at(i) << "%";
    }
    ofs << '\n';

    for(const auto frame : traj)
    {
        Eigen::VectorXd snapshot = Eigen::VectorXd::Zero(particles_to_be_used.size() * 3);
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            snapshot[i*3+0] = frame.at(particles_to_be_used[i]).position()[0] - means[i*3+0];
            snapshot[i*3+1] = frame.at(particles_to_be_used[i]).position()[1] - means[i*3+1];
            snapshot[i*3+2] = frame.at(particles_to_be_used[i]).position()[2] - means[i*3+2];
        }

        std::size_t idx=0;
        for(const auto& eigen : eigens)
        {
            const auto component = snapshot.dot(eigen.second);
            ofs << component << ' ';

            component_range.at(idx).first  = std::min(component, component_range.at(idx).first);
            component_range.at(idx).second = std::max(component, component_range.at(idx).second);

            ++idx;
        }
        ofs << '\n';
    }

    log::info("trajectory along PCs are written in ", output_basename + "_PCA.dat");

    // -----------------------------------------------------------------------
    // output principal motion

    // freeze the trajectory at the mean position to see the movement of selected particles
    auto& init = traj.at(0);
    for(std::size_t i=1; i<traj.size(); ++i)
    {
        const auto frame = traj.at(i);
        for(std::size_t p_idx=0; p_idx<frame.size(); ++p_idx)
        {
            init.at(p_idx).position() += frame.at(p_idx).position();
        }
    }
    for(std::size_t p_idx=0; p_idx<init.size(); ++p_idx)
    {
        init.at(p_idx).position() *= normalize;
    }

    const std::size_t PC_movement_len = std::min<std::size_t>(1000, traj.size());
    traj.snapshots().resize(PC_movement_len);

    for(std::size_t i=1; i<traj.size(); ++i)
    {
        traj.at(i) = init;
    }

    // construct movement along PCs

    std::size_t idx=0;
    for(const auto& eigen : eigens)
    {
        const auto [lower, upper] = component_range.at(idx);

        const auto dx = (upper - lower) / PC_movement_len;
        for(std::size_t t=0; t<PC_movement_len; ++t)
        {
            auto& frame = traj.at(t);

            const auto x = lower + dx * t;
            for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
            {
                frame.at(particles_to_be_used[i]).position()[0] = means.at(i*3+0) + eigen.second[i*3+0] * x;
                frame.at(particles_to_be_used[i]).position()[1] = means.at(i*3+1) + eigen.second[i*3+1] * x;
                frame.at(particles_to_be_used[i]).position()[2] = means.at(i*3+2) + eigen.second[i*3+2] * x;
            }
        }
        const auto outtrajname = output_basename + "_PC"s +
            std::to_string(idx+1) + std::string(extension_of(trajfile));
        auto w = writer(outtrajname);
        w.write(traj);

        log::info("structure change along PC", idx+1, " (", contribution_rate.at(idx) , "% contribution) is written in ", outtrajname);

        ++idx;
    }

    // -----------------------------------------------------------------------
    // output principal vectors as a trajectory

    // normally, number of Eigen vectors to be written is smaller than 1000...
    traj.snapshots().resize(eigens.size());
    for(std::size_t i=0; i<eigens.size(); ++i)
    {
        const auto& [val, vec] = eigens.at(i);

        auto& frame = traj.at(i);
        for(auto& p : frame)
        {
            p.position() = Vector<double, 3>(0, 0, 0); // clear frame
        }
        // write eigen vector
        for(std::size_t i=0; i<particles_to_be_used.size(); ++i)
        {
            frame.at(particles_to_be_used[i]).position()[0] = vec[i*3+0];
            frame.at(particles_to_be_used[i]).position()[1] = vec[i*3+1];
            frame.at(particles_to_be_used[i]).position()[2] = vec[i*3+2];
        }
    }
    const auto eigen_vec_name = output_basename + "_EigenVectors" +
                                std::string(extension_of(trajfile));
    auto w = writer(eigen_vec_name);
    w.write(traj);

    return 0;
}

} // mill


/// @file types.hpp Simple typedefs used by Biggles

#ifndef BIGGLES_TYPES_HPP__
#define BIGGLES_TYPES_HPP__

#include <Eigen/Dense>

namespace biggles {

typedef Eigen::Matrix2f matrix2f;
typedef Eigen::Matrix4f matrix4f;
typedef Eigen::Matrix<float, 2, 4> matrix2x4f;
typedef Eigen::Matrix<float, 4, 2> matrix4x2f;

typedef Eigen::Vector4f state_t; // the underlying state vector type: [x, x', y, y']'

namespace model {

/// @brief A tuple representing the model parameters.
///
/// The model parameters are represented by the tuple \f$ ( \lambda_b, \lambda_f, p_s, p_d, R ) \f$ where
///
/// - \f$ \lambda_b \f$: the mean number of new tracks appearing per frame.
/// - \f$ \lambda_f \f$: the mean number of false observations per frame.
/// - \f$ p_s \f$: the probability that a target will survive from frame \f$ t \f$ to \f$ t+1 \f$.
/// - \f$ p_d \f$: the probability that a target will generate an observation.
/// - \f$ r \f$: the constraint radius.
/*
typedef boost::tuples::tuple<float, float, float, float, matrix2f, float, matrix4f> parameters;
*/

struct parameters {
    float birth_rate;
    float clutter_rate;
    float survival_probability;
    float observation_probability;
    matrix2f observation_error_covariance;
    float constraint_radius;
    matrix4f process_noise_covariance;
    bool operator==(const parameters &other) const {
        return
        other.birth_rate == birth_rate and
        other.clutter_rate == clutter_rate and
        other.survival_probability == survival_probability and
        other.observation_probability == observation_probability and
        other.constraint_radius == constraint_radius and
        other.observation_error_covariance == observation_error_covariance and
        other.process_noise_covariance == process_noise_covariance;
    }
};

}

}

#endif // BIGGLES_MODEL_HPP__


#pragma once

#include <boost/optional.hpp>
#include <math/Plane.hpp>
#include <math/Ray.hpp>
#include <math/Vec.hpp>

template <int n, class T>
inline boost::optional<Vec<n, T> > intersection(Ray<n, T> const & ray, Plane<n, T> const & plane) {
	T denom = dot(ray.get_dir(), plane.get_normal());
	if (denom == 0) {
        if (plane.contains(ray.get_start())) { return ray.get_start(); }
        else { return boost::none; }
    }

	T t = (plane.get_dist() - dot(ray.get_start(), plane.get_normal())) / denom;
    if (t < 0) { return boost::none; }
	return ray(t);
}

template <int n, class T>
inline boost::optional<Vec<n, T> > intersection(Plane<n, T> const & plane, Ray<n, T> const & ray) {
	return intersection(ray, plane);
}


#include <stdio.h>
#include "EM.h"
#include <cstdlib>
#include <string>
#include <time.h>
#include <boost/random.hpp>

using namespace ai::em;
using namespace std;
int main(int argc, char** argv)
{
    Parameters param[6];
    // peff[j][i]: probability of effect #i = 0 given gender = j
	// j = 0 for male, j=1 for female
	// i = 0 for height, i = 1 for weight
	// so peff[0][0] is the probability of height > 55  given male gender
	//    peff[0][1] is the probability of weight > 130 given male gender
	//    peff[1][0] is the probability of height > 55  given female gender
	//    peff[1][1] is the probability of weight > 130 given female gender
    param[0].pg0 = 0.7;
    param[0].peff[0][0] = 0.7; // height 0 (>55) given male
    param[0].peff[0][1] = 0.8; // weight 0 (>130) given male
    param[0].peff[1][0] = 0.3; // height 0 (>55) given female
    param[0].peff[1][1] = 0.4; // weight 0 (>130) given female

    if(argc == 7)
    {
        param[0].pg0 = atof(argv[2]);
        param[0].peff[0][0] = atof(argv[3]); // height 0 (>55) given male
        param[0].peff[0][1] = atof(argv[4]); // weight 0 (>130) given male
        param[0].peff[1][0] = atof(argv[5]); // height 0 (>55) given female
        param[0].peff[1][1] = atof(argv[6]); // weight 0 (>130) given female
    }
//    //generating five random parameters set
    time_t timer;
    time(&timer);  /* get current time; same as: timer = time(NULL)  */
    boost::mt19937 seed( (int)timer );
    boost::uniform_real<> dist(0.0,1.0);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > random(seed,dist);
    for (int i=1; i <6 ; ++i)
    {
        param[i].pg0 = random();
        param[i].peff[0][0] = random(); // height 0 (>55) given male
        param[i].peff[0][1] = random(); // weight 0 (>130) given male
        param[i].peff[1][0] = random();// height 0 (>55) given female
        param[i].peff[1][1] = random();// weight 0 (>130) given female
    }
    EM em;
    em.read_input(argv[1]);
    // five more random starting parameters
//    cout<<"random number 1 = "<<random()<<endl;
//    cout<<"random number 2 = "<<random()<<endl;
//    cout<<"random number 3= "<<random()<<endl;
    for (int i=0; i <6 ; ++i)
    {

        std::cout <<"Starting parameter set "<<i<<": " << endl;
        em.setParameters(param[i]);
        //em.read_input(argv[1]);
        Parameters final_param = em.optimize();
        printf ("%40s\t %20s\t %20s\n", "Probability", "Starting parameter", "final parameters");
        printf ("%40s\t %20.6f\t %20.6f\n", "P(gender=M)" ,  param[i].pg0, final_param.pg0);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(weight=greater_than_130|gender=M)", param[i].peff[0][0], final_param.peff[0][0]);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(weight=greater_than_130|gender=F)", param[i].peff[0][1], final_param.peff[0][1]);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(height= greater_than_55|gender=M)", param[i].peff[1][0], final_param.peff[1][0]);
        printf ("%40s\t %20.6f\t %20.6f\n", "P(height= greater_than_55|gender=F)", param[i].peff[1][1], final_param.peff[1][1]);
    }

    return 0;

}


// arrayopt.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#define FMT_HEADER_ONLY
#define CXXOPTIONS_NO_EXCEPTIONS
#include <iostream>
#include <filesystem>
#include <complex>
#include <fmt/format.h> //while waiting for official C++20 support
#include <Eigen/Dense>
#include <boost/math/quadrature/gauss.hpp>
#include <cxxopts.hpp>

namespace bmq = boost::math::quadrature;

#define pi EIGEN_PI

constexpr int EXIT_UNRECOGNIZED_OPTION = 2;
constexpr int EXIT_LINALG_FAILURE = 3;

std::string header(const size_t width, const std::string msg)
{
    std::string hdr = "";
    size_t hmn = (width - msg.size()-4);
    if (hmn % 2 == 1)
    {
        hmn += 1;
    }
    hdr += "\n";
    for (int i = 0; i < hmn / 2; i++)
    {
        hdr += "-";
    }
    hdr += " " + msg + " ";
    while (!(hdr.size() == width))
    {
        hdr += "-";
    }
    hdr += "\n";
    return hdr;
}

double f_dipole(double theta, double phi)
{
    return std::sin(theta);
}

std::complex<double> blm_integrand(double k, double theta, double phi, double xl, double xm, double yl, double ym)
{
    double amp = std::pow(f_dipole(theta, phi), 2);
    double tmx = k * (xl - xm) * std::sin(theta) * std::cos(phi);
    double tmy = k * (yl - ym) * std::sin(theta) * std::sin(phi);
    std::complex<double> arg(0, -(tmx+tmy)); //-i(tmx+tmy)

    return amp * std::exp(arg);
}

std::complex<double> blm(double k, double xl, double xm, double yl, double ym)
{
    // what are our options to do the double integral here??
    // https://royalsocietypublishing.org/doi/pdf/10.1098/rspa.2016.0401
    // delunay integration https://scholarworks.gsu.edu/cgi/viewcontent.cgi?article=1158&context=math_theses
    // monte carlo brute force?
    // can we get this in closed form for dipole /
    return std::complex(nan(""), nan(""));
}

std::string efstr = "\n**ERROR! {}\n";
std::string msg;

int main(int argc, char** argv)
{
    cxxopts::Options options("arrayopt", "Uses M.T. Ma's approach to compute array currents for optimal directivity.");
    options.add_options()
        ("help", "Print this screen")
        ("freq", "Frequency in MHz", cxxopts::value<double>()->default_value("1.8"))
        ("az", "Target azimuth angle, degrees", cxxopts::value<double>()->default_value("0.0"))
        ("el", "Target elevation angle, degrees", cxxopts::value<double>()->default_value("0.0"))
        ("file", "Wire description file, EZNEC export format, units in meters/mm", cxxopts::value<std::string>())
        ;

    double az, el, azrad, elrad, freq;
    std::string fn;
    try
    {
        auto opts = options.parse(argc, argv);
        if (opts.count("help"))
        {
            std::cout << options.help() << std::endl;
            return EXIT_FAILURE;
        }
        az = opts["az"].as<double>();
        el = opts["el"].as<double>();
        freq = opts["freq"].as<double>();
        azrad = pi * az / 180;
        elrad = pi * az / 180;
    }
    catch (cxxopts::OptionParseException& e)
    {
        std::cout << fmt::format(efstr, e.what()) << std::endl;
        return EXIT_UNRECOGNIZED_OPTION;
    }

    msg = "N3OX / M.T. Ma optimal directivity array calculator";
    std::cout << header(80, msg) << std::endl;

    double k0 = 2 * pi / (freq * 1e6); //wavenumber

    msg = "azimuth: {0:.3f} degrees, elevation: {1:.3f} degrees, freq: {2:.3f} MHz, wavenumber: {3:.3e}/m";
    std::cout << fmt::format(msg, az, el, freq, k0) << std::endl;

    // ==== hardcode some element positions for now ====
    std::vector<double> elx;
    std::vector<double> ely;
    for (int i = 0; i < 3; i++)
    {
        elx.push_back(20.0 * i);
        ely.push_back(0.0);
    }
    if (!(elx.size() == ely.size()))
    {
        std::cout << fmt::format(efstr, "Element x, y coordinates not the same size!") << std::endl;
    }
    size_t Nel = elx.size();

    // ==== B matrix ====
    Eigen::MatrixXcd B;
    B.resize(Nel, Nel);

    for (size_t l = 0; l < Nel; l++)
    {
        for (size_t m = 0; m < Nel; m++)
        {
            B(l, m) = std::complex(l / 10.0, m / 10.0);
        }
    }

    std::cout << B << std::endl;

    std::cout << header(80, "Finished");
    return EXIT_SUCCESS;
}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started:
//   1. Use the Solution Explorer window to add/manage files
//   2. Use the Team Explorer window to connect to source control
//   3. Use the Output window to see build output and other messages
//   4. Use the Error List window to view errors
//   5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
//   6. In the future, to open this project again, go to File > Open > Project and select the .sln file


/**
 * @file fastmks_test.cpp
 * @author Ryan Curtin
 *
 * Ensure that fast max-kernel search is correct.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/fastmks/fastmks.hpp>
#include <mlpack/methods/fastmks/fastmks_model.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::tree;
using namespace mlpack::fastmks;
using namespace mlpack::kernel;
using namespace mlpack::metric;

BOOST_AUTO_TEST_SUITE(FastMKSTest);

/**
 * Compare single-tree and naive.
 */
BOOST_AUTO_TEST_CASE(SingleTreeVsNaive)
{
  // First create a random dataset.
  arma::mat data;
  data.randn(5, 1000);
  LinearKernel lk;

  // Now run FastMKS naively.
  FastMKS<LinearKernel> naive(data, lk, false, true);

  arma::Mat<size_t> naiveIndices;
  arma::mat naiveProducts;
  naive.Search(10, naiveIndices, naiveProducts);

  // Now run it in single-tree mode.
  FastMKS<LinearKernel> single(data, lk, true);

  arma::Mat<size_t> singleIndices;
  arma::mat singleProducts;
  single.Search(10, singleIndices, singleProducts);

  // Compare the results.
  for (size_t q = 0; q < singleIndices.n_cols; ++q)
  {
    for (size_t r = 0; r < singleIndices.n_rows; ++r)
    {
      BOOST_REQUIRE_EQUAL(singleIndices(r, q), naiveIndices(r, q));
      BOOST_REQUIRE_CLOSE(singleProducts(r, q), naiveProducts(r, q), 1e-5);
    }
  }
}

/**
 * Compare dual-tree and naive.
 */
BOOST_AUTO_TEST_CASE(DualTreeVsNaive)
{
  // First create a random dataset.
  arma::mat data;
  data.randn(10, 5000);
  LinearKernel lk;

  // Now run FastMKS naively.
  FastMKS<LinearKernel> naive(data, lk, false, true);

  arma::Mat<size_t> naiveIndices;
  arma::mat naiveProducts;
  naive.Search(10, naiveIndices, naiveProducts);

  // Now run it in dual-tree mode.
  FastMKS<LinearKernel> tree(data, lk);

  arma::Mat<size_t> treeIndices;
  arma::mat treeProducts;
  tree.Search(10, treeIndices, treeProducts);

  for (size_t q = 0; q < treeIndices.n_cols; ++q)
  {
    for (size_t r = 0; r < treeIndices.n_rows; ++r)
    {
      BOOST_REQUIRE_EQUAL(treeIndices(r, q), naiveIndices(r, q));
      BOOST_REQUIRE_CLOSE(treeProducts(r, q), naiveProducts(r, q), 1e-5);
    }
  }
}

/**
 * Compare dual-tree and single-tree on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(DualTreeVsSingleTree)
{
  // First create a random dataset.
  arma::mat data;
  data.randu(8, 5000);
  PolynomialKernel pk(5.0, 2.5);

  FastMKS<PolynomialKernel> single(data, pk, true);

  arma::Mat<size_t> singleIndices;
  arma::mat singleProducts;
  single.Search(10, singleIndices, singleProducts);

  // Now run it in dual-tree mode.
  FastMKS<PolynomialKernel> tree(data, pk);

  arma::Mat<size_t> treeIndices;
  arma::mat treeProducts;
  tree.Search(10, treeIndices, treeProducts);

  for (size_t q = 0; q < treeIndices.n_cols; ++q)
  {
    for (size_t r = 0; r < treeIndices.n_rows; ++r)
    {
      BOOST_REQUIRE_EQUAL(treeIndices(r, q), singleIndices(r, q));
      BOOST_REQUIRE_CLOSE(treeProducts(r, q), singleProducts(r, q), 1e-5);
    }
  }
}

/**
 * Test sparse FastMKS (how useful is this, I'm not sure).
 */
BOOST_AUTO_TEST_CASE(SparseFastMKSTest)
{
  // First create a random sparse dataset.
  arma::sp_mat dataset;
  dataset.sprandu(10, 100, 0.3);

  FastMKS<LinearKernel, arma::sp_mat> sparsemks(dataset);

  arma::mat denseset(dataset);
  FastMKS<LinearKernel> densemks(denseset);

  // Store the results in these.
  arma::Mat<size_t> sparseIndices, denseIndices;
  arma::mat sparseKernels, denseKernels;

  // Do the searches.
  sparsemks.Search(3, sparseIndices, sparseKernels);
  densemks.Search(3, denseIndices, denseKernels);

  // Make sure the results are the same.
  for (size_t i = 0; i < sparseIndices.n_cols; ++i)
  {
    for (size_t j = 0; j < sparseIndices.n_rows; ++j)
    {
      if (std::abs(sparseKernels(j, i)) > 1e-15)
        BOOST_REQUIRE_CLOSE(sparseKernels(j, i), denseKernels(j, i), 1e-5);
      else
        BOOST_REQUIRE_SMALL(denseKernels(j, i), 1e-15);
      BOOST_REQUIRE_EQUAL(sparseIndices(j, i), denseIndices(j, i));
    }
  }
}

BOOST_AUTO_TEST_CASE(SparsePolynomialFastMKSTest)
{
  // Do it again with the polynomial kernel, just to be sure.
  arma::sp_mat dataset;
  dataset.sprandu(10, 100, 0.3);
  arma::mat denseset(dataset);

  PolynomialKernel pk(3);

  for (size_t i = 0; i < 100; ++i)
    for (size_t j = 0; j < 100; ++j)
      if (std::abs(pk.Evaluate(dataset.col(i), dataset.col(j))) < 1e-10)
        BOOST_REQUIRE_SMALL(pk.Evaluate(denseset.col(i), denseset.col(j)), 1e-10);
      else
        BOOST_REQUIRE_CLOSE(pk.Evaluate(dataset.col(i), dataset.col(j)),
                            pk.Evaluate(denseset.col(i), denseset.col(j)),
                            1e-5);

  FastMKS<PolynomialKernel, arma::sp_mat> sparsepoly(dataset);
  FastMKS<PolynomialKernel> densepoly(denseset);

  // Store the results in these.
  arma::Mat<size_t> sparseIndices, denseIndices;
  arma::mat sparseKernels, denseKernels;

  // Do the searches.
  sparsepoly.Search(3, sparseIndices, sparseKernels);
  densepoly.Search(3, denseIndices, denseKernels);

  // Make sure the results are the same.
  for (size_t i = 0; i < sparseIndices.n_cols; ++i)
  {
    for (size_t j = 0; j < sparseIndices.n_rows; ++j)
    {
      if (std::abs(sparseKernels(j, i)) > 1e-15)
        BOOST_REQUIRE_CLOSE(sparseKernels(j, i), denseKernels(j, i), 1e-5);
      else
        BOOST_REQUIRE_SMALL(denseKernels(j, i), 1e-15);
      BOOST_REQUIRE_EQUAL(sparseIndices(j, i), denseIndices(j, i));
    }
  }
}

// Make sure the empty constructor works.
BOOST_AUTO_TEST_CASE(EmptyConstructorTest)
{
  FastMKS<LinearKernel> f;

  arma::mat queryData = arma::randu<arma::mat>(5, 100);
  arma::Mat<size_t> indices;
  arma::mat products;
  BOOST_REQUIRE_THROW(f.Search(queryData, 3, indices, products),
      std::invalid_argument);
}

// Make sure the simplest overload of Train() works.
BOOST_AUTO_TEST_CASE(SimpleTrainTest)
{
  arma::mat referenceSet = arma::randu<arma::mat>(5, 100);

  FastMKS<LinearKernel> f(referenceSet);
  FastMKS<LinearKernel> f2;
  f2.Train(referenceSet);

  arma::Mat<size_t> indices, indices2;
  arma::mat products, products2;

  arma::mat querySet = arma::randu<arma::mat>(5, 20);

  f.Search(querySet, 3, indices, products);
  f2.Search(querySet, 3, indices2, products2);

  BOOST_REQUIRE_EQUAL(indices.n_rows, indices2.n_rows);
  BOOST_REQUIRE_EQUAL(products.n_rows, products2.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_cols, indices2.n_cols);
  BOOST_REQUIRE_EQUAL(products.n_cols, products2.n_cols);

  for (size_t i = 0; i < products.n_elem; ++i)
  {
    if (std::abs(products[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(products2[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(products[i], products2[i], 1e-5);

    BOOST_REQUIRE_EQUAL(indices[i], indices2[i]);
  }
}

// Test the Train() overload that takes a kernel too.
BOOST_AUTO_TEST_CASE(SimpleTrainKernelTest)
{
  arma::mat referenceSet = arma::randu<arma::mat>(5, 100);
  GaussianKernel gk(2.0);

  FastMKS<GaussianKernel> f(referenceSet, gk);
  FastMKS<GaussianKernel> f2;
  f2.Train(referenceSet, gk);

  arma::Mat<size_t> indices, indices2;
  arma::mat products, products2;

  arma::mat querySet = arma::randu<arma::mat>(5, 20);

  f.Search(querySet, 3, indices, products);
  f2.Search(querySet, 3, indices2, products2);

  BOOST_REQUIRE_EQUAL(indices.n_rows, indices2.n_rows);
  BOOST_REQUIRE_EQUAL(products.n_rows, products2.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_cols, indices2.n_cols);
  BOOST_REQUIRE_EQUAL(products.n_cols, products2.n_cols);

  for (size_t i = 0; i < products.n_elem; ++i)
  {
    if (std::abs(products[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(products2[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(products[i], products2[i], 1e-5);

    BOOST_REQUIRE_EQUAL(indices[i], indices2[i]);
  }
}

BOOST_AUTO_TEST_CASE(SerializationTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 200);

  FastMKS<LinearKernel> f(dataset);

  FastMKS<LinearKernel> fXml, fText, fBinary;
  arma::mat otherDataset = arma::randu<arma::mat>(3, 10);
  fBinary.Train(otherDataset);

  SerializeObjectAll(f, fXml, fText, fBinary);

  arma::mat kernels, xmlKernels, textKernels, binaryKernels;
  arma::Mat<size_t> indices, xmlIndices, textIndices, binaryIndices;

  arma::mat querySet = arma::randu<arma::mat>(5, 100);

  f.Search(querySet, 5, indices, kernels);
  fXml.Search(querySet, 5, xmlIndices, xmlKernels);
  fText.Search(querySet, 5, textIndices, textKernels);
  fBinary.Search(querySet, 5, binaryIndices, binaryKernels);

  CheckMatrices(indices, xmlIndices, textIndices, binaryIndices);
  CheckMatrices(kernels, xmlKernels, textKernels, binaryKernels);
}

// Make sure that we get an exception if we try to build the wrong FastMKSModel.
BOOST_AUTO_TEST_CASE(FastMKSModelWrongModelTest)
{
  PolynomialKernel pk(2.0);
  arma::mat data = arma::randu<arma::mat>(5, 5);

  FastMKSModel m(FastMKSModel::LINEAR_KERNEL);
  BOOST_REQUIRE_THROW(m.BuildModel(data, pk, false, false, 2.0),
      std::invalid_argument);
}

// Test the linear kernel mode of the FastMKSModel.
BOOST_AUTO_TEST_CASE(FastMKSModelLinearTest)
{
  LinearKernel lk;
  arma::mat referenceData = arma::randu<arma::mat>(10, 100);

  FastMKS<LinearKernel> f(referenceData, lk);

  FastMKSModel m(FastMKSModel::LINEAR_KERNEL);
  FastMKSModel mNaive(FastMKSModel::LINEAR_KERNEL);
  FastMKSModel mSingle(FastMKSModel::LINEAR_KERNEL);

  m.BuildModel(referenceData, lk, false, false, 2.0);
  mNaive.BuildModel(referenceData, lk, false, true, 2.0);
  mSingle.BuildModel(referenceData, lk, true, false, 2.0);

  // Now search, first monochromatically.
  arma::Mat<size_t> indices, mIndices, mNaiveIndices, mSingleIndices;
  arma::mat kernels, mKernels, mNaiveKernels, mSingleKernels;

  f.Search(3, indices, kernels);
  m.Search(3, mIndices, mKernels);
  mNaive.Search(3, mNaiveIndices, mNaiveKernels);
  mSingle.Search(3, mSingleIndices, mSingleKernels);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }

  // Now test with a different query set.
  arma::mat querySet = arma::randu<arma::mat>(10, 50);

  f.Search(querySet, 3, indices, kernels);
  m.Search(querySet, 3, mIndices, mKernels, 2.0);
  mNaive.Search(querySet, 3, mNaiveIndices, mNaiveKernels, 2.0);
  mSingle.Search(querySet, 3, mSingleIndices, mSingleKernels, 2.0);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }
}

// Test the polynomial kernel mode of the FastMKSModel.
BOOST_AUTO_TEST_CASE(FastMKSModelPolynomialTest)
{
  PolynomialKernel pk(2.0);
  arma::mat referenceData = arma::randu<arma::mat>(10, 100);

  FastMKS<PolynomialKernel> f(referenceData, pk);

  FastMKSModel m(FastMKSModel::POLYNOMIAL_KERNEL);
  FastMKSModel mNaive(FastMKSModel::POLYNOMIAL_KERNEL);
  FastMKSModel mSingle(FastMKSModel::POLYNOMIAL_KERNEL);

  m.BuildModel(referenceData, pk, false, false, 2.0);
  mNaive.BuildModel(referenceData, pk, false, true, 2.0);
  mSingle.BuildModel(referenceData, pk, true, false, 2.0);

  // Now search, first monochromatically.
  arma::Mat<size_t> indices, mIndices, mNaiveIndices, mSingleIndices;
  arma::mat kernels, mKernels, mNaiveKernels, mSingleKernels;

  f.Search(3, indices, kernels);
  m.Search(3, mIndices, mKernels);
  mNaive.Search(3, mNaiveIndices, mNaiveKernels);
  mSingle.Search(3, mSingleIndices, mSingleKernels);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }

  // Now test with a different query set.
  arma::mat querySet = arma::randu<arma::mat>(10, 50);

  f.Search(querySet, 3, indices, kernels);
  m.Search(querySet, 3, mIndices, mKernels, 2.0);
  mNaive.Search(querySet, 3, mNaiveIndices, mNaiveKernels, 2.0);
  mSingle.Search(querySet, 3, mSingleIndices, mSingleKernels, 2.0);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }
}

// Test the cosine distance mode of the FastMKSModel.
BOOST_AUTO_TEST_CASE(FastMKSModelCosineTest)
{
  CosineDistance ck;
  arma::mat referenceData = arma::randu<arma::mat>(10, 100);

  FastMKS<CosineDistance> f(referenceData, ck);

  FastMKSModel m(FastMKSModel::COSINE_DISTANCE);
  FastMKSModel mNaive(FastMKSModel::COSINE_DISTANCE);
  FastMKSModel mSingle(FastMKSModel::COSINE_DISTANCE);

  m.BuildModel(referenceData, ck, false, false, 2.0);
  mNaive.BuildModel(referenceData, ck, false, true, 2.0);
  mSingle.BuildModel(referenceData, ck, true, false, 2.0);

  // Now search, first monochromatically.
  arma::Mat<size_t> indices, mIndices, mNaiveIndices, mSingleIndices;
  arma::mat kernels, mKernels, mNaiveKernels, mSingleKernels;

  f.Search(3, indices, kernels);
  m.Search(3, mIndices, mKernels);
  mNaive.Search(3, mNaiveIndices, mNaiveKernels);
  mSingle.Search(3, mSingleIndices, mSingleKernels);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }

  // Now test with a different query set.
  arma::mat querySet = arma::randu<arma::mat>(10, 50);

  f.Search(querySet, 3, indices, kernels);
  m.Search(querySet, 3, mIndices, mKernels, 2.0);
  mNaive.Search(querySet, 3, mNaiveIndices, mNaiveKernels, 2.0);
  mSingle.Search(querySet, 3, mSingleIndices, mSingleKernels, 2.0);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }
}

// Test the Gaussian kernel mode of the FastMKSModel.
BOOST_AUTO_TEST_CASE(FastMKSModelGaussianTest)
{
  GaussianKernel gk(1.5);
  arma::mat referenceData = arma::randu<arma::mat>(10, 100);

  FastMKS<GaussianKernel> f(referenceData, gk);

  FastMKSModel m(FastMKSModel::GAUSSIAN_KERNEL);
  FastMKSModel mNaive(FastMKSModel::GAUSSIAN_KERNEL);
  FastMKSModel mSingle(FastMKSModel::GAUSSIAN_KERNEL);

  m.BuildModel(referenceData, gk, false, false, 2.0);
  mNaive.BuildModel(referenceData, gk, false, true, 2.0);
  mSingle.BuildModel(referenceData, gk, true, false, 2.0);

  // Now search, first monochromatically.
  arma::Mat<size_t> indices, mIndices, mNaiveIndices, mSingleIndices;
  arma::mat kernels, mKernels, mNaiveKernels, mSingleKernels;

  f.Search(3, indices, kernels);
  m.Search(3, mIndices, mKernels);
  mNaive.Search(3, mNaiveIndices, mNaiveKernels);
  mSingle.Search(3, mSingleIndices, mSingleKernels);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }

  // Now test with a different query set.
  arma::mat querySet = arma::randu<arma::mat>(10, 50);

  f.Search(querySet, 3, indices, kernels);
  m.Search(querySet, 3, mIndices, mKernels, 2.0);
  mNaive.Search(querySet, 3, mNaiveIndices, mNaiveKernels, 2.0);
  mSingle.Search(querySet, 3, mSingleIndices, mSingleKernels, 2.0);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }
}

// Test the Epanechnikov kernel mode of the FastMKSModel.
BOOST_AUTO_TEST_CASE(FastMKSModelEpanTest)
{
  EpanechnikovKernel ek(2.5);
  arma::mat referenceData = arma::randu<arma::mat>(10, 100);

  FastMKS<EpanechnikovKernel> f(referenceData, ek);

  FastMKSModel m(FastMKSModel::EPANECHNIKOV_KERNEL);
  FastMKSModel mNaive(FastMKSModel::EPANECHNIKOV_KERNEL);
  FastMKSModel mSingle(FastMKSModel::EPANECHNIKOV_KERNEL);

  m.BuildModel(referenceData, ek, false, false, 2.0);
  mNaive.BuildModel(referenceData, ek, false, true, 2.0);
  mSingle.BuildModel(referenceData, ek, true, false, 2.0);

  // Now search, first monochromatically.
  arma::Mat<size_t> indices, mIndices, mNaiveIndices, mSingleIndices;
  arma::mat kernels, mKernels, mNaiveKernels, mSingleKernels;

  f.Search(3, indices, kernels);
  m.Search(3, mIndices, mKernels);
  mNaive.Search(3, mNaiveIndices, mNaiveKernels);
  mSingle.Search(3, mSingleIndices, mSingleKernels);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }

  // Now test with a different query set.
  arma::mat querySet = arma::randu<arma::mat>(10, 50);

  f.Search(querySet, 3, indices, kernels);
  m.Search(querySet, 3, mIndices, mKernels, 2.0);
  mNaive.Search(querySet, 3, mNaiveIndices, mNaiveKernels, 2.0);
  mSingle.Search(querySet, 3, mSingleIndices, mSingleKernels, 2.0);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }
}

// Test the triangular kernel mode of the FastMKSModel.
BOOST_AUTO_TEST_CASE(FastMKSModelTriangularTest)
{
  TriangularKernel tk(2.0);
  arma::mat referenceData = arma::randu<arma::mat>(10, 100);

  FastMKS<TriangularKernel> f(referenceData, tk);

  FastMKSModel m(FastMKSModel::TRIANGULAR_KERNEL);
  FastMKSModel mNaive(FastMKSModel::TRIANGULAR_KERNEL);
  FastMKSModel mSingle(FastMKSModel::TRIANGULAR_KERNEL);

  m.BuildModel(referenceData, tk, false, false, 2.0);
  mNaive.BuildModel(referenceData, tk, false, true, 2.0);
  mSingle.BuildModel(referenceData, tk, true, false, 2.0);

  // Now search, first monochromatically.
  arma::Mat<size_t> indices, mIndices, mNaiveIndices, mSingleIndices;
  arma::mat kernels, mKernels, mNaiveKernels, mSingleKernels;

  f.Search(3, indices, kernels);
  m.Search(3, mIndices, mKernels);
  mNaive.Search(3, mNaiveIndices, mNaiveKernels);
  mSingle.Search(3, mSingleIndices, mSingleKernels);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }

  // Now test with a different query set.
  arma::mat querySet = arma::randu<arma::mat>(10, 50);

  f.Search(querySet, 3, indices, kernels);
  m.Search(querySet, 3, mIndices, mKernels, 2.0);
  mNaive.Search(querySet, 3, mNaiveIndices, mNaiveKernels, 2.0);
  mSingle.Search(querySet, 3, mSingleIndices, mSingleKernels, 2.0);

  BOOST_REQUIRE_EQUAL(indices.n_cols, mIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mNaiveIndices.n_cols);
  BOOST_REQUIRE_EQUAL(indices.n_cols, mSingleIndices.n_cols);

  BOOST_REQUIRE_EQUAL(indices.n_rows, mIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mNaiveIndices.n_rows);
  BOOST_REQUIRE_EQUAL(indices.n_rows, mSingleIndices.n_rows);

  BOOST_REQUIRE_EQUAL(kernels.n_cols, mKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mNaiveKernels.n_cols);
  BOOST_REQUIRE_EQUAL(kernels.n_cols, mSingleKernels.n_cols);

  BOOST_REQUIRE_EQUAL(kernels.n_rows, mKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mNaiveKernels.n_rows);
  BOOST_REQUIRE_EQUAL(kernels.n_rows, mSingleKernels.n_rows);

  for (size_t i = 0; i < indices.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(indices[i], mIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mNaiveIndices[i]);
    BOOST_REQUIRE_EQUAL(indices[i], mSingleIndices[i]);

    if (std::abs(kernels[i]) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(mKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_SMALL(mSingleKernels[i], 1e-5);
    }
    else
    {
      BOOST_REQUIRE_CLOSE(kernels[i], mKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mNaiveKernels[i], 1e-5);
      BOOST_REQUIRE_CLOSE(kernels[i], mSingleKernels[i], 1e-5);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>

#include "IO/readPLY.h"
#include "IO/writePLY.h"

#include "visualization/plotMesh.h"
#include "visualization/plotTwoMeshes.h"

#include "mesh/computeNormals.h"
#include "mesh/computeFacesCentroids.h"
#include "sdf.h"

int main() {
    bool visualization = true;
    int grid_resolution = 100;
    double bounding_box_scale = 1;

    // IO: load files
    std::cout << "Progress: load data\n";
    Eigen::MatrixXd V, faces_V;
    Eigen::MatrixXi F;
    Eigen::MatrixXd N, faces_N;
    Eigen::MatrixXi RGB;

    readPLY("../data/Lucy100k.ply", V, F, N, RGB);

    faces_V = compute_faces_centroids(V,F);
    faces_N = compute_faces_normals(V,F);

    if (visualization)
        plot_mesh(V,F);

    // grid_resolution is used to define the number of grids

    SDF sdf(faces_V, faces_N, grid_resolution, bounding_box_scale);

    Eigen::MatrixXd graph_V;
    Eigen::MatrixXi graph_E;
    sdf.generate_graph(graph_V, graph_E);
    sdf.print_to_folder("../data/sdf/");

}


// File: recursator.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>

int main(int, char**)
{
    using namespace mtl; using std::cout;

    // Z-order matrix
    typedef morton_dense<double, recursion::morton_z_mask>  matrix_type;
    matrix_type                                             A(10, 10);
    mat::hessian_setup(A, 3.0);

    // Define a recursator over A
    mat::recursator<matrix_type>                          rec(A);

    // Access a quadrant of the matrix
    cout << "Upper right quadrant (north_east) of A is \n" << *north_east(rec) << "\n";

    // Access a quadrant's quadrant of the matrix
    cout << "Lower left (south_west) of upper right quadrant (north_east) of A is \n"
	 << *south_west(north_east(rec)) << "\n";

    cout << "The virtual bound of 'rec' is " << rec.bound() << "\n";

    return 0;
}


#define BOOST_TEST_MODULE test_mul
#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mave/mave.hpp>
#include <tests/generate_random_matrices.hpp>
#include <tests/tolerance.hpp>

typedef boost::mpl::list<
    mave::vector<double, 3>,    mave::vector<float, 3>,
    mave::matrix<double, 3, 3>, mave::matrix<float, 3, 3>
    > test_targets;

constexpr std::size_t N = 12000;

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplication_1arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto& s  = scalars.at(i);
        const auto& v1 = vectors.at(i);

        const auto v2 = s * v1;
        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(v2[j] == s * v1[j],
                       mave::test::tolerance<typename T::value_type>());
        }

        const auto v3 = v1 * s;
        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(v3[j] == v1[j] * s,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
        BOOST_TEST(v3.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplication_2arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=2)
    {
        const auto& s1 = scalars.at(i);
        const auto& s2 = scalars.at(i+1);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);

        const auto v2 = std::make_tuple(s1, s2) * std::tie(v11, v12);

        const auto v21 = std::get<0>(v2);
        const auto v22 = std::get<1>(v2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == s1 * v11[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == s2 * v12[j],
                       mave::test::tolerance<typename T::value_type>());
        }

        const auto v3 = std::tie(v11, v12) * std::make_tuple(s1, s2);

        const auto v31 = std::get<0>(v3);
        const auto v32 = std::get<1>(v3);

        for(std::size_t j=0; j<v31.size(); ++j)
        {
            BOOST_TEST(v31[j] == v11[j] * s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v32[j] == v12[j] * s2,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v31.diagnosis());
        BOOST_TEST(v32.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplication_3arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=3)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);

        const auto v2 = std::make_tuple(s1, s2, s3) * std::tie(v11, v12, v13);

        const auto v21 = std::get<0>(v2);
        const auto v22 = std::get<1>(v2);
        const auto v23 = std::get<2>(v2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == s1 * v11[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == s2 * v12[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == s3 * v13[j],
                       mave::test::tolerance<typename T::value_type>());
        }

        const auto v3 = std::tie(v11, v12, v13) * std::make_tuple(s1, s2, s3);

        const auto v31 = std::get<0>(v3);
        const auto v32 = std::get<1>(v3);
        const auto v33 = std::get<2>(v3);

        for(std::size_t j=0; j<v31.size(); ++j)
        {
            BOOST_TEST(v31[j] == v11[j] * s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v32[j] == v12[j] * s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v33[j] == v13[j] * s3,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());

        BOOST_TEST(v31.diagnosis());
        BOOST_TEST(v32.diagnosis());
        BOOST_TEST(v33.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(multiplication_4arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=4)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& s4  = scalars.at(i+3);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);
        const auto& v14 = vectors.at(i+3);

        const auto v2 = std::make_tuple(s1, s2, s3, s4) * std::tie(v11, v12, v13, v14);

        const auto v21 = std::get<0>(v2);
        const auto v22 = std::get<1>(v2);
        const auto v23 = std::get<2>(v2);
        const auto v24 = std::get<3>(v2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == s1 * v11[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == s2 * v12[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == s3 * v13[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v24[j] == s4 * v14[j],
                       mave::test::tolerance<typename T::value_type>());
        }

        const auto v3 = std::tie(v11, v12, v13, v14) * std::make_tuple(s1, s2, s3, s4);

        const auto v31 = std::get<0>(v3);
        const auto v32 = std::get<1>(v3);
        const auto v33 = std::get<2>(v3);
        const auto v34 = std::get<3>(v3);

        for(std::size_t j=0; j<v31.size(); ++j)
        {
            BOOST_TEST(v31[j] == v11[j] * s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v32[j] == v12[j] * s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v33[j] == v13[j] * s3,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v34[j] == v14[j] * s4,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());
        BOOST_TEST(v14.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
        BOOST_TEST(v24.diagnosis());

        BOOST_TEST(v31.diagnosis());
        BOOST_TEST(v32.diagnosis());
        BOOST_TEST(v33.diagnosis());
        BOOST_TEST(v34.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(mul_assign_1arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto& s  = scalars.at(i);
        const auto& v1 = vectors.at(i);

        auto v2 = v1;
        v2 *= s;
        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(v2[j] == v1[j] * s,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(mul_assign_2arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=2)
    {
        const auto& s1 = scalars.at(i);
        const auto& s2 = scalars.at(i+1);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);

        auto v21(v11);
        auto v22(v12);
        std::tie(v21, v22) *= std::make_tuple(s1, s2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] * s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] * s2,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(mul_assign_3arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=3)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);

        auto v21(v11);
        auto v22(v12);
        auto v23(v13);
        std::tie(v21, v22, v23) *= std::make_tuple(s1, s2, s3);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] * s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] * s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == v13[j] * s3,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(mul_assign_4arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=4)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& s4  = scalars.at(i+3);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);
        const auto& v14 = vectors.at(i+3);

        auto v21(v11);
        auto v22(v12);
        auto v23(v13);
        auto v24(v14);
        std::tie(v21, v22, v23, v24) *= std::make_tuple(s1, s2, s3, s4);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] * s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] * s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == v13[j] * s3,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v24[j] == v14[j] * s4,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());
        BOOST_TEST(v14.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
        BOOST_TEST(v24.diagnosis());
    }
}


#include <cstdlib>
#include <iostream>
#include <complex>
#define BOOST_UBLAS_NO_ELEMENT_PROXIES
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/unit_lower.hpp>
#include <boost/numeric/bindings/unit_upper.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include <boost/numeric/bindings/left.hpp>
#include <boost/numeric/bindings/right.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef typename vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix A_l(n, n), A_u(n, n), B(n, n);
    for (size_type j=0; j<n; ++j) {
      for (size_type i=0; i<j; ++i) {
    	A_u(i, j)=rand_normal<complex>::get();
	A_l(j, i)=rand_normal<complex>::get();
      }
      A_u(j, j)=rand_normal<complex>::get();
      A_l(j, j)=rand_normal<complex>::get();
      for (size_type i=j+1; i<n; ++i) {
    	A_u(i, j)=0;
	A_l(j, i)=0;
      }
    }
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
	B(j, i)=rand_normal<complex>::get();
    complex alpha=rand_normal<complex>::get();
    {
      matrix B1(alpha*ublas::prod(A_l, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(A_u, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_l), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_u), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_l)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_u)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

    {
      matrix B1(alpha*ublas::prod(B, A_l));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, A_u));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_l)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_u)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_l))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_u))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

    for (size_type i=0; i<n; ++i) {
      A_l(i, i)=1;
      A_u(i, i)=1;
    }

    {
      matrix B1(alpha*ublas::prod(A_l, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::unit_lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(A_u, B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::unit_upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_l), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::trans(A_u), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::trans(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_l)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(ublas::conj(ublas::trans(A_u)), B));
      matrix B2(B);
      blas::trmm(blas::left(), alpha, blas::conj(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (left multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (left multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

    {
      matrix B1(alpha*ublas::prod(B, A_l));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::unit_lower(A_l), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, A_u));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::unit_upper(A_u), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_l)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::trans(A_u)));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::trans(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_l))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::unit_lower(A_l)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_lower):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_lower):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }
    {
      matrix B1(alpha*ublas::prod(B, ublas::conj(ublas::trans(A_u))));
      matrix B2(B);
      blas::trmm(blas::right(), alpha, blas::conj(blas::unit_upper(A_u)), B2);
      std::cout << "testing boost::ublas containers\n"
     		<< "using ublas (right multiply, unit_upper):\n" << print_mat(B1) << '\n'
    		<< "using blas  (right multiply, unit_upper):\n" << print_mat(B2) << '\n'
    		<< '\n';
    }

  }
  return EXIT_SUCCESS;
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>
#include <limits>
#include <stdexcept>

#include <boost/math/constants/constants.hpp>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h"

namespace tudat
{

namespace gravitation
{

//! Compute gravitational acceleration due to multiple spherical harmonics terms, defined using geodesy-normalization.
Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameter,
        const double equatorialRadius,
        const Eigen::MatrixXd& cosineHarmonicCoefficients,
        const Eigen::MatrixXd& sineHarmonicCoefficients,
        std::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache,
        std::map< std::pair< int, int >, Eigen::Vector3d >& accelerationPerTerm,
        const bool saveSeparateTerms,
        const Eigen::Matrix3d& accelerationRotation )
{
    // Set highest degree and order.
    const int highestDegree = cosineHarmonicCoefficients.rows( );
    const int highestOrder = cosineHarmonicCoefficients.cols( );

    // Declare spherical position vector.
    Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = coordinate_conversions::
            convertCartesianToSpherical( positionOfBodySubjectToAcceleration );
    sphericalpositionOfBodySubjectToAcceleration( 1 ) = mathematical_constants::PI / 2.0 -
            sphericalpositionOfBodySubjectToAcceleration( 1 );

    double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) );
    sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ),
                                     sineOfAngle,
                                     sphericalpositionOfBodySubjectToAcceleration( 2 ),
                                     equatorialRadius );

    std::shared_ptr< basic_mathematics::LegendreCache > legendreCacheReference =
            sphericalHarmonicsCache->getLegendreCache( );

    // Compute gradient premultiplier.
    const double preMultiplier = gravitationalParameter / equatorialRadius;

    // Initialize gradient vector.
    Eigen::Vector3d sphericalGradient = Eigen::Vector3d::Zero( );

    Eigen::Matrix3d transformationToCartesianCoordinates = coordinate_conversions::getSphericalToCartesianGradientMatrix(
                positionOfBodySubjectToAcceleration );

    // Loop through all degrees.
    for ( int degree = 0; degree < highestDegree; degree++ )
    {
        // Loop through all orders.
        for ( int order = 0; ( order <= degree ) && ( order < highestOrder ); order++ )
        {
            // Compute geodesy-normalized Legendre polynomials.
            const double legendrePolynomial = legendreCacheReference->getLegendrePolynomial( degree, order );

            // Compute geodesy-normalized Legendre polynomial derivative.
            const double legendrePolynomialDerivative = legendreCacheReference->getLegendrePolynomialDerivative(
                        degree, order );

            // Compute the potential gradient of a single spherical harmonic term.
            if( saveSeparateTerms )
            {
                accelerationPerTerm[ std::make_pair( degree, order ) ] =
                        basic_mathematics::computePotentialGradient(
                            sphericalpositionOfBodySubjectToAcceleration,
                            preMultiplier,
                            degree,
                            order,
                            cosineHarmonicCoefficients( degree, order ),
                            sineHarmonicCoefficients( degree, order ),
                            legendrePolynomial,
                            legendrePolynomialDerivative, sphericalHarmonicsCache );
                sphericalGradient += accelerationPerTerm[ std::make_pair( degree, order ) ];
                accelerationPerTerm[ std::make_pair( degree, order ) ] =
                        accelerationRotation * (
                            transformationToCartesianCoordinates * accelerationPerTerm[ std::make_pair( degree, order ) ] );
            }
            else
            {
                // Compute the potential gradient of a single spherical harmonic term.
                sphericalGradient += basic_mathematics::computePotentialGradient(
                            sphericalpositionOfBodySubjectToAcceleration,
                            preMultiplier,
                            degree,
                            order,
                            cosineHarmonicCoefficients( degree, order ),
                            sineHarmonicCoefficients( degree, order ),
                            legendrePolynomial,
                            legendrePolynomialDerivative, sphericalHarmonicsCache );
            }
        }
    }

    // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector) and
    // return the resulting acceleration vector.
    return accelerationRotation * ( transformationToCartesianCoordinates * sphericalGradient );
}

//! Compute gravitational acceleration due to single spherical harmonics term.
Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameter,
        const double equatorialRadius,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        std::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache )
{
    // Declare spherical position vector.
    Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = coordinate_conversions::
            convertCartesianToSpherical( positionOfBodySubjectToAcceleration );
    sphericalpositionOfBodySubjectToAcceleration( 1 ) = mathematical_constants::PI / 2.0 -
            sphericalpositionOfBodySubjectToAcceleration( 1 );

    double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) );
    sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ),
                                     sineOfAngle,
                                     sphericalpositionOfBodySubjectToAcceleration( 2 ),
                                     equatorialRadius );

    // Compute gradient premultiplier.
    const double preMultiplier = gravitationalParameter / equatorialRadius;

    // Compute geodesy-normalized Legendre polynomials.
    const double legendrePolynomial = sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomial( degree, order );

    // Compute geodesy-normalized Legendre polynomial derivative.
    const double legendrePolynomialDerivative =
            sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomialDerivative( degree, order );

    // Compute the potential gradient of a single spherical harmonic term.
    Eigen::Vector3d sphericalGradient = basic_mathematics::computePotentialGradient(
                sphericalpositionOfBodySubjectToAcceleration,
                preMultiplier,
                degree,
                order,
                cosineHarmonicCoefficient,
                sineHarmonicCoefficient,
                legendrePolynomial,
                legendrePolynomialDerivative, sphericalHarmonicsCache );

    // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector),
    // and return resulting acceleration vector.
    return coordinate_conversions::convertSphericalToCartesianGradient(
                sphericalGradient, positionOfBodySubjectToAcceleration );
}

} // namespace gravitation

} // namespace tudat


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include <gcs/constraints/all_different.hh>
#include <gcs/constraints/comparison.hh>
#include <gcs/constraints/linear_equality.hh>
#include <gcs/problem.hh>
#include <gcs/solve.hh>

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <vector>

#include <boost/program_options.hpp>

using namespace gcs;

using std::cerr;
using std::cout;
using std::endl;
using std::ifstream;
using std::pair;
using std::stoi;
using std::vector;

namespace po = boost::program_options;

auto main(int argc, char * argv[]) -> int
{
    po::options_description display_options{ "Program options" };
    display_options.add_options()
        ("help", "Display help information")
        ("prove", "Create a proof")
        ("all-different", "Use AllDifferent rather than inequalities");

    po::options_description all_options{ "All options" };
    all_options.add_options()
        ("size", po::value<int>()->default_value(5), "Size of the problem to solve")
        ;

    all_options.add(display_options);

    po::positional_options_description positional_options;
    positional_options
        .add("size", -1);

    po::variables_map options_vars;

    try {
        po::store(po::command_line_parser(argc, argv)
                .options(all_options)
                .positional(positional_options)
                .run(), options_vars);
        po::notify(options_vars);
    }
    catch (const po::error & e) {
        cerr << "Error: " << e.what() << endl;
        cerr << "Try " << argv[0] << " --help" << endl;
        return EXIT_FAILURE;
    }

    if (options_vars.count("help")) {
        cout << "Usage: " << argv[0] << " [options] [size]" << endl;
        cout << endl;
        cout << display_options << endl;
        return EXIT_SUCCESS;
    }

    cout << "Replicating the MiniCP Magic Square benchmark." << endl;
    cout << "See Laurent D. Michel, Pierre Schaus, Pascal Van Hentenryck:" << endl;
    cout << "\"MiniCP: a lightweight solver for constraint programming.\"" << endl;
    cout << "Math. Program. Comput. 13(1): 133-184 (2021)." << endl;
    cout << "This should take 6042079 recursions with default options." << endl;
    cout << endl;

    int size = options_vars["size"].as<int>();
    Problem p = options_vars.count("prove") ? Problem{ Proof{ "magic_square.opb", "magic_square.veripb" } } : Problem{ };
    Integer m{ size * (size * size + 1) / 2 };

    vector<vector<IntegerVariableID> > grid;
    vector<IntegerVariableID> grid_flat;
    for (int x = 0 ; x < size ; ++x) {
        grid.emplace_back();
        for (int y = 0 ; y < size ; ++y) {
            auto var = p.create_integer_variable(1_i, Integer{ size * size });
            grid[x].push_back(var);
            grid_flat.push_back(var);
        }
    }

    // As far as I can tell, the statistics reported in the paper only make
    // sense for non-GAC all-different.
    if (options_vars.count("all-different")) {
        p.post(AllDifferent{ grid_flat });
    }
    else {
        for (unsigned x = 0 ; x < grid_flat.size() ; ++x)
            for (unsigned y = x + 1 ; y < grid_flat.size() ; ++y)
                p.post(NotEquals{ grid_flat[x], grid_flat[y] });
    }

    for (int x = 0 ; x < size ; ++x) {
        Linear coeff_vars;
        for (int y = 0 ; y < size ; ++y)
            coeff_vars.emplace_back(1_i, grid[x][y]);
        p.post(LinearEquality{ move(coeff_vars), m });
    }

    for (int y = 0 ; y < size ; ++y) {
        Linear coeff_vars;
        for (int x = 0 ; x < size ; ++x)
            coeff_vars.emplace_back(1_i, grid[x][y]);
        p.post(LinearEquality{ move(coeff_vars), m });
    }

    Linear coeff_vars1, coeff_vars2;
    for (int xy = 0 ; xy < size ; ++xy) {
        coeff_vars1.emplace_back(1_i, grid[xy][xy]);
        coeff_vars2.emplace_back(1_i, grid[size - xy - 1][xy]);
    }
    p.post(LinearEquality{ move(coeff_vars1), m });
    p.post(LinearEquality{ move(coeff_vars2), m });

    p.post(LessThan{ grid[0][size - 1], grid[size - 1][0] });
    p.post(LessThan{ grid[0][0], grid[size - 1][size - 1] });
    p.post(LessThan{ grid[0][0], grid[size - 1][0] });

    p.branch_on(grid_flat);

    unsigned long long n_solutions = 0;
    auto stats = solve_with(p, SolveCallbacks{
            .solution = [&] (const State &) -> bool {
                return ++n_solutions < 10000;
            },
            .guess = [&] (const State & state, IntegerVariableID var) -> vector<Literal> {
                return vector<Literal>{ var == state.lower_bound(var), var != state.lower_bound(var) };
            }
            } );

    cout << stats;

    return EXIT_SUCCESS;
}