#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>

using namespace std;
// std::ofstream debug("debug_gn.txt");
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;

// baseline
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// bilinear interpolation
inline float get(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);

    float f =
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1];
    return f;
}

Eigen::Vector3d get_3D_point_from_depth(const Eigen::Vector2d& p, double depth, const Eigen::Matrix3d& K)
{
    return Eigen::Vector3d(depth * (p.x() - K(0, 2)) / K(0, 0),
                           depth * (p.y() - K(1, 2)) / K(1, 1),
                           depth);
}

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt // points from cam1 reference frame to cam2
)
{
    int nb_iters = 11;
    int half_w_size = 1;
    double prev_cost = 0.0;
    double fx = K(0, 0);
    double fy = K(1, 1);
    double cx = K(0, 2);
    double cy = K(1, 2);

    for (int iter = 0; iter < nb_iters; iter++)
    {
        // std::cout << "Iter: " << iter << " ";
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
        Eigen::Matrix<double, 6, 1> g = Eigen::Matrix<double, 6, 1>::Zero();
        double total_cost = 0.0;
        int cnt_good = 0;
        for (int k = 0; k < px_ref.size(); ++k)
        {
            const auto& p1 = px_ref[k];
            Eigen::Vector3d P_ref = get_3D_point_from_depth(p1, depth_ref[k], K);
            Eigen::Vector3d P2  = Rt * P_ref;
            double X2 = std::pow(P2.x(), 2);
            double Y2 = std::pow(P2.y(), 2);
            double Z2 = std::pow(P2.z(), 2);

            if (P2.z() < 0) // invalid depth
                continue;

            Eigen::Vector3d p2 = K  * P2;
            p2 /= p2.z();

            if (p2.x() < half_w_size || p2.x() > img2.cols - half_w_size
                || p2.y() < half_w_size || p2.y() > img2.rows - half_w_size)
                continue;

            // debug << p1.x() << " " << p1.y() << "\n";
            // debug << p2.x() << " " << p2.y() << "\n";

            cnt_good++;
            for (int xx = -half_w_size; xx <= half_w_size; ++xx)
            {
                for (int yy = -half_w_size; yy <= half_w_size; ++yy)
                {
                    auto v1 = get(img1, p1.x() + xx, p1.y() + yy);
                    auto v2 = get(img2, p2.x() + xx, p2.y() + yy);
                    // debug << v1 << " " << v2 << "\n";

                    double dx = 0.5 * (get(img2, p2.x() + xx + 1, p2.y() + yy) - get(img2, p2.x() + xx - 1, p2.y() + yy));
                    double dy = 0.5 * (get(img2, p2.x() + xx, p2.y() + yy + 1) - get(img2, p2.x() + xx, p2.y() + yy - 1));
                    Eigen::Vector2d dIdu(dx, dy);
                    Eigen::Matrix<double, 2, 6> dudRt;
                    dudRt << fx/P2.z(), 0.0, -fx*P2.x() / Z2,-fx * P2.x() * P2.y() / Z2, fx + fx * X2 / Z2, -fx * P2.y() / P2.z(),
                             0.0, fy / P2.z(), -fy*P2.y()/Z2, -fy-fy * Y2 / Z2, fy * P2.x() * P2.y() / Z2, fy * P2.x() / P2.z();
                    Eigen::Matrix<double, 6, 1> J = -(dIdu.transpose() * dudRt).transpose();
                    // debug << p1.x() << " " << p2.y() << " " << p2.x() << " " << p2.y() << "\n";
                    // debug << v1 << " " << v2 << "\n";
                    // debug << J.transpose() << "\n";
                    double err = v1 - v2;
                    H += J * J.transpose();
                    g += -J.transpose() * err;
                    total_cost += err * err;
                }
            }
        }
        // debug << "-------------\n";
        Eigen::Matrix<double, 6, 1> delta = H.ldlt().solve(g);
        // std::cout << std::setw(4) << std::setprecision(3) << "\tcost: " << total_cost << "\t update norm: " << delta.norm() << "\n";

        if (std::isnan(delta[0]))
        {
            std::cout << "Error during optimization (linear equation solving failed)" << std::endl;
            break;
        }

        if (iter >  0 && total_cost > prev_cost)
        {
            std::cout << "Cost increased. Stop." << std::endl;
            break;
        }

        Rt = Sophus::SE3d::exp(delta) * Rt;
        prev_cost = total_cost;

        if (delta.norm() < 1e-3)
        {
            std::cout << "Optimization converged." << std::endl;
            break;
        }
    }
    std::cout << "translation: " << Rt.translation().transpose() << "\n";
    std::cout << "rotation: " << Rt.so3().unit_quaternion().toRotationMatrix() << "\n";

}

void DirectPoseEstimationPyramidal(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt)
{
    int nb_levels = 4;
    double factor = 0.5;

    std::vector<cv::Mat> pyr1, pyr2;
    std::vector<double> scales;
    for (int i = 0; i < nb_levels; ++i)
    {
        if (i == 0)
        {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
            scales.push_back(1.0);
        }
        else
        {
            cv::Mat img1_r, img2_r;
            cv::resize(pyr1[i-1], img1_r, cv::Size(pyr1[i-1].cols * factor, pyr1[i-1].rows * factor));
            cv::resize(pyr2[i-1], img2_r, cv::Size(pyr2[i-1].cols * factor, pyr2[i-1].rows * factor));
            pyr1.push_back(img1_r);
            pyr2.push_back(img2_r);
            scales.push_back(scales[i-1] * factor);
        }
    }

    for (int l = nb_levels-1; l >= 0; l--)
    {

        cv::Mat img1_r = pyr1[l];
        cv::Mat img2_r = pyr2[l];
        double scale = scales[l];

        Eigen::Matrix3d K_r = K;
        K_r(0, 0) *= scale;
        K_r(1, 1) *= scale;
        K_r(0, 2) *= scale;
        K_r(1, 2) *= scale;
        auto p_r = px_ref;
        for (auto& p : p_r)
        {
            p *= scale;
        }

        DirectPoseEstimationSingleLayer(img1_r, img2_r, p_r, depth_ref, K_r, Rt);
    }
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng(1994);
    int nPoints = 2000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d Rt;
    Eigen::Matrix3d K;
    K << fx, 0.0, cx,
         0.0, fy, cy,
         0.0, 0.0, 1.0;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);

        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, K, Rt);
        DirectPoseEstimationPyramidal(left_img, img, pixels_ref, depth_ref, K, Rt);

        // plot the projected pixels here
        cv::Mat img2_show;
        cv::cvtColor(img, img2_show, CV_GRAY2BGR);
        std::vector<Eigen::Vector2d> projections(pixels_ref.size());
        for (int i = 0; i < pixels_ref.size(); ++i)
        {
            Eigen::Vector3d P_ref = get_3D_point_from_depth(pixels_ref[i], depth_ref[i], K);
            Eigen::Vector3d uv = K * (Rt * P_ref);
            projections[i] = uv.hnormalized();
        }

        for (size_t i = 0; i < pixels_ref.size(); ++i) {
            auto p_ref = pixels_ref[i];
            auto p_cur = projections[i];
            if (p_cur[0] > 0 && p_cur[1] > 0 && p_cur[0] < img2_show.cols && p_cur[1] < img2_show.rows) {
                cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
                cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                        cv::Scalar(0, 250, 0));
            }
        }
        // cv::imshow("current", img2_show);
        // cv::waitKey();
        cv::imwrite("img_"+std::to_string(i) + ".png", img2_show);

    }
    // debug.close();

    return 0;
}


/*
 * utils.cpp
 *
 *  Created on: Dec 5, 2017
 *      Author: dumbledore
 */
#include <iostream>
#include "utils.hpp"
#include <Eigen/QR>
#include <assert.h>
#include <limits>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
Utils::Utils() {
  // nothing for now
}

Utils::~Utils() {

  this->previousPsi = 0.0;
  this->previousCTE = 0.0;
}

bool Utils::Compare(double a, double b)
{
	//https://stackoverflow.com/a/17341
	std::cout << __FILE__ << ": " << __LINE__ << "\t Comparing: " << a << " vs " << b << std::endl;
    return fabs(a - b) < std::numeric_limits<double>::epsilon();
}

Eigen::VectorXd Utils::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order)
{
	  assert(xvals.size() == yvals.size());
	  assert(order >= 1 && order <= xvals.size() - 1);
	  Eigen::MatrixXd A(xvals.size(), order + 1);

	  for (int i = 0; i < xvals.size(); i++) {
	    A(i, 0) = 1.0;
	  }

	  for (int j = 0; j < xvals.size(); j++) {
	    for (int i = 0; i < order; i++) {
	      A(j, i + 1) = A(j, i) * xvals(j);
	    }
	  }

	  auto Q = A.householderQr();
	  auto result = Q.solve(yvals);
	  return result;
}

// Evaluate a polynomial.
double Utils::polyeval(Eigen::VectorXd coeffs, double x)
{
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

double Utils::velocityInMetersPerSecondFromMilesPerHour(const double v)
{
	return(v*0.44704);
}

void Utils::coordinatesInVehicleReference(std::vector<double>& wayPoints_ptsx, std::vector<double>& wayPoints_ptsy, double& location_px, double& location_py, double& psi)
{
	/*
	 * psi is 0 degrees in the direction of the vehicle, and increases counter-clockwise
	 */

	assert (wayPoints_ptsx.size() == wayPoints_ptsy.size());

	/*double check_x;
	double check_y;*/
	double newX;
	double newY;

	for (size_t i = 0; i< wayPoints_ptsx.size(); i++)
	{
		newX = ((wayPoints_ptsx[i] - location_px) * std::cos(psi)) + ((wayPoints_ptsy[i] - location_py) * std::sin(psi));
		newY = ((location_px - wayPoints_ptsx[i]) * std::sin(psi)) - ((location_py - wayPoints_ptsy[i]) * std::cos(psi));

		/*newX = ((wayPoints_ptsx[i] - location_px) * std::cos(-psi)) - ((wayPoints_ptsy[i] - location_py) * std::sin(-psi));
		newY = ((wayPoints_ptsx[i] - location_px) * std::sin(-psi)) + ((wayPoints_ptsy[i] - location_py) * std::cos(-psi));*/

/*		double x = wayPoints_ptsx[i] - location_px;
		double y = wayPoints_ptsy[i] - location_py;*/

		wayPoints_ptsx[i] = newX;
		wayPoints_ptsy[i] = newY;

/*

		check_x = x * cos(-psi) - y * sin(-psi);
		check_y = x * sin(-psi) + y * cos(-psi);

		assert(this->Compare(check_x, wayPoints_ptsx[i]));
		assert(this->Compare(check_y, wayPoints_ptsy[i]));
*/

		/*wayPoints_ptsx[i] = (wayPoints_ptsx[i] - location_px)*cos(-psi) - (wayPoints_ptsy[i] - location_py)*sin(-psi);
		wayPoints_ptsy[i] = (wayPoints_ptsx[i] - location_px)*sin(-psi) + (wayPoints_ptsy[i] - location_py)*cos(-psi);*/

	}

	/* in vehicle's reference, the vehicle is always at (0,0), and heading 0 degrees*/
	location_px = 0.0;
	location_py = 0.0;
	psi = 0.0;
}


#ifndef SEQUENTIAL_LINE_SEARCH_GAUSSIAN_PROCESS_REGRESSOR_HPP
#define SEQUENTIAL_LINE_SEARCH_GAUSSIAN_PROCESS_REGRESSOR_HPP

#include <Eigen/Core>
#include <sequential-line-search/regressor.hpp>

namespace sequential_line_search
{
    class GaussianProcessRegressor : public Regressor
    {
    public:
        /// \details Hyperparameters will be set via MAP estimation.
        GaussianProcessRegressor(const Eigen::MatrixXd& X,
                                 const Eigen::VectorXd& y,
                                 const KernelType       kernel_type = KernelType::ArdMatern52Kernel);

        /// \details Specified hyperparameters will be used.
        GaussianProcessRegressor(const Eigen::MatrixXd& X,
                                 const Eigen::VectorXd& y,
                                 const Eigen::VectorXd& kernel_hyperparams,
                                 double                 noise_hyperparam,
                                 const KernelType       kernel_type = KernelType::ArdMatern52Kernel);

        double PredictMu(const Eigen::VectorXd& x) const override;
        double PredictSigma(const Eigen::VectorXd& x) const override;

        Eigen::VectorXd PredictMuDerivative(const Eigen::VectorXd& x) const override;
        Eigen::VectorXd PredictSigmaDerivative(const Eigen::VectorXd& x) const override;

        // Can be derived after MAP
        Eigen::MatrixXd m_K_y;
        Eigen::MatrixXd m_K_y_inv;

        // Getter
        const Eigen::MatrixXd& GetLargeX() const override { return m_X; }
        const Eigen::VectorXd& GetSmallY() const override { return m_y; }

        const Eigen::VectorXd& GetKernelHyperparams() const override { return m_kernel_hyperparams; }
        double                 GetNoiseHyperparam() const override { return m_noise_hyperparam; }

    private:
        void PerformMapEstimation();

        /// \brief Data points.
        Eigen::MatrixXd m_X;

        /// \brief Values on data points.
        Eigen::VectorXd m_y;

        /// \brief Kernel hyperparameters
        ///
        /// \details Derived from MAP or specified directly.
        Eigen::VectorXd m_kernel_hyperparams;

        /// \brief A hyperparameter about noise level of ARD.
        ///
        /// \details Derived from MAP or specified directly.
        double m_noise_hyperparam;
    };
} // namespace sequential_line_search

#endif // SEQUENTIAL_LINE_SEARCH_GAUSSIAN_PROCESS_REGRESSOR_HPP


// See LICENSE for license details.

#include <string>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <future>
#include <boost/program_options.hpp>

#include "mmapped_file.hpp"
#include "bin.hpp"
#include "dims_create.hpp"

bool divisible(ssize_t v, int d)
{
    return (v / d) * d == v;
}

using bins_type = Bins<3>;

bins_type bin_all(bins_type::index_type nbins, bins_type::point_type bounding_box, double const*first, double const*last)
{
    if (!divisible(last - first, 3)) {
        throw std::runtime_error("Range not a multiple of 3.");
    }

    auto b = bins_type{nbins, bounding_box};
    for (; first != last; first += 3)
        b.insert({first[0], first[1], first[2]});
    return b;
}

namespace statistics {
template <typename T, typename R = double>
R mean(const std::vector<T>& v)
{
    T sum = std::accumulate(v.begin(), v.end(), T{0}, std::plus<T>{});
    return static_cast<R>(sum) / v.size();
}

template <typename T, typename R = double>
R var(const std::vector<T>& v)
{
    T sqsum = std::accumulate(v.begin(), v.end(), T{0}, [](T acc, T val){ return acc + val * val; });
    R m = mean(v);
    return static_cast<R>(sqsum) / v.size() - m * m;
}
}

// For boost::program_options
namespace streamable {
template <int N>
struct NDoubles {
    typename Bins<N>::point_type data;
};
template <int N>
std::istream& operator>>(std::istream& is, NDoubles<N>& ti)
{
    char c;
    for (int i = 0; i < N; ++i) {
        is >> ti.data[i];
        if (i < N - 1)
            is.read(&c, 1);
    }
    return is;
}
}

int main(int argc, char **argv)
{
    const int nthreads = 4;
    using namespace std::string_literals;
    namespace po = boost::program_options;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("file", po::value<std::string>(), "MPI-IO position file")
        ("box", po::value<streamable::NDoubles<3>>(), "Bounding box of the simulation")
        ("nproc", po::value<int>(), "Number of processes")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") || !vm.count("file") || !vm.count("box") || !vm.count("nproc")) {
        std::cout << desc << std::endl;
        return 1;
    }

    const auto fn = vm["file"].as<std::string>();
    const auto nbins = dims_create(vm["nproc"].as<int>());
    const auto bbox = vm["box"].as<streamable::NDoubles<3>>().data;

    std::cout << "File : " << fn << std::endl;
    std::cout << "NProc: " << vm["nproc"].as<int>() << " = " << nbins[0] << " x " << nbins[1] << " x " << nbins[2] << std::endl;
    std::cout << "Box  : " << bbox[0] << " " << bbox[1] << " " << bbox[2] << "\n" << std::endl;

    auto f = MFile<double>{fn.c_str()};
    std::cout << "File has " << f.size() << " elemets." << std::endl;

    auto data = f.data();
    auto b = bin_all(nbins, bbox, data, data + f.size());

    /* Sanity check */
    int i = std::accumulate(b.bins.begin(), b.bins.end(), 0, std::plus<int>{});
    std::cout << "Binned   " << i << " particles." << std::endl;
    if (3 * i != f.size()) {
        throw std::runtime_error("Particles disappeared...");
    }
    /* End */

    std::cout << std::endl;
    std::cout << "Min: " << *std::min_element(b.bins.begin(), b.bins.end()) << std::endl;
    std::cout << "Max: " << *std::max_element(b.bins.begin(), b.bins.end()) << std::endl;

    auto dmean = statistics::mean(b.bins);
    auto dsdev = std::sqrt(statistics::var(b.bins));

    std::cout << "Mean: " << dmean << std::endl;
    std::cout << "SDev: " << dsdev << " ( = " << std::floor(dsdev / dmean * 1000.)/10. << " %)" << std::endl;
}


#ifndef _DISTORTION_CALIBRATION_HPP_
#define _DISTORTION_CALIBRATION_HPP_

#include <limits>
#include <boost/math/special_functions/binomial.hpp>
#include "matrixOperations.hpp"
#include "parseCSV_CIS_pointCloud.hpp"

void boundingBox(Eigen::MatrixXd& X, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner){
    maxCorner = X.colwise().maxCoeff();
    minCorner = X.colwise().minCoeff();
}

/// Scales every dimension of the point cloud to the max and min values
/// Also finds the original bounding box which is subsequently scaled down.
///
/// @todo what are the expected dimensions of X?
/// @todo which return row is the min and which is the max
/// @todo this does not scale to the unit box, it scales to the size of the max and min point of the matrix.
///
/// @param X nx3 matrix containing points that will be scaled
/// @param maxCorner the maximum coordinate in all dimensions of the bounding box
/// @param ignoreBounds ignore if coordinates are not between 0 and 1. Defaults to false, which means there is an assertion checking the bounds.
void ScaleToUnitBox(Eigen::MatrixXd& X, const Eigen::Vector3d& minCorner, const Eigen::Vector3d& maxCorner, bool ignoreBounds = false )
{
    // bounding box max and min
    Eigen::Vector3d diff = maxCorner-minCorner;
    /// @todo come up with better way to handle when diff is 0
    if(diff(0)==0) diff(0) = 1;
    if(diff(1)==0) diff(1) = 1;
    if(diff(2)==0) diff(2) = 1;

    for (int i=0; i<X.cols(); i++){
        for (int j=0; j<X.rows(); j++){
            // scale the x,y,z of the point
            auto coord = (diff(i)==0.0) ? 0.0 : (X(j,i)-minCorner(i))/diff(i);
            if(!ignoreBounds){
                BOOST_VERIFY(coord <= 1); // verify scaling is working
                BOOST_VERIFY(coord >= 0);
            }
            X(j,i) = coord;
        }
    }
}

template <class T>
T boost::math::binomial_coefficient(unsigned n, unsigned k);

double BersteinPolynomial(double v, int N, int k)
{
    BOOST_VERIFY(N>=k);
    double B = boost::math::binomial_coefficient<double>(N,k)*pow(1-v,N-k)*pow(v,k);
    return B;
}

/// Makes F matrix of Berstein Polynomials
/// @param N the polynomial degree
/// @see slide 42 and 43 of InterpolationReview.pdf
/// @todo advanced implementation: template on the polynomial size
Eigen::MatrixXd FMatrixRow(const Eigen::Vector3d& v,int N = 5, bool debug = false)
{
    int index = 0; // position in the output matrix
    int columns = pow(N+1,3);
    Eigen::MatrixXd F(1,columns);
    //std::cout << "\n\nF is " << F << std::endl;
    for (int i=0; i<=N; i++){
        for(int j=0; j<=N; j++){
            for(int k=0; k<=N; k++){
                BOOST_VERIFY(index<columns);
                // Fijk = Bi * Bj * Bk F(0,index)
                //std::cout << "\n\nindex is " << index << std::endl;
                double bSum = BersteinPolynomial(v(0),N,i)*BersteinPolynomial(v(1),N,j)*BersteinPolynomial(v(2),N,k);
                //std::cout << "\n\nB is " << B << std::endl;
                F.block<1,1>(0,index) << bSum;
                index++;
                //std::cout << "\n\nF is " << F << std::endl;
            }
        }
    }

    if(debug){
        std::cout << "\n\ncolumns is " << columns << std::endl;
        std::cout << "\n\nFMatrixRow:\n\n" << F << "\n\n";
    }
    return F;
}

/// Normalize the cEM matrix of points, then find the
/// F Matrix row of each point and insert it into a larger
/// matrix on which SVD will be solved.
///
/// @pre cEM must be normalized to the unit rectangle
///
/// @see slide 42 and 43 of InterpolationReview.pdf
///
///
/// @param cEM numPoints x n (with n=3 normally) matrix containing the c expected value, aka actual points measured by EM tracker in EM coordinate system, after translation from EM coord system
/// @param N the polynomial degree
Eigen::MatrixXd FMatrix(const Eigen::MatrixXd& normalcEM, int N = 5, bool debug = false){
    /// @todo don't recompute pow here and in FMatrixRow
    int columns = pow(N+1,3);
    int rows = normalcEM.rows();
    Eigen::MatrixXd cEMFMatrix(rows,columns);

    for (int i=0; i<rows; i++){
        Eigen::Vector3d vXYZ;
        vXYZ = normalcEM.block<1,3>(i,0);
        Eigen::MatrixXd row = FMatrixRow(vXYZ,N,debug);
        if(debug) std::cout << "\n\nreturned FMatrixRow:\n\n" << row << "\n\n";
        cEMFMatrix.row(i) = row;
    }
    if(debug) std::cout << "\n\ncEMFMatrix:\n\n" << cEMFMatrix << "\n\n";
    return cEMFMatrix;
}

/// Take a vector of matrices and stack it vertically into one large matrix
/// with the first matrix in the vector at the top and the last at the bottom.
///
/// @pre assumes all matrices have the same dimensions
template<typename T>
Eigen::MatrixXd stackRange(const T & vecMat){
    auto begin = std::begin(vecMat);
    auto end = std::end(vecMat);
    auto distance = std::distance(begin,end);
    if(!distance) return Eigen::MatrixXd();

    std::size_t rows = begin->rows();
    std::size_t cols = begin->cols();
    Eigen::MatrixXd stack(rows*distance,cols);

    std::size_t i = 0;
    for(auto mat : vecMat ){
        stack.block(i*rows, 0, rows, cols) = mat;
        ++i;
    }

    return stack;
}

/// Take a vector of Vector3d (or points) and stack the transpose of each vector (aka row vector)
/// vertically into one large matrix with the first Vector3d in the vector at the top and the last
/// at the bottom.
///
/// @note Currently only works with vectors
///
/// @pre assumes all matrices have the same dimensions
template<typename T>
Eigen::MatrixXd stackRangeTranspose(const T & vecMat){
    auto begin = std::begin(vecMat);
    auto end = std::end(vecMat);
    auto distance = std::distance(begin,end);
    if(!distance) return Eigen::MatrixXd();

    std::size_t rows = begin->rows();
    std::size_t cols = begin->cols();
    Eigen::MatrixXd stack(distance,rows);

    std::size_t i = 0;
    for(auto mat : vecMat ){
        stack.block(i, 0, cols, rows) = mat.transpose();
        ++i;
    }

    return stack;
}

/// Takes a set of points and converts it to a matrix of normalized points aka points scaled to the unit box,
/// where they are subsequently used to calculate F values for SVD.
///
/// @see slide 43 of InterpolationReview.pdf
///
/// @param pointInAllFrames an numPoints x 3 matrix cointaining all the points to be normalized and inserted into an F Matrix for solving with SVD
/// @param[out] minCorner the minimum coordinate of the distorted parameter, used for scaling to the unit box
/// @param[out] maxCorner the maximum coordinate of the distorted parameter, used for scaling to the unit box
Eigen::MatrixXd normalizedFMatrix(const Eigen::MatrixXd& pointsInAllFrames, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner)
{
    Eigen::MatrixXd pointsNormalizedToUnitBox(pointsInAllFrames); // aka normal cEM
    boundingBox(pointsNormalizedToUnitBox,minCorner,maxCorner);
    ScaleToUnitBox(pointsNormalizedToUnitBox,minCorner,maxCorner); // normalize into unit box

    Eigen::MatrixXd FMatForSVD = FMatrix(pointsNormalizedToUnitBox);

    return FMatForSVD;
}

///
/// Solving for SVD F*C=P, where F is the EMPointsInEMFrameOnCalObj with BernsteinPolynomials applied.
///
/// @return distortion Calibration Matrix C
/// @see slide 43 of InterpolationReview.pdf
Eigen::MatrixXd distortionCalibrationMatrixC(const Eigen::MatrixXd& EMPointsInEMFrameOnCalObj, const Eigen::MatrixXd& OptPointsInEMFrameOnCalibObject, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner ){

    Eigen::MatrixXd FMatofEMPointsInEMFrameOnCalObj = normalizedFMatrix(EMPointsInEMFrameOnCalObj, minCorner, maxCorner);
    std::cout << "\n\nFMatrix for SVD is rows: "<< FMatofEMPointsInEMFrameOnCalObj.rows() << " cols: " << FMatofEMPointsInEMFrameOnCalObj.cols() << std::endl << std::endl;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(FMatofEMPointsInEMFrameOnCalObj, Eigen::ComputeThinU | Eigen::ComputeThinV);

    /// this is cx cy cz on slide 43 of InterpolationReview.pdf
    Eigen::MatrixXd pointCorrectionMatrix = svd.solve(OptPointsInEMFrameOnCalibObject);
    std::cout << "\n\npointCorrectionMatrix rows: " << pointCorrectionMatrix.rows() << " cols: " << pointCorrectionMatrix.cols() << "\n\n";

    return pointCorrectionMatrix;
}

/// Correct distortions in one point cloud by utilizing distorted and undistorted versions of a second point cloud.
/// Bernstein Polynomials are utilized to perform the correction.
///
/// @param[in] distortedToCorrect the distorted data set to correct
/// @param[in] distortedGroundTruth the same data as groundTruth, but this data has distortion, and the variation between this and the real groundTruth will be used to correct distortedToCorrect.
/// @param[in] groundTruth previously known exact values with no distortion to determine the coefficient matrix to correct the distortion
/// @param[out] minCorner the minimum coordinate of the distorted parameter, used for scaling to the unit box
/// @param[out] maxCorner the maximum coordinate of the distorted parameter, used for scaling to the unit box
///
/// @return Eigen::MatrixXd containing data that should match groundTruth
Eigen::MatrixXd correctDistortion(const Eigen::MatrixXd& distortedToCorrect, const Eigen::MatrixXd& distortedGroundTruth, const Eigen::MatrixXd& groundTruth, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner){

    Eigen::MatrixXd dcmC = distortionCalibrationMatrixC(distortedGroundTruth, groundTruth,minCorner,maxCorner);

    // scale using the same scaling factor as before, ignoring if it doesn't fit in the 0 to 1 bounds
    // this bool only affects a BOOST_VERIFY check, not function program behavior.
    bool ignoreUnitBoxScalingBounds = true;
    Eigen::MatrixXd distortedToCorrectScaled = distortedToCorrect;
    ScaleToUnitBox(distortedToCorrectScaled, minCorner, maxCorner,ignoreUnitBoxScalingBounds);

    Eigen::MatrixXd FMatrixDistorted = FMatrix(distortedToCorrectScaled);
    //               corrected distortion matrix =        F*C
    Eigen::MatrixXd undistorted = FMatrixDistorted*dcmC;

    return undistorted;
}

/// @todo move elsewhere and remove dependency on parsing data structure
template<typename T, typename U>
Eigen::MatrixXd correctDistortionOnSourceData(
                                   const T& calreadingsFrames, // typicaly std::vector<std::vector<Eigen::MatrixXd> >
                                   const std::vector<Eigen::MatrixXd>&         cExpected,
                                   const U& EMPtsInEMFrameOnProbe  // typicaly std::vector<std::vector<Eigen::MatrixXd> >
                                   ){

    static const int firstFrame = 0;
    static const int IndexOptPtsInOptFrameOnEMTracker = 0;
    static const int IndexOptInOptFrameOnCalObj = 1;
    static const int IndexEMPointsInEMFrameOnCalObj = 2;

    BOOST_VERIFY(calreadingsFrames.size()==cExpected.size());
    BOOST_VERIFY(cExpected[0].cols()>0);

    // create stacked version of cExpected
    Eigen::MatrixXd cExpectedStacked = stackRange(cExpected);

    // prep cEM for manual stacking since it is a vector of vectors
    // Stack EM Points in EM frame on to cEM matrix
    static const std::size_t NumEMPointsInEMFrameOnCalObj = calreadingsFrames[firstFrame][IndexEMPointsInEMFrameOnCalObj].rows();
    static const std::size_t NumFrames = calreadingsFrames.size();
    Eigen::MatrixXd cEM;
    cEM.resize(NumEMPointsInEMFrameOnCalObj*NumFrames,3);
    for (std::size_t outputRow = 0, i = 0; i < NumFrames; outputRow+=NumEMPointsInEMFrameOnCalObj, i++){
        const Eigen::MatrixXd& markerTrackersOnCalBodyInEMFrame=calreadingsFrames[i][IndexEMPointsInEMFrameOnCalObj];
        // @todo For some reason putting numMarkers in for 27 does not work
        cEM.block(outputRow,0,NumEMPointsInEMFrameOnCalObj,3) = markerTrackersOnCalBodyInEMFrame;
    }

    Eigen::Vector3d minCorner;
    Eigen::Vector3d maxCorner;

    auto StackedEMPtsInEMFrameOnProbe = stackRange(EMPtsInEMFrameOnProbe);

    Eigen::MatrixXd undistortedEMPointsInEMFrame = correctDistortion(StackedEMPtsInEMFrameOnProbe, cEM, cExpectedStacked, minCorner, maxCorner);

    return undistortedEMPointsInEMFrame;
}

#endif // _DISTORTION_CALIBRATION_HPP_


// BLAS level 2
// TNT arrays

#include <iostream>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/traits/tnt.hpp>
#include "utils.h"
#include "tnt_utils.h"

namespace atlas = boost::numeric::bindings::atlas;

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

#ifndef F_FORTRAN
typedef TNT::Array1D<double> vct_t;
typedef TNT::Array2D<double> matr_t;
#else
typedef TNT::Fortran_Array1D<double> vct_t;
typedef TNT::Fortran_Array2D<double> matr_t;
#endif

int main() {

  cout << endl;

  vct_t vx (2);
  atlas::set (1., vx);
  print_v (vx, "vx");
  vct_t vy (3);
  atlas::set (0., vy);
  print_v (vy, "vy");
  cout << endl;

  matr_t m (3, 2);
  init_m (m, kpp (1));
  print_m (m, "m");
  cout << endl;

  atlas::gemv (CblasNoTrans, 1.0, m, vx, 0.0, vy);
  print_v (vy, "m vx");

  atlas::gemv (m, vx, vy);
  print_v (vy, "m vx");
  cout << endl;

  atlas::set (0, vx);
  atlas::set (1, vy);
  atlas::gemv (CblasTrans, 1.0, m, vy, 0.0, vx);
  print_v (vx, "m^T vy");
  cout << endl;

  atlas::set (1, vy);
  atlas::gemv (CblasNoTrans, 1.0, m, vx, 1.0, vy);
  print_v (vy, "vy + m vx");
  cout << endl;

  atlas::set (1, vy);
  atlas::gemv (CblasNoTrans, 2.0, m, vx, 0.5, vy);
  print_v (vy, "0.5 vy + 2.0 m vx");
  cout << endl;

}


#pragma once

#include <numeric>

#include "ArithmeticProgression.hpp"
#include "Misc.hpp"
#include "Partitions.hpp"
#include "Sequences.hpp"
#include "VectorHelpers.hpp"
#include <boost/iterator/iterator_facade.hpp>

namespace discreture
{

////////////////////////////////////////////////////////////
/// \brief class of set_partitions of the number n.
/// \param IntType should be an integral type with enough space to store n and
/// k. It can be signed or unsigned. # Example:
///
///	 set_partitions X(3);
///		for (auto&& x : X)
///			cout << x << endl;
///
/// Prints out all set partitions of {0,1,2}:
///
/// 	[ [ 0 ] [ 1 ] [ 2 ] ]
///		[ [ 0 1 ] [ 2 ] ]
///		[ [ 0 2 ] [ 1 ] ]
///		[ [ 1 2 ] [ 0 ] ]
///		[ [ 0 1 2 ] ]
///
///
///	# Example 2:
///	One can specify the number of parts:
///
///		set_partitions X(4,2);
///		for (auto&& x : X)
///			cout << x << endl;
///
/// Prints out all set partitions of {0,1,2,3,4} with exactly 2 parts:
///
///		[ [ 0 1 2 ] [ 3 ] ]
///		[ [ 0 1 3 ] [ 2 ] ]
///		[ [ 0 2 3 ] [ 1 ] ]
///		[ [ 1 2 3 ] [ 0 ] ]
///		[ [ 0 1 ] [ 2 3 ] ]
///		[ [ 0 2 ] [ 1 3 ] ]
///		[ [ 0 3 ] [ 1 2 ] ]
///
///
////////////////////////////////////////////////////////////
template <class IntType = int>
class SetPartitions
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using number_partition = std::vector<IntType>;
    using value_type = std::vector<number_partition>;
    using set_partition = value_type;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    class iterator;
    using const_iterator = iterator;

    // **************** Begin static functions

    static bool next_set_partition(set_partition& data,
                                   const number_partition& part)
    {
        auto n = std::accumulate(part.begin(), part.end(), 0L);
        return next_set_partition(data, part, n);
    }

    static bool next_set_partition(set_partition& data,
                                   const number_partition& part,
                                   difference_type n)
    {
        difference_type anteriorpos = pop(data, n - 1);
        difference_type curr = n - 2;
        difference_type currpos = 0;

        while (true)
        {
            currpos = pop(data, curr);

            if (shouldBreak(data, part, currpos, anteriorpos))
                break;

            anteriorpos = currpos;
            --curr;

            if (curr == -1)
                break;
        }

        if (curr == -1)
            return false;

        auto newpos = NextAcceptablePlaceToAdd(data, part, currpos);

        data[newpos].push_back(curr);

        for (difference_type i = curr + 1; i < n; ++i)
        {
            data[NextAcceptablePlaceToAdd(data, part)].push_back(i);
        }

        return true;
    }

    static void fill_first_set_partition(set_partition& data,
                                         const number_partition& part)
    {
        IntType numpart = 0;
        IntType etiqueta = 0;
        data.resize(part.size());

        for (auto x : part)
        {
            data[numpart].resize(x);

            for (IntType i = 0; i < x; ++i, ++etiqueta)
            {
                data[numpart][i] = etiqueta;
                // 					cout << etiqueta << endl;
            }

            ++numpart;
        }
    }

    // **************** End static functions

public:
    explicit SetPartitions(IntType n)
        : n_(n), min_num_parts_(1), max_num_parts_(n), size_(calc_size(n, 1, n))
    {}

    SetPartitions(IntType n, IntType numparts)
        : n_(n)
        , min_num_parts_(numparts)
        , max_num_parts_(numparts)
        , size_(calc_size(n, numparts, numparts))
    {}

    SetPartitions(IntType n, IntType minnumparts, IntType maxnumparts)
        : n_(n)
        , min_num_parts_(minnumparts)
        , max_num_parts_(maxnumparts)
        , size_(calc_size(n, minnumparts, maxnumparts))
    {}

    size_type size() const { return size_; }

    IntType get_n() const { return n_; }

    iterator begin() const { return iterator(n_, max_num_parts_); }

    const iterator end() const
    {
        return iterator::make_invalid_with_id(size());
    }

    class iterator
        : public boost::iterator_facade<iterator, const set_partition&, boost::forward_traversal_tag>
    {
    public:
        iterator() : ID_(0), data_(), n_(0) {}

        explicit iterator(IntType n, IntType numparts)
            : ID_(0), data_(n), n_(n), num_partition()
        {
            Partitions<IntType>::first_with_given_number_of_parts(num_partition,
                                                                  n,
                                                                  numparts);
            fill_first_set_partition(data_, num_partition);
        }

        inline size_type ID() const { return ID_; }

        static const iterator make_invalid_with_id(size_type id)
        {
            iterator it;
            it.ID_ = id;
            return it;
        }

    private:
        void increment()
        {
            ++ID_;

            if (!next_set_partition(data_, num_partition))
            {
                Partitions<IntType>::next_partition(num_partition, n_);
                fill_first_set_partition(data_, num_partition);
            }
        }

        const set_partition& dereference() const { return data_; }

        bool equal(const iterator& it) const { return it.ID() == ID(); }

    private:
        size_type ID_{0};
        set_partition data_{};
        IntType n_{0};
        number_partition num_partition{};

        friend class boost::iterator_core_access;
    }; // end class iterator

private:
    IntType n_;
    IntType min_num_parts_;
    IntType max_num_parts_;
    size_type size_;

private:
    // Private static functions
    static size_type calc_size(IntType n, IntType minnumparts, IntType maxnumparts)
    {
        size_type toReturn = 0;

        for (IntType k = minnumparts; k <= maxnumparts; ++k)
            toReturn += stirling_partition_number(n, k);

        return toReturn;
    }

    static difference_type pop(set_partition& data, IntType num)
    {
        const difference_type n = data.size();
        for (difference_type i = 0; i < n; ++i)
        {
            if (!data[i].empty() && data[i].back() == num)
            {
                data[i].pop_back();
                return i;
            }
        }

        // 			cout << "not found, returning -1" << endl;
        return -1;
    }

    static difference_type NextAcceptablePlaceToAdd(const set_partition& data,
                                                    const number_partition& part,
                                                    difference_type oldpos = -1)
    {
        // 			cout << "Finding if I can put the next number where " <<
        // endl;
        const difference_type n = data.size();
        for (difference_type i = oldpos + 1; i < n; ++i)
        {
            const difference_type dataisize = data[i].size();
            if (dataisize == part[i])
                continue;

            if ((i > 0) && (part[i - 1] == part[i]) && data[i - 1].empty())
                continue;

            return i;
        }

        return -1;
    }
    static bool shouldBreak(const set_partition& data,
                            const number_partition& part,
                            difference_type currpos,
                            difference_type anteriorpos)
    {
        if (currpos == -1)
            return true;

        if (currpos < anteriorpos)
        {
            if (part[currpos] != part[anteriorpos])
                return true;

            if (!data[currpos].empty())
                return true;
        }

        return false;
    }

}; // end class SetPartitions

using set_partitions = SetPartitions<int>;

} // namespace discreture


/**
 *
 * Copyright (c) 2010 Matthias Walter (xammy@xammy.homelinux.net)
 *
 * Authors: Matthias Walter
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bipartite.hpp>

using namespace boost;

/// Example to test for bipartiteness and print the certificates.

template <typename Graph>
void print_bipartite (const Graph& g)
{
  typedef graph_traits <Graph> traits;
  typename traits::vertex_iterator vertex_iter, vertex_end;

  /// Most simple interface just tests for bipartiteness.

  bool bipartite = is_bipartite (g);

  if (bipartite)
  {
    typedef std::vector <default_color_type> partition_t;
    typedef typename property_map <Graph, vertex_index_t>::type index_map_t;
    typedef iterator_property_map <partition_t::iterator, index_map_t> partition_map_t;

    partition_t partition (num_vertices (g));
    partition_map_t partition_map (partition.begin (), get (vertex_index, g));

    /// A second interface yields a bipartition in a color map, if the graph is bipartite.

    is_bipartite (g, get (vertex_index, g), partition_map);

    for (boost::tie (vertex_iter, vertex_end) = vertices (g); vertex_iter != vertex_end; ++vertex_iter)
    {
      std::cout << "Vertex " << *vertex_iter << " has color " << (get (partition_map, *vertex_iter) == color_traits <
          default_color_type>::white () ? "white" : "black") << std::endl;
    }
  }
  else
  {
    typedef std::vector <typename traits::vertex_descriptor> vertex_vector_t;
    vertex_vector_t odd_cycle;

    /// A third interface yields an odd-cycle if the graph is not bipartite.

    find_odd_cycle (g, get (vertex_index, g), std::back_inserter (odd_cycle));

    std::cout << "Odd cycle consists of the vertices:";
    for (size_t i = 0; i < odd_cycle.size (); ++i)
    {
      std::cout << " " << odd_cycle[i];
    }
    std::cout << std::endl;
  }
}

int main (int argc, char **argv)
{
  typedef adjacency_list <vecS, vecS, undirectedS> vector_graph_t;
  typedef std::pair <int, int> E;

  /**
   * Create the graph drawn below.
   *
   *       0 - 1 - 2
   *       |       |
   *   3 - 4 - 5 - 6
   *  /      \   /
   *  |        7
   *  |        |
   *  8 - 9 - 10
   **/

  E bipartite_edges[] = { E (0, 1), E (0, 4), E (1, 2), E (2, 6), E (3, 4), E (3, 8), E (4, 5), E (4, 7), E (5, 6), E (
      6, 7), E (7, 10), E (8, 9), E (9, 10) };
  vector_graph_t bipartite_vector_graph (&bipartite_edges[0],
      &bipartite_edges[0] + sizeof(bipartite_edges) / sizeof(E), 11);

  /**
   * Create the graph drawn below.
   *
   *       2 - 1 - 0
   *       |       |
   *   3 - 6 - 5 - 4
   *  /      \   /
   *  |        7
   *  |       /
   *  8 ---- 9
   *
   **/

  E non_bipartite_edges[] = { E (0, 1), E (0, 4), E (1, 2), E (2, 6), E (3, 6), E (3, 8), E (4, 5), E (4, 7), E (5, 6),
      E (6, 7), E (7, 9), E (8, 9) };
  vector_graph_t non_bipartite_vector_graph (&non_bipartite_edges[0], &non_bipartite_edges[0]
      + sizeof(non_bipartite_edges) / sizeof(E), 10);

  /// Call test routine for a bipartite and a non-bipartite graph.

  print_bipartite (bipartite_vector_graph);

  print_bipartite (non_bipartite_vector_graph);

  return 0;
}


//
// Created by Bartłomiej Boczek on 30/10/2019.
//
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE vector

#include <boost/test/unit_test.hpp>
#include "heap.h"

template<typename T, typename cmp_class>
void heap<T, cmp_class>::swap(int a, int b) {
    T tmp = data[a];
    data[a] = data[b];
    data[b] = tmp;
}

template<typename T, typename cmp_class>
void heap<T, cmp_class>::heap_up(int idx) {
    int parent = idx / 2;
    if (idx == 1) return;
    if (cmp_class::is_first_better(data[idx], data[parent])) {
        swap(idx, parent);
        heap_up(parent);
    }
}

template<typename T, typename cmp_class>
void heap<T, cmp_class>::heap_down(int idx) {
    int best = idx;
    int left = idx * 2;
    int right = left + 1;
    if (left <= data.size()-1 && cmp_class::is_first_better(data[left], data[best]))
        best = left;
    if (right <= data.size()-1 && cmp_class::is_first_better(data[right], data[best]))
        best = right;
    if (best == idx) return;
    else {
        swap(best, idx);
        heap_down(best);
    }
}

template<typename T, typename cmp_class>
heap<T, cmp_class>::heap() {
    data.push_back(T());
}

template<typename T, typename cmp_class>
void heap<T, cmp_class>::push(const T &elem) {
    data.push_back(elem);
    heap_up(data.size() - 1);
}

template<typename T, typename cmp_class>
void heap<T, cmp_class>::pop() {
    data[1] = data.back();
    data.pop_back();
    heap_down(1);
}

template<typename T, typename cmp_class>
int heap<T, cmp_class>::size() {
    return data.size() - 1;
}

template<typename T, typename cmp_class>
bool heap<T, cmp_class>::empty() {
    return 0 == size();
}

template<typename T, typename cmp_class>
T &heap<T, cmp_class>::top() {
    return data[1];
}

struct cmp_int {
    static bool is_first_better(int a, int b) { return a < b; }
};

BOOST_AUTO_TEST_CASE(test_heap_size) {
    heap<int, cmp_int> pq;
    pq.push(2);
    pq.push(5);
    pq.push(7);
    pq.push(1);
    pq.push(0);
    BOOST_CHECK(pq.size() == 5);
}

BOOST_AUTO_TEST_CASE(test_heap_empty) {
    heap<int, cmp_int> pq;
    BOOST_CHECK(pq.empty());
    pq.push(2);
    BOOST_CHECK(!pq.empty());
}

BOOST_AUTO_TEST_CASE(test_heap_sort) {
    heap<int, cmp_int> pq;
    int ordered[] = {0,1,2,5,7};
    pq.push(2);
    pq.push(5);
    pq.push(7);
    pq.push(1);
    pq.push(0);
    for (int i=0; i<5; i++){
        BOOST_CHECK(ordered[i] == pq.top());
        pq.pop();
    }
}


/*******
edit_distance: STL and Boost compatible edit distance functions for C++

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

Distributed under the Boost Software License, Version 1.0.
See accompanying file LICENSE or copy at
http://www.boost.org/LICENSE_1_0.txt
*******/

#include <iostream>

// get the edit_distance() function
#include <boost/algorithm/sequence/edit_distance.hpp>
using boost::algorithm::sequence::edit_distance;
using namespace boost::algorithm::sequence::parameter;

// define a custom cost function where insertion or deletion of space costs nothing
struct cost_free_space {
    // edit_distance uses cost_type to store cost values internally
    // you can define it smaller to save space, or define it as
    // floating point to support non-integer costs, etc.

    // cost_type is inferred from the return values of cost functions if not defined explicitly
    typedef unsigned cost_type;

    // inserting or deleting a space is free:
    unsigned insertion(char c) const { return (c == ' ') ? 0 : 1; }
    unsigned deletion(char c) const { return (c == ' ') ? 0 : 1; }

    // replacing one char with another costs 1
    // (equal elements always incur zero cost)
    unsigned substitution(char c, char d) const { return 1; }

    // Defining substitution() is optional if substitution is compile-time disabled (the default).
    // To enable, pass the optional _substitution=boost::true_type(), or _substitution=<bool-value>
};

int main(int argc, char** argv) {
    char const* str1 = " so   many spaces     ";
    char const* str2 = "    so many   spaces ";

    // with custom "free space" cost function, the distance should be zero:
    // here we also enable substitution
    unsigned dist = edit_distance(str1, str2, _cost = cost_free_space(), _substitution = true);
    std::cout << "The edit distance between \"" << str1 << "\" and \"" << str2 << "\" = " << dist << "\n";

    return 0;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk_simplex.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk_simplex);

BOOST_AUTO_TEST_CASE(simplex_testing)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;

  typedef OpenTissue::gjk::Simplex<vector3_type>  simplex_type;

  vector3_type const not_in_simplex = vector3_type(1.5, 5.0, 1.0);

  simplex_type S;

  // Vertify that simplex have been initialized correctly as being empty and ``zeroed''
  BOOST_CHECK( S.m_bitmask == 0u );
  for(size_t i=0u; i<4; ++i)
  {
    BOOST_CHECK( S.m_w[0] == 0.0 );
    for(size_t j=0u; j<3; ++j)
    {
      BOOST_CHECK( S.m_v[i](j) == 0.0 );
      BOOST_CHECK( S.m_a[i](j) == 0.0 );
      BOOST_CHECK( S.m_b[i](j) == 0.0 );
    }
  }

  // Vertify how different query methods behave on an empty Simplex

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );

  BOOST_CHECK( !OpenTissue::gjk::is_full_simplex(S) );

  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 0u );

  int bit_A    = 0;
  size_t idx_A = 0;
  BOOST_CHECK_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ), std::invalid_argument );

  int bit_B    = 0;
  size_t idx_B = 0;
  BOOST_CHECK_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ), std::invalid_argument );

  int bit_C    = 0;
  size_t idx_C = 0;
  BOOST_CHECK_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::invalid_argument );

  // Next try to insert one simplex vertex into the simplex

  vector3_type const p1 = vector3_type(1.0, 0.0, 0.0);
  vector3_type const a1 = vector3_type(1.0, 1.0, 0.0);
  vector3_type const b1 = vector3_type(1.0, 0.0, 1.0);

  BOOST_CHECK_NO_THROW( OpenTissue::gjk::add_point_to_simplex( p1, a1, b1, S ) );

  // Verify how differnt query method works on a 1-simplex
  BOOST_CHECK( !OpenTissue::gjk::is_full_simplex(S) );
  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 1u );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ) );
  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  BOOST_CHECK_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ), std::logic_error );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  bit_C  = 0xFFFF;
  idx_C  = 0xFFFF;
  BOOST_CHECK_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::logic_error );

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p1, S )             );

  // Next try to insert one more simplex vertex into the simplex

  vector3_type const p2 = vector3_type(2.0, 0.5, 1.0);
  vector3_type const a2 = vector3_type(2.0, 1.0, 7.0);
  vector3_type const b2 = vector3_type(2.0, 0.5, 1.0);

  BOOST_CHECK_NO_THROW( OpenTissue::gjk::add_point_to_simplex( p2, a2, b2, S ) );

  BOOST_CHECK( !OpenTissue::gjk::is_full_simplex(S) );
  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 2u );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ) );
  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 2 );
  BOOST_CHECK( idx_B == 1 );
  BOOST_CHECK( S.m_v[idx_B] == p2 );
  BOOST_CHECK( S.m_a[idx_B] == a2 );
  BOOST_CHECK( S.m_b[idx_B] == b2 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  bit_C  = 0xFFFF;
  idx_C  = 0xFFFF;
  BOOST_CHECK_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::logic_error );

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p1, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p2, S )             );

  // Insert one more simplex vertex

  vector3_type const p3 = vector3_type(2.3, 7.5, 1.2);
  vector3_type const a3 = vector3_type(2.1, 1.1, 2.3);
  vector3_type const b3 = vector3_type(2.2, 2.5, 0.1);

  BOOST_CHECK_NO_THROW( OpenTissue::gjk::add_point_to_simplex( p3, a3, b3, S ) );

  BOOST_CHECK( !OpenTissue::gjk::is_full_simplex(S) );
  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 3u );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ) );
  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 2 );
  BOOST_CHECK( idx_B == 1 );
  BOOST_CHECK( S.m_v[idx_B] == p2 );
  BOOST_CHECK( S.m_a[idx_B] == a2 );
  BOOST_CHECK( S.m_b[idx_B] == b2 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  bit_C  = 0xFFFF;
  idx_C  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 2 );
  BOOST_CHECK( idx_B == 1 );
  BOOST_CHECK( S.m_v[idx_B] == p2 );
  BOOST_CHECK( S.m_a[idx_B] == a2 );
  BOOST_CHECK( S.m_b[idx_B] == b2 );

  BOOST_CHECK( bit_C == 4 );
  BOOST_CHECK( idx_C == 2 );
  BOOST_CHECK( S.m_v[idx_C] == p3 );
  BOOST_CHECK( S.m_a[idx_C] == a3 );
  BOOST_CHECK( S.m_b[idx_C] == b3 );

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p1, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p2, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p3, S )             );

  // Insert one more vertex then we have a full simplex

  vector3_type const p4 = vector3_type(1.3, 1.5, 1.2);
  vector3_type const a4 = vector3_type(1.1, 1.1, 1.3);
  vector3_type const b4 = vector3_type(1.2, 1.5, 1.1);

  BOOST_CHECK_NO_THROW( OpenTissue::gjk::add_point_to_simplex( p4, a4, b4, S ) );

  BOOST_CHECK( OpenTissue::gjk::is_full_simplex(S) );
  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 4u );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ) );
  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 2 );
  BOOST_CHECK( idx_B == 1 );
  BOOST_CHECK( S.m_v[idx_B] == p2 );
  BOOST_CHECK( S.m_a[idx_B] == a2 );
  BOOST_CHECK( S.m_b[idx_B] == b2 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  bit_C  = 0xFFFF;
  idx_C  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 2 );
  BOOST_CHECK( idx_B == 1 );
  BOOST_CHECK( S.m_v[idx_B] == p2 );
  BOOST_CHECK( S.m_a[idx_B] == a2 );
  BOOST_CHECK( S.m_b[idx_B] == b2 );

  BOOST_CHECK( bit_C == 4 );
  BOOST_CHECK( idx_C == 2 );
  BOOST_CHECK( S.m_v[idx_C] == p3 );
  BOOST_CHECK( S.m_a[idx_C] == a3 );
  BOOST_CHECK( S.m_b[idx_C] == b3 );

  BOOST_CHECK( S.m_v[3] == p4 );
  BOOST_CHECK( S.m_a[3] == a4 );
  BOOST_CHECK( S.m_b[3] == b4 );

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p1, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p2, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p3, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p4, S )             );

  // Test what happens if we try to insert five vertices into the simplex

  vector3_type const p5 = vector3_type(2.3, 2.5, 2.2);
  vector3_type const a5 = vector3_type(2.1, 2.1, 2.3);
  vector3_type const b5 = vector3_type(2.2, 2.5, 2.1);

  BOOST_CHECK_THROW( OpenTissue::gjk::add_point_to_simplex( p5, a5, b5, S ), std::logic_error );

  // Now let us erase one of the simplex vertices

  S.m_bitmask = S.m_bitmask & ~bit_B;

  BOOST_CHECK( !OpenTissue::gjk::is_full_simplex(S) );
  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 3u );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ) );
  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 4 );
  BOOST_CHECK( idx_B == 2 );
  BOOST_CHECK( S.m_v[idx_B] == p3 );
  BOOST_CHECK( S.m_a[idx_B] == a3 );
  BOOST_CHECK( S.m_b[idx_B] == b3 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  bit_C  = 0xFFFF;
  idx_C  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 4 );
  BOOST_CHECK( idx_B == 2 );
  BOOST_CHECK( S.m_v[idx_B] == p3 );
  BOOST_CHECK( S.m_a[idx_B] == a3 );
  BOOST_CHECK( S.m_b[idx_B] == b3 );

  BOOST_CHECK( bit_C == 8 );
  BOOST_CHECK( idx_C == 3 );
  BOOST_CHECK( S.m_v[idx_C] == p4 );
  BOOST_CHECK( S.m_a[idx_C] == a4 );
  BOOST_CHECK( S.m_b[idx_C] == b4 );

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p1, S )             );
  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( p2, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p3, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p4, S )             );

  // Let us erase one more simplex

  S.m_bitmask = S.m_bitmask & ~bit_B;

  BOOST_CHECK( !OpenTissue::gjk::is_full_simplex(S) );
  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 2u );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ) );
  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 8 );
  BOOST_CHECK( idx_B == 3 );
  BOOST_CHECK( S.m_v[idx_B] == p4 );
  BOOST_CHECK( S.m_a[idx_B] == a4 );
  BOOST_CHECK( S.m_b[idx_B] == b4 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  bit_C  = 0xFFFF;
  idx_C  = 0xFFFF;
  BOOST_CHECK_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ), std::logic_error );

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p1, S )             );
  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( p2, S )             );
  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( p3, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p4, S )             );

  // Insert a new simplex vertex

  BOOST_CHECK_NO_THROW( OpenTissue::gjk::add_point_to_simplex( p5, a5, b5, S ) );

  BOOST_CHECK( !OpenTissue::gjk::is_full_simplex(S) );
  BOOST_CHECK( OpenTissue::gjk::dimension(S) == 3u );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A ) );
  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 2 );
  BOOST_CHECK( idx_B == 1 );
  BOOST_CHECK( S.m_v[idx_B] == p5 );
  BOOST_CHECK( S.m_a[idx_B] == a5 );
  BOOST_CHECK( S.m_b[idx_B] == b5 );

  bit_A  = 0xFFFF;
  idx_A  = 0xFFFF;
  bit_B  = 0xFFFF;
  idx_B  = 0xFFFF;
  bit_C  = 0xFFFF;
  idx_C  = 0xFFFF;
  BOOST_CHECK_NO_THROW( OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B, idx_C, bit_C ) );

  BOOST_CHECK( bit_A == 1 );
  BOOST_CHECK( idx_A == 0 );
  BOOST_CHECK( S.m_v[idx_A] == p1 );
  BOOST_CHECK( S.m_a[idx_A] == a1 );
  BOOST_CHECK( S.m_b[idx_A] == b1 );

  BOOST_CHECK( bit_B == 2 );
  BOOST_CHECK( idx_B == 1 );
  BOOST_CHECK( S.m_v[idx_B] == p5 );
  BOOST_CHECK( S.m_a[idx_B] == a5 );
  BOOST_CHECK( S.m_b[idx_B] == b5 );

  BOOST_CHECK( bit_C == 8 );
  BOOST_CHECK( idx_C == 3 );
  BOOST_CHECK( S.m_v[idx_C] == p4 );
  BOOST_CHECK( S.m_a[idx_C] == a4 );
  BOOST_CHECK( S.m_b[idx_C] == b4 );

  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( not_in_simplex, S ) );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p1, S )             );
  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( p2, S )             );
  BOOST_CHECK( !OpenTissue::gjk::is_point_in_simplex( p3, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p4, S )             );
  BOOST_CHECK(  OpenTissue::gjk::is_point_in_simplex( p5, S )             );

}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <boost/math/common_factor_rt.hpp>
using namespace std;
using namespace boost::math;

int main() {
	int a, b;
	while (cin >> a >> b) {
		int Ngcd = gcd(a, b);
		int Nlcm = lcm(a, b);
		cout << Ngcd << " " << Nlcm << endl;
	}
	return 0;
}


// Copyright 2011-2012 Renato Tegon Forti
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// -----------------------------------------------------------------------------
// This example shows how implement a work quee (thread pool) to work with
// Boost.Application using Boost.Asio.
//
// The result will be printed on CTRL-C (Stop) signal
// -----------------------------------------------------------------------------

#define BOOST_ALL_DYN_LINK
#define BOOST_LIB_DIAGNOSTIC

#include <boost/asio.hpp>
#include <boost/thread.hpp>
#include <boost/application.hpp>
#include <boost/timer/timer.hpp>
#include <boost/bind.hpp>

#include <iostream>
#include <math.h>

#include "work_queue.hpp"

using namespace std;
using namespace boost;

// worker class that calculate gaussian blur
// http://en.wikipedia.org/wiki/Gaussian_blur
template< int kernelRadius = 3>
struct gaussian_blur
{
   typedef boost::function< void (vector< vector<double> >) > callback;

   gaussian_blur(const callback& cb)
      : callback_(cb)
   {
   }

   void operator()()
   {
      boost::timer::cpu_timer timer;

      kernel2d_ = produce_gaussian_kernel(kernelRadius);

      boost::timer::cpu_times const elapsed_times(timer.elapsed());

      std::cout
         << "gaussian_blur takes:"
         <<  format(elapsed_times, 9)
         << ", for size: "
         << kernelRadius
         << std::endl;

      callback_(kernel2d_);
   }

protected:

   double gaussian (double x, double mu, double sigma)
   {
      return exp( -(((x-mu)/(sigma))*((x-mu)/(sigma)))/2.0 );
   }

   vector< vector<double> > produce_gaussian_kernel (int internalKernelRadius)
   {
      // get kernel matrix
      vector< vector<double> > kernel2d ( 2*internalKernelRadius+1, vector<double>(2*internalKernelRadius+1) );

      // determine sigma
      double sigma = internalKernelRadius/2.;

      // fill values
      double sum = 0;
      for (int row = 0; row < kernel2d.size(); row++)
      {
         for (int col = 0; col < kernel2d[row].size(); col++)
         {
            kernel2d[row][col] = gaussian(row, internalKernelRadius, sigma) * gaussian(col, internalKernelRadius, sigma);
            sum += kernel2d[row][col];
         }
      }

      // normalize kernel, or the image becomes dark
      for (int row = 0; row < kernel2d.size(); row++)
         for (int col = 0; col < kernel2d[row].size(); col++)
            kernel2d[row][col] /= sum;

      return kernel2d;
   }

private:

   callback callback_;
   vector< vector<double> > kernel2d_;
};

// application class
class myapp : work_queue<0>
{
public:

   myapp(application::context& context)
      : context_(context)
   {
   }

   void add_result(vector< vector<double> > kernel2d)
   {
      boost::lock_guard<boost::mutex> lock(mutex_);

      task_count_++;

      result_.push_back(kernel2d);

      if(task_count_== 3)
      {
         cout << "all tasks are completed, waiting ctrl-c to display the results..." << endl;
      }
   }

   int operator()()
   {
      // your application logic here!
      task_count_ = 0;

      //our tasks
      add_task(gaussian_blur<3>( boost::bind( &myapp::add_result, this, _1 )));
      add_task(gaussian_blur<6>( boost::bind( &myapp::add_result, this, _1 )));
      add_task(gaussian_blur<9>( boost::bind( &myapp::add_result, this, _1 )));

      context_.find<application::wait_for_termination_request>()->wait();

      return 0;
   }

   bool stop()
   {
      std::cout << "Result..." << std::endl;

      for(int i = 0; i < result_.size(); ++i)
      {
         cout << i << " : -----------------------" << std::endl;

         vector< vector<double> > & kernel2d = result_[i];

         for (int row = 0; row < kernel2d.size(); row++)
         {
            for (int col = 0; col < kernel2d[row].size(); col++)
            {
               cout << setprecision(5) << fixed << kernel2d[row][col] << " ";
            }
            cout << endl;
         }
      }

      return 1;
   }

private:

   boost::mutex mutex_;
   vector< vector< vector<double> > > result_;

   int task_count_;

   application::context& context_;

}; // myapp

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

   application::context app_context;
   myapp app(app_context);

   application::handler<>::callback cb
      = boost::bind(&myapp::stop, &app);

   app_context.insert<application::termination_handler>(
      make_shared<application::termination_handler_default_behaviour>(cb));

   return application::launch<application::common>(app, app_context);
}


#include <benchmark/benchmark.h>
#include <Eigen/Dense>

template <typename T> void BM_GEMM(benchmark::State& state) {
  // Perform setup here
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> A = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Random(state.range(0), state.range(0));
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> B = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Random(state.range(0), state.range(0));
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> C = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Random(state.range(0), state.range(0));
  for (auto _ : state) {
    // This code gets timed
    C = A*B;
    benchmark::DoNotOptimize(C);
  }
}

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

  //Small size benchmarks (8 -> 256)
  benchmark::RegisterBenchmark("GEMM_double", BM_GEMM<double>)
	->RangeMultiplier(2)->Range(8, 256);

  benchmark::Initialize(&argc, argv);
  benchmark::RunSpecifiedBenchmarks();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

// Calculates the side of the intersection-point (if any) of
// of segment a//b w.r.t. segment c
// This is calculated without (re)calculating the IP itself again and fully
// based on integer mathematics; there are no divisions
// It can be used for either integer (rescaled) points, and also for FP
class side_of_intersection
{
public :

    // Calculates the side of the intersection-point (if any) of
    // of segment a//b w.r.t. segment c
    // This is calculated without (re)calculating the IP itself again and fully
    // based on integer mathematics
    template <typename T, typename Segment>
    static inline T side_value(Segment const& a, Segment const& b,
                Segment const& c)
    {
        // The first point of the three segments is reused several times
        T const ax = get<0, 0>(a);
        T const ay = get<0, 1>(a);
        T const bx = get<0, 0>(b);
        T const by = get<0, 1>(b);
        T const cx = get<0, 0>(c);
        T const cy = get<0, 1>(c);

        T const dx_a = get<1, 0>(a) - ax;
        T const dy_a = get<1, 1>(a) - ay;

        T const dx_b = get<1, 0>(b) - bx;
        T const dy_b = get<1, 1>(b) - by;

        T const dx_c = get<1, 0>(c) - cx;
        T const dy_c = get<1, 1>(c) - cy;

        // Cramer's rule: d (see cart_intersect.hpp)
        T const d = geometry::detail::determinant<T>
                    (
                        dx_a, dy_a,
                        dx_b, dy_b
                    );

        T const zero = T();
        if (d == zero)
        {
            // There is no IP of a//b, they are collinear or parallel
            // We don't have to divide but we can already conclude the side-value
            // is meaningless and the resulting determinant will be 0
            return zero;
        }

        // Cramer's rule: da (see cart_intersect.hpp)
        T const da = geometry::detail::determinant<T>
                    (
                        dx_b,    dy_b,
                        ax - bx, ay - by
                    );

        // IP is at (ax + (da/d) * dx_a, ay + (da/d) * dy_a)
        // Side of IP is w.r.t. c is: determinant(dx_c, dy_c, ipx-cx, ipy-cy)
        // We replace ipx by expression above and multiply each term by d
        T const result = geometry::detail::determinant<T>
                    (
                        dx_c * d,                   dy_c * d,
                        d * (ax - cx) + dx_a * da,  d * (ay - cy) + dy_a * da
                    );

        // Note: result / (d * d)
        // is identical to the side_value of side_by_triangle
        // Therefore, the sign is always the same as that result, and the
        // resulting side (left,right,collinear) is the same

        return result;

    }

    template <typename Segment>
    static inline int apply(Segment const& a, Segment const& b, Segment const& c)
    {
        typedef typename geometry::coordinate_type<Segment>::type coordinate_type;
        coordinate_type const s = side_value<coordinate_type>(a, b, c);
        coordinate_type const zero = coordinate_type();
        return math::equals(s, zero) ? 0
            : s > zero ? 1
            : -1;
    }

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP


// This file copyright 2021 by Jérôme Plût <plut.jerome@gmail.org>,
// licensed under the MIT license <https://opensource.org/licenses/MIT>.
#include <Eigen/Dense>
#include <stdlib.h>
#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#include <igl/copyleft/cgal/mesh_boolean.h>
#include <igl/copyleft/cgal/minkowski_sum.h>
#include <igl/copyleft/cgal/piecewise_constant_winding_number.h>
#include <igl/offset_surface.h>
#include <igl/decimate.h>
#include <igl/loop.h>
#include <igl/centroid.h>
#include <igl/swept_volume.h>
// #include <igl/embree/reorient_facets_raycast.h>
#include <igl/copyleft/cgal/intersect_with_half_space.h>
#include <boost/bind/bind.hpp>

using namespace boost::placeholders;
using namespace Eigen;

typedef Matrix<double,Dynamic,3> VertexMatrix;
typedef Matrix<int,Dynamic,3> FaceMatrix;
typedef double vec3d[3];
typedef double mat3d[3][3];

// typedef Matrix<double, Dynamic, 3, RowMajor> vertices_t;
// typedef Matrix<int, Dynamic, 3, RowMajor> faces_t;

// Eigen::IOFormat CommaInitFmt(32, DontAlignCols, ", ", ", ", "", "", " << ", ";");

struct Mesh {/*««*/
	MatrixXd v;
	MatrixXi f;
//   Matrix<double,Dynamic,3> v;
//   Matrix<int,Dynamic,3> f;
  Mesh(): v(), f() { }
  Mesh(int nv, int nf, const double *mv, const int *mf, int d = 3):
			v(nv, 3), f(nf, d) {
    for(int i = 0; i < nv; i++) {
      for(int j = 0; j < 3; j++) {
				v(i, j) = mv[3*i+j];
      }
    }
    for(int i = 0; i < nf; i++) {
      for(int j = 0; j < d; j++) {
	f(i, j) = mf[d*i+j]-1;
      }
    };
  };
};/*»»*/

void to_jl(Mesh m, int *nv, int *nf, double **mv, int **mf) {/*««*/
  // sets mv to zero in case of failure
  *nv = m.v.rows();
  *nf = m.f.rows();
  *mv = (double *) malloc(3*(*nv)*sizeof(double));
  if(*mv == 0) {
    return;
  }
  *mf = (int *) malloc(3* (*nf)*sizeof(int));
  if(*mf == 0) {
    free(*mv);
    *mv = 0;
    return;
  }
  for(int i = 0; i < *nv; i++) {
    for(int j = 0; j < 3; j++) {
      (*mv)[3*i+j] = m.v(i, j);
    }
  }
  for(int i = 0; i < *nf; i++) {
    for(int j = 0; j < 3; j++) {
      (*mf)[3*i+j] = 1 + m.f(i,j);
    }
  }
}/*»»*/

extern "C" {
int mesh_boolean(/*««*/
  int op,
  int nv1, int nf1, const double *mv1, const int *mf1,
  int nv2, int nf2, const double *mv2, const int *mf2,
  int *nv3, int *nf3, double **mv3, int **mf3, int **index) {

  Mesh m1(nv1, nf1, mv1, mf1),
       m2(nv2, nf2, mv2, mf2),
       m3;
  VectorXi j;
  igl::copyleft::cgal::mesh_boolean(m1.v,m1.f,m2.v,m2.f,
  igl::MeshBooleanType(op), m3.v,m3.f, j);

  *index = (int *) malloc(m3.f.rows()*sizeof(int));
  if (*index == 0) {
    return -1;
  }
  for(int i = 0; i < m3.f.rows(); i++) {
    (*index)[i] = j(i)+1;
  }
  to_jl(m3, nv3, nf3, mv3, mf3);
  if(*mv3 == 0) {
    free(*index);
    return -1;
  }
  return 0;
}/*»»*/
int minkowski_sum(/*««*/
  int nv1, int nf1, const double *mv1, const int *mf1,
  int nv2, int nf2, const double *mv2, const int *mf2, int dim2,
  int *nv3, int *nf3, double **mv3, int **mf3, int **index) {

  Mesh m1(nv1, nf1, mv1, mf1),
       m2(nv2, nf2, mv2, mf2, dim2),
       m3;
  MatrixXi j;
// 	std::cout << "mesh m1: "
// 		<< m1.v.rows() << " vertices:\n" << m1.v << "\n"
// 		<< m1.f.rows() << " faces:\n" << m1.f << "\n\n";
// 	std::cout << "mesh m2: "
// 		<< m2.v.rows() << " vertices:\n" << m2.v << "\n"
// 		<< m2.f.rows() << " faces:\n" << m2.f << "\n\n";
  igl::copyleft::cgal::minkowski_sum(m1.v,m1.f,m2.v,m2.f, true,
  	m3.v,m3.f, j);
// 	std::cout << "mesh m3: "
// 		<< m3.v.rows() << " vertices:\n" << m3.v << "\n"
// 		<< m3.f.rows() << " faces:\n" << m3.f << "\n\n";
// 	std::cout << "j has " << j.rows() << " rows:\n"  << j << "\n";

	int n3 = m3.f.rows();
  *index = (int *) malloc(2*n3*sizeof(int));
  if (*index == 0) {
    return -1;
  }
  for(int i = 0; i < n3; i++) {
    (*index)[i] = j(i,0)+1;
		(*index)[i+n3] = j(i,1)+1;
  }
  to_jl(m3, nv3, nf3, mv3, mf3);
  if(*mv3 == 0) {
    free(*index);
    return -1;
  }
  return 0;
}/*»»*/

int mesh_is_pwn(int nv, int nf, const double *mv, const int *mf) {
  Mesh m(nv, nf, mv, mf);
  return (int)igl::copyleft::cgal::piecewise_constant_winding_number(m.v, m.f);
}

int offset_surface(/*««*/
	int nv, int nf, const double *mv, const int *mf,
	double level, int grid,
	int *nvout, int *nfout, double **mvout, int **mfout) {

	Mesh m(nv, nf, mv, mf), mout;
	MatrixXd GV;
	MatrixXi side;
	MatrixXd S;

	igl::offset_surface(m.v, m.f, level, grid,
		igl::SignedDistanceType::SIGNED_DISTANCE_TYPE_DEFAULT,
		mout.v, mout.f, GV, side, S);

	to_jl(mout, nvout, nfout, mvout, mfout);
	if(*mvout == 0) {
		return -1;
	}
	return 0;
}/*»»*/
int decimate(int nv, int nf, const double *mv, const int *mf, int faces,/*««*/
	int *nvout, int *nfout, double **mvout, int **mfout, int **index) {
	Mesh m(nv, nf, mv, mf), mout;
	VectorXi fidx, vidx;
	igl::decimate(m.v, m.f, faces,
		mout.v, mout.f, fidx, vidx);

  *index = (int *) malloc(fidx.rows()*sizeof(int));
  if (*index == 0) {
    return -1;
  }
  for(int i = 0; i < fidx.rows(); i++) {
    (*index)[i] = fidx(i)+1;
  }
	to_jl(mout, nvout, nfout, mvout, mfout);
	if(*mvout == 0) {
		free(*index);
		return -1;
	}
	return 0;
}/*»»*/
int loop(int nv, int nf, const double *mv, const int *mf, int n,/*««*/
	int *nvout, int *nfout, double **mvout, int **mfout) {
	Mesh m(nv, nf, mv, mf), mout;
	VectorXi fidx, vidx;

	igl::loop(m.v, m.f, mout.v, mout.f, n);

	to_jl(mout, nvout, nfout, mvout, mfout);
	return 0;
}/*»»*/
// int reorient_facets_raycast(int nv, int nf, const double *mv, const int *mf,/*««*/
// 	int **mfout, int **flipped) {
// 	Mesh m(nv, nf, mv, mf);
// 	MatrixXd mfo;
// 	VectorXi fl;
//
// 	igl::embree:reorient_facets_raycast(m.v, m.f, mfo, fl);
//   *mfout = (int *) malloc(3*nf*sizeof(int));
//   if(*mfout == 0) {
//     return -1;
//   }
// 	*flipped = (int *) malloc(nf*sizeof(int));
// 	if(*flipped == 0) {
// 		free(mfout);
// 		return -1;
// 	}
//   for(int i = 0; i < nf; i++) {
//     for(int j = 0; j < 3; j++) {
//       (*mfout)[3*i+j] = 1 + m.f(i, j);
//     }
// 		(*flipped)[i] = fl[i];
//   }
// 	return 0;
// }/*»»*/
double centroid_and_volume(int nv, int nf, const double *mv, const int *mf,
	vec3d *c) {
	Mesh m(nv, nf, mv, mf);
	Vector3d cen;
	double vol;

	igl::centroid(m.v, m.f, cen, vol);
	std::cout << "centroid = " << cen << "\n";
	(*c)[0] = cen(0); (*c)[1] = cen(1); (*c)[2] = cen(2);
	return vol;
}

int intersect_with_half_space(int nv, int nf, /*««*/
	const double *mv, const int *mf,
	const vec3d *p, const vec3d *n,
	int *nvout, int *nfout, double **mvout, int **mfout, int **index) {
	Mesh m(nv, nf, mv, mf), mout;
	Vector3d vp (*p);
	Vector3d vn (*n);
	VectorXi fidx;

	igl::copyleft::cgal::intersect_with_half_space(m.v, m.f, vp, vn,
		mout.v, mout.f, fidx);

  *index = (int *) malloc(fidx.rows()*sizeof(int));
  if (*index == 0) {
    return -1;
  }
  for(int i = 0; i < fidx.rows(); i++) {
    (*index)[i] = fidx(i)+1;
  }
	to_jl(mout, nvout, nfout, mvout, mfout);
	if(*mvout == 0) {
		free(*index);
		return -1;
	}
	return 0;
}/*»»*/
int swept_volume(int nv, int nf, const double *mv, const int *mf,
	void (*ctransform)(double, mat3d*, vec3d*),
	size_t steps, size_t gridres, double isolevel,
	int *nvout, int *nfout, double **mvout, int **mfout) {

	Mesh m(nv, nf, mv, mf), mout;
	std::function<Eigen::Affine3d(const double)> f(
		[ctransform] (double t) {
		mat3d a; vec3d b;
		ctransform(t, &a, &b);
		Eigen::Affine3d m;
		m.linear() = Map<Matrix3d>(&a[0][0], 3, 3);
		m.translation() = Map<Vector3d>(&b[0], 3);
		return m;
	});
	igl::swept_volume(m.v, m.f, f, steps, gridres, isolevel, mout.v, mout.f);
	to_jl(mout, nvout, nfout, mvout, mfout);
	if(*mvout == 0) {
		return -1;
	}
	return 0;
}

}

// vim: noet ts=2 sw=2 fmr=««,»»:


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_on_sphere_traits_2.h>
#include <CGAL/Delaunay_triangulation_on_sphere_2.h>
#include <CGAL/Projection_on_sphere_traits_3.h>

#include <CGAL/algorithm.h>
#include <CGAL/convex_hull_3.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/squared_distance_3.h>
#include <CGAL/Timer.h>

#include <boost/iterator/transform_iterator.hpp>

#include <cmath>
#include <fstream>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel    K;
typedef CGAL::Surface_mesh<K>                                  Surface_mesh;

typedef K::Segment_3                                           Segment_3;
typedef CGAL::Delaunay_triangulation_3<K>                      Delaunay;

typedef CGAL::Delaunay_triangulation_on_sphere_traits_2<K>     Gt;
typedef CGAL::Projection_on_sphere_traits_3<K>                 Gt2;
typedef CGAL::Delaunay_triangulation_on_sphere_2<Gt>           DTOS;
typedef CGAL::Delaunay_triangulation_on_sphere_2<Gt2>          DTOS2;
typedef K::Point_3                                             Point;

typedef CGAL::Delaunay_triangulation_3<K, CGAL::Fast_location> Delaunay_fast;
typedef CGAL::Creator_uniform_3<double, Point>                 Creator;

int main(int, char**)
{
  CGAL::Timer time;

  const std::size_t nu_of_pts = 1e7;
  const double radius = 5184.152;

  CGAL::Random random;
  std::cout << "Seed is " << random.get_seed() << std::endl;

  CGAL::Random_points_on_sphere_3<Point, Creator> on_sphere(radius, random);

  std::vector<Point> points;
  points.reserve(nu_of_pts);

  for(std::size_t count=0; count<nu_of_pts; ++count)
    points.push_back(*on_sphere++);
  std::cout << points.size() << " points" << std::endl;

  // Delaunay_traits
  DTOS dtos;
  dtos.set_radius(radius);

  std::cout << " ***STARTING***" << std::endl;
  time.start();
  dtos.insert(points.begin(), points.end());
  time.stop();
  assert(dtos.number_of_vertices() == nu_of_pts);
  std::cout << "Triangulation sphere: "
            << dtos.number_of_vertices() << " vertices in " << time.time() << " sec" << std::endl;

  //Triangulation with points on the sphere (projection_traits)
  Gt2 traits(K::Point_3(0, 0, 0), radius);
  DTOS2 dtos2(traits);
  Gt2::Construct_point_on_sphere_2 cst = traits.construct_point_on_sphere_2_object();

  time.reset();
  time.start();
  dtos2.insert(boost::make_transform_iterator(points.begin(), cst),
               boost::make_transform_iterator(points.end(), cst));
  time.stop();
  std::cout << "Triangulation w/ sphere projection traits: "
            << dtos2.number_of_vertices() << " vertices in " << time.time() << " sec" << std::endl;

//  Surface_mesh sm;

//  time.reset();
//  time.start();
//  CGAL::convex_hull_3(points.begin(), points.end(), sm);
//  time.stop();
//  std::cout << "Convex hull 3D: " << time.time() << " " << std::endl;

  time.reset();
  time.start();
  Delaunay T;
  T.insert(Point(0, 0, 0));
  T.insert(points.begin(), points.end());
  time.stop();
  std::cout << "Delaunay 3D with origin: "
            << T.number_of_vertices() << " vertices in " << time.time() << " sec" << std::endl;

  time.reset();
  time.start();
  Delaunay_fast T_fast_on2;
  T_fast_on2.insert(Point(0, 0, 0));
  T_fast_on2.insert(points.begin(), points.end());
  time.stop();
  std::cout << "Delaunay 3D with origin, fast location: " << time.time() << std::endl;

  return EXIT_SUCCESS;
}


#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>

#include "NeuralNetwork.h"

using namespace Eigen;

/*
 * Class AffineLayer
 *
 * @desript A kind of Neural Network Layer.
 */
NeuralNetwork::~NeuralNetwork()
{
  NNLayer *tmp;
  for(NNLayer *layer = top_layer; layer != NULL; )
    {
      tmp = layer->getNext();
      delete layer;
      layer = tmp;
    }
}

void NeuralNetwork::createNewLayer(int innum, int outnum, bool is_last, Matrix<float, Dynamic, Dynamic> *default_w)
{
  NNLayer *new_affine;
  NNLayer *activation;

  new_affine = new AffineLayer(innum, outnum, default_w);

  if (is_last)
    {
      activation = new SoftMaxLayer();
    }
  else
    {
      activation = new ReLULayer();
    }

  new_affine->setNext(activation);

  if( last_layer == NULL )
    {
      top_layer = new_affine;
    }
  else
    {
      last_layer->setNext(new_affine);
    }

  last_layer = activation;
}

void NeuralNetwork::printnet()
{
  for(NNLayer *layer = top_layer; layer != NULL; layer = layer->getNext())
    {
      layer->printnet();
    };
}

void NeuralNetwork::print_layers()
{
  for(NNLayer *layer = top_layer; layer != NULL; layer = layer->getNext())
    {
      std::cout << layer << " --> ";
    };
  std::cout << "NULL" << std::endl;
}

Matrix<float, Dynamic, Dynamic> NeuralNetwork::forward(Matrix<float, Dynamic, Dynamic> input)
{
  output = top_layer->forward(input);
  return output;
}

float NeuralNetwork::backward(float train_ratio, Matrix<float, Dynamic, Dynamic> m)
{
  float ret = ((LastActivation *)last_layer)->loss(m);
  last_layer->backward(train_ratio, m);
  return ret;
}


#include <boost/test/unit_test.hpp>
#include <ayla/geometry/vector.hpp>

#include <glm/gtc/epsilon.hpp>

BOOST_AUTO_TEST_SUITE( ayla )
BOOST_AUTO_TEST_SUITE( _float )

BOOST_AUTO_TEST_CASE( less_than_zero ) {
	const Float a(0);
	BOOST_CHECK(!(a < 0.0f));
	BOOST_CHECK(a <= 0.0f);
}

BOOST_AUTO_TEST_CASE(_isZero) {
	const Float a(0.0f);

	BOOST_CHECK(isZero(a));

	BOOST_CHECK(isZero(std::numeric_limits<Float>::epsilon()));
	BOOST_CHECK(!isZero(2 * std::numeric_limits<Float>::epsilon()));
}

BOOST_AUTO_TEST_CASE(_epsilonEqual) {
	const Float epsilon = std::numeric_limits<Float>::epsilon();

	const Float a = 1.0f;
	const Float b = a + epsilon;

	BOOST_CHECK(glm::epsilonEqual(a, b, epsilon * 2.0f));
	BOOST_CHECK(glm::epsilonNotEqual(a, b, epsilon));

	const Float c = b + epsilon;

	BOOST_CHECK(glm::epsilonNotEqual(a, c, epsilon));
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


#ifndef ARRAYHELPERS_H
#define ARRAYHELPERS_H

#include "random_generator.hpp"
#include "types.hpp"

#include <Eigen/Core>
#include <random>

namespace Optima
{

class ArrayHelpers {

public:
  static Eigen::ArrayXd randomArray(const size_t dimension) {
    auto randNum  = []() -> double {
      return RandomGenerator::uniform<double>();
    };
    return Eigen::ArrayXd::NullaryExpr(dimension, 1, randNum);
  }

  static Eigen::ArrayXd uniformFromBounds(const Bounds &bounds) {
    const Eigen::ArrayXd &lower = bounds.first;
    const Eigen::ArrayXd &upper = bounds.second;
    const Eigen::ArrayXd &range = upper - lower;
    const size_t dimension = lower.size();
    return ((randomArray(dimension) + 1) / 2) * range + lower;
  }

};
}

#endif // ARRAYHELPERS_H


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Mageswaran.D <mageswaran1989@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>
#include <string>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/interop/opencv/core.hpp>
#include <boost/compute/interop/opencv/highgui.hpp>
#include <boost/compute/utility/source.hpp>

#include <boost/program_options.hpp>

namespace compute = boost::compute;
namespace po = boost::program_options;

// Create convolution program
const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE (
    __kernel void convolution(__read_only  image2d_t  sourceImage,
                              __write_only image2d_t  outputImage,
                              __constant float* filter,
                              int filterWidth)
    {
        const sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE |
                                  CLK_ADDRESS_CLAMP_TO_EDGE   |
                                  CLK_FILTER_NEAREST;

        // Store each work-item’s unique row and column
        int x   = get_global_id(0);
        int y   = get_global_id(1);

        // Half the width of the filter is needed for indexing
        // memory later
        int halfWidth = (int)(filterWidth/2);

        // All accesses to images return data as four-element vector
        // (i.e., float4).
        float4 sum = {0.0f, 0.0f, 0.0f, 0.0f};

        // Iterator for the filter
        int filterIdx = 0;

        // Each work-item iterates around its local area based on the
        // size of the filter
        int2 coords;  // Coordinates for accessing the image

        // Iterate the filter rows
        for(int i = -halfWidth; i <= halfWidth; i++)
        {
            coords.y = y + i;

            // Iterate over the filter columns
            for(int j = -halfWidth; j <= halfWidth; j++)
            {
                coords.x = x + j;

                float4 pixel;

                // Read a pixel from the image.
                // Work on a channel
                pixel = read_imagef(sourceImage, sampler, coords);
                sum.x += pixel.x * filter[filterIdx++];
                //sum.y += pixel.y * filter[filterIdx++];
                //sum.z += pixel.z * filter[filterIdx++];
            }
        }

         barrier(CLK_GLOBAL_MEM_FENCE);
        // Copy the data to the output image if the
        // work-item is in bounds
        if(y < get_image_height(sourceImage) &&
                x < get_image_width(sourceImage))
        {
            coords.x = x;
            coords.y = y;

            //Same channel is copied in all three channels
            //write_imagef(outputImage, coords,
                        // (float4)(sum.x,sum.x,sum.x,1.0f));

            write_imagef(outputImage, coords, sum);
        }
    }
);

// This example shows how to read two images or use camera
// with OpenCV, transfer the frames to the GPU,
// and apply a convolution written in OpenCL
int main(int argc, char *argv[])
{
    ///////////////////////////////////////////////////////////////////////////

    // setup the command line arguments
    po::options_description desc;
    desc.add_options()
            ("help",  "show available options")
            ("camera", po::value<int>()->default_value(-1),
                                 "if not default camera, specify a camera id")
            ("image", po::value<std::string>(), "path to image file");

    // Parse the command lines
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    //check the command line arguments
    if(vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 0;
    }

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

    //OpenCV variables
    cv::Mat cv_mat;
    cv::VideoCapture cap; //OpenCV camera handle.

    //Filter Variables
    float filter[9] =  {
                -1.0,      0.0,      1.0,
                -2.0,      0.0,      2.0,
                -1.0,      0.0,      1.0,
                };

    // The convolution filter is 3x3
    int filterWidth = 3;

    //OpenCL variables
    // Get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);
    compute::buffer dev_filter(context, sizeof(filter),
                               compute::memory_object::read_only |
                               compute::memory_object::copy_host_ptr,
                               filter);

    compute::program filter_program =
            compute::program::create_with_source(source, context);

    try
    {
        filter_program.build();
    }
    catch(compute::opencl_error e)
    {
        std::cout<<"Build Error: "<<std::endl
                 <<filter_program.build_log();
	return -1;
    }

    // create fliter kernel and set arguments
    compute::kernel filter_kernel(filter_program, "convolution");

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

    //check for image paths
    if(vm.count("image"))
    {
        // Read image with OpenCV
        cv_mat = cv::imread(vm["image"].as<std::string>(),
                                       CV_LOAD_IMAGE_COLOR);
        if(!cv_mat.data){
            std::cerr << "Failed to load image" << std::endl;
            return -1;
        }
    }
    else //by default use camera
    {
        //open camera
        cap.open(vm["camera"].as<int>());
        // read first frame
        cap >> cv_mat;
        if(!cv_mat.data){
            std::cerr << "failed to capture frame" << std::endl;
            return -1;
        }
    }

    // Convert image to BGRA (OpenCL requires 16-byte aligned data)
    cv::cvtColor(cv_mat, cv_mat, CV_BGR2BGRA);

    // Transfer image/frame data to gpu
    compute::image2d dev_input_image =
            compute::opencv_create_image2d_with_mat(
                cv_mat, compute::image2d::read_write, queue
                );

    // Create output image
    // Be sure what will be your ouput image/frame size
    compute::image2d dev_output_image(
                context,
                dev_input_image.width(),
                dev_input_image.height(),
                dev_input_image.format(),
                compute::image2d::write_only
                );

    filter_kernel.set_arg(0, dev_input_image);
    filter_kernel.set_arg(1, dev_output_image);
    filter_kernel.set_arg(2, dev_filter);
    filter_kernel.set_arg(3, filterWidth);

    // run flip kernel
    size_t origin[2] = { 0, 0 };
    size_t region[2] = { dev_input_image.width(),
                         dev_input_image.height() };

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

    queue.enqueue_nd_range_kernel(filter_kernel, 2, origin, region, 0);

    //check for image paths
    if(vm.count("image"))
    {
        // show host image
        cv::imshow("Original Image", cv_mat);

        // show gpu image
        compute::opencv_imshow("Convoluted Image", dev_output_image, queue);

        // wait and return
        cv::waitKey(0);
    }
    else
    {
        char key = '\0';
        while(key != 27) //check for escape key
        {
            cap >> cv_mat;

            // Convert image to BGRA (OpenCL requires 16-byte aligned data)
            cv::cvtColor(cv_mat, cv_mat, CV_BGR2BGRA);

            // Update the device image memory with current frame data
            compute::opencv_copy_mat_to_image(cv_mat,
                                              dev_input_image,queue);

            // Run the kernel on the device
            queue.enqueue_nd_range_kernel(filter_kernel, 2, origin, region, 0);

            // Show host image
            cv::imshow("Camera Frame", cv_mat);

            // Show GPU image
            compute::opencv_imshow("Convoluted Frame", dev_output_image, queue);

            // wait
            key = cv::waitKey(10);
        }
    }
    return 0;
}


/**
 * @file discontinuousgalerkin1d.cc
 * @brief NPDE homework "DiscontinuousGalerkin1D" code
 * @author Oliver Rietmann
 * @date 22.05.2019
 * @copyright Developed at ETH Zurich
 */

#include "discontinuousgalerkin1d.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace DiscontinuousGalerkin1D {

/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<double> compBmat(int Ml, int Mr, double h) {
  const int N = 2 * (Ml + Mr + 1);
  Eigen::SparseMatrix<double> A(N, N);
  //====================
  // Your code goes here
  //====================
  return A;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double Feo(double v, double w) {
  double result;
  //====================
  // Your code goes here
  //====================
  return result;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
Solution solveTrafficFlow() {
  int Ml = 40;
  int Mr = 40;
  int N_half = Mr + Ml + 1;
  int N = 2 * N_half;

  double h = 0.05;
  double tau = h / 3;
  double T = 1.0;
  unsigned int m = (unsigned int)(T / tau);

  //====================
  // Your code goes here
  // Fill the following vectors
  Eigen::VectorXd x;
  Eigen::VectorXd u;
  //====================

  return Solution(std::move(x), std::move(u));
}
/* SAM_LISTING_END_3 */

}  // namespace DiscontinuousGalerkin1D