#include "perlin_noise.h"

#include <array>
#include <boost/random.hpp>
#include <vector>

#include "composite_data_buffer.h"

namespace noises {
namespace nodes
{
    struct PerlinNoiseData
    {
        PerlinNoiseData() { }

        std::unique_ptr<boost::random::mt19937> rng;

        std::vector<signed char> vectors;

        int dimensions;
    };

    PerlinNoise::PerlinNoise()
    {
        seed_socket_ = &inputs().add("Seed", SocketType::uniform);
        seed_socket_->set_accepts(ConnectionDataType::value<long, 1>());

        // Just a vector2 to start out with
        points_socket_ = &inputs().add("Points", SocketType::attribute);
        points_socket_->set_accepts(ConnectionDataType::value<float, 2>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 3>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 4>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 5>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 6>());

        output_socket_ = &outputs().add("Output", ConnectionDataType::value<float, 1>(), SocketType::attribute);
    }

    std::string PerlinNoise::node_name() const
    {
        return "Perlin Noise";
    }

    void PerlinNoise::execute_uniforms(const CompositeDataBuffer &input, DataBuffer &output) const
    {
        long seed = input.get_uniform<long, 1>(*seed_socket_);

        std::shared_ptr<PerlinNoiseData> data(new PerlinNoiseData);

        data->rng.reset(new boost::random::mt19937);
        data->rng->seed(seed);

        int dimensions = points_socket_->connection()->get().data_type().dimensions();
        load_hypercube_edge_vectors(*data, dimensions);

        utils::shuffle_group(data->vectors, *data->rng, dimensions);

        output.set_scratch(0, std::move(data));
    }

    void PerlinNoise::load_hypercube_edge_vectors(PerlinNoiseData &data, int dimensions) const
    {
        switch(dimensions)
        {
            case 0:
                throw std::logic_error("Cannot have 0 dimension noise.");
            case 1:
                throw std::logic_error("Cannot have 1 dimension perlin noise.");
            case 2:
                data.vectors = HypercubeEdges<2>::edge_vectors_flattened();
                break;
            case 3:
                data.vectors = HypercubeEdges<3>::edge_vectors_flattened();
                break;
            case 4:
                data.vectors = HypercubeEdges<4>::edge_vectors_flattened();
                break;
            case 5:
                data.vectors = HypercubeEdges<5>::edge_vectors_flattened();
                break;
            case 6:
                data.vectors = HypercubeEdges<6>::edge_vectors_flattened();
        }

        data.dimensions = dimensions;
    }

    void PerlinNoise::execute_attributes(const CompositeDataBuffer &input, DataBuffer &output, DataBuffer::size_type index) const
    {
        PerlinNoiseData& data = *output.get_scratch_ref<std::shared_ptr<PerlinNoiseData>>(0);
        int dimensions = data.dimensions;

        const float* attribute_point = get_input_point_attribute(index, input, dimensions);

        std::vector<int> dimension_starts(dimensions);
        std::vector<std::vector<int>> points;
        std::vector<int> gradient_indexes;

        int num_gradients = data.vectors.size() / dimensions;

        // Get floor(x), floor(y), floor(z) etc
        for(int i = 0; i < dimensions; i++)
        {
            dimension_starts[i] = std::floor(attribute_point[i]);
        }

        // Get all points in the hypercube surrounding the input point
        get_point_permutations(dimension_starts, dimensions, std::vector<int>(), points);

        // Get all rng gradients for each point. Hash x/y/z to get index into gradients array
        for(unsigned int i = 0; i < points.size(); i++)
        {
            std::vector<int>& point = points[i];
            unsigned int sum = 0;
            for(int j = 0; j < dimensions; j++)
            {
                static const unsigned int primes[] { 1699, 2237, 2671, 3571, 1949, 2221, 3469, 3083 };
                sum += point[j] * point[j] * primes[j]; //Hopefully this has enough entropy and random enough results
            }

            gradient_indexes.push_back(sum % num_gradients);
        }

        // Because of how the get_point_permutations algorithm works, adjacent verticies are in pairs recursively
        // There will always be an even number of points (2^n)
        std::vector<float> out(points.size());

        // vector from the first hypercube vertex to the input point (main diagonal) i.e. uv
        std::vector<float> uvs(dimensions);

        for(int i = 0; i < dimensions; i++)
        {
            uvs[i] = attribute_point[i] - points[0][i];
        }

        // Number of vertexes for an n-dimensional hypercube is 2^n
        int num_points = std::pow(2, dimensions);

        // Find dot product for every vertex.
        // Multiply the vector from the hypercube vertex to the input point, and the gradient at the hypercube vertex
        for(int i = 0; i < num_points; i++)
        {
            std::vector<int>& point = points[i];
            int gradient_index = gradient_indexes[i] * dimensions;

            // In theory, the dot products will be -1 to 1
            float dot = 0;

            for(int j = 0; j < dimensions; j++)
            {
                // gradient value (for this dimension) at hypercube vertex
                signed char gradient_u = data.vectors[gradient_index + j];

                // input point (in this dimension)
                float attr_p = attribute_point[j];

                // vector (for this dimension) from hypercube vertex to input point
                float u = attr_p - point[j];

                dot += u * gradient_u;
            }

            out[i] = dot;
        }

        // Collapse the dimensions down
        int current_dimension = dimensions;
        for(; num_points > 1; num_points /= 2)
        {
            // Take the dot products two at a time and interpolate between then
            int half_num = num_points / 2;
            for(int i = 0; i < half_num; i++)
            {
                int index = i * 2;

                float x_1 = out[index];
                float x_2 = out[index + 1];

                float u = uvs[current_dimension - 1];
                float interpolated = 6 * std::pow(u, 5) - (15 * std::pow(u, 4)) + (10 * std::pow(u, 3));

                out[i] = (x_1 * (1.0f - interpolated)) + (x_2 * interpolated);
            }

            current_dimension--;
        }

        output.set_attribute<float, 1>(*output_socket_, index, &out[0]);
    }

    const float* PerlinNoise::get_input_point_attribute(DataBuffer::size_type index, const CompositeDataBuffer& input, int dimensions) const
    {
        switch(dimensions)
        {
            case 2:
                return &input.get_attribute<float, 2>(*points_socket_, index);
            case 3:
                return &input.get_attribute<float, 3>(*points_socket_, index);
            case 4:
                return &input.get_attribute<float, 4>(*points_socket_, index);
            case 5:
                return &input.get_attribute<float, 5>(*points_socket_, index);
            case 6:
                return &input.get_attribute<float, 7>(*points_socket_, index);
        }
        throw std::logic_error("Not supported");
    }

    void PerlinNoise::get_point_permutations(std::vector<int>& dimension_starts, int dimensions, std::vector<int> point, std::vector<std::vector<int>>& points)
    {
        int dimension = point.size();
        if(dimension == dimensions)
        {
            points.push_back(point);
        }
        else
        {
            int start = dimension_starts[dimension];

            point.push_back(start);
            get_point_permutations(dimension_starts, dimensions, point, points);

            point[dimension] = start + 1;
            get_point_permutations(dimension_starts, dimensions, point, points);
        }
    }
} }


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

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

    typedef std::complex<double>      cdouble;
    const unsigned                    xd= 2, yd= 5, n= xd * yd;
    compressed2D<cdouble>             A(n, n);
    mat::laplacian_setup(A, xd, yd);

    // Fill imaginary part of the matrix
    A*= cdouble(1, -1);
    std::cout << "A is\n" << with_format(A, 7, 1) << "\n";

    std::cout << "trace(A) is " << trace(A) << "\n\n";
    std::cout << "conj(A) is\n" << with_format(mtl::mat::conj(A), 7, 1) << "\n"; // ADL issue on g++ 4.4
    std::cout << "trans(A) is\n" << with_format(trans(A), 7, 1) << "\n";
    std::cout << "hermitian(A) is\n" << with_format(hermitian(A), 7, 1) << "\n";

    return 0;
}


#include "potts.h"
#include "ising.h"
#include "../util/observable.hpp"
#include "../util/squarelattice.hpp"
#include <iostream>
#include <boost/random.hpp>
#include <ctime>
#include <mpi.h>
#include <boost/shared_ptr.hpp>
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

int main(int argc, char **argv)
{
  typedef util::SquareLattice Lattice;
  typedef boost::variate_generator<boost::mt19937, boost::uniform_real<> > RNG01;
  typedef boost::shared_ptr<potts::Model<Lattice, RNG01> > ModelPtr;

  MPI_Init(&argc, &argv);

  int nprocs, rank;
  MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);

  RNG01 rnd(boost::mt19937(static_cast<uint32_t>(std::time(0)+rank*137)), boost::uniform_real<>(0.0, 1.0));

  using namespace boost;
  using namespace boost::program_options;
  options_description opt("options");
  opt.add_options()
    ("help", "show this message")
    ("Ising", "simulate Ising model")
    ("q,q", value<int>()->default_value(2), "number of state of a spin")
    ("h,h", value<double>()->default_value(0.0), "magnetic field")
    ("L,L", value<int>()->default_value(10), "length of lattice")
    ("beta-min", value<double>()->default_value(0.1), "minimum beta")
    ("beta-max", value<double>()->default_value(1.0), "maximum beta")
    ("beta-num", value<int>()->default_value(10), "number of beta")
    ("thermalization", value<int>()->default_value(8), "number of exchange to thermalize")
    ("mcs", value<int>()->default_value(64), "number of exchange to measure")
    ("interval,i", value<int>()->default_value(1024), "temperature exchange interval")
    ("no-exchange", "switch off temperature exchange")
    ;

  variables_map vm;
  store(parse_command_line(argc, argv, opt), vm);
  notify(vm);

  if( vm.count("help") ){
    std::cout << opt << std::endl;
    MPI_Barrier(MPI_COMM_WORLD);
    MPI_Finalize();
    return 0;
  }

  const bool bIsing = vm.count("Ising");
  const bool noEx = vm.count("no-exchange");

  const int q = vm["q"].as<int>();
  const int L = vm["L"].as<int>();
  const double h = vm["h"].as<double>();
  const double bmin = vm["beta-min"].as<double>();
  const double bmax = vm["beta-max"].as<double>();
  const int nbeta = vm["beta-num"].as<int>();
  const int therm = vm["thermalization"].as<int>();
  const int MCS = vm["mcs"].as<int>();
  const int interval = vm["interval"].as<int>();

  const double dbeta = (bmax-bmin)/(nbeta-1);

  // the i-th worker runs under beta = betas[i]
  // the beta_index[i]-th worker runs under the i-th lowest beta (bmin + i*dbeta)
  // in other words, the value of the i-th lowest beta is betas[beta_index[i]]
  std::vector<double> betas(nbeta);
  std::vector<int> beta_index(nbeta);
  // inv_beta_index is the inverse of beta_index
  std::vector<int> inv_beta_index(nbeta);
  for(int i=0; i<nbeta; ++i){
    betas[i] = bmin + i*dbeta;
    beta_index[i] = i;
    inv_beta_index[i] = i;
  }

  std::vector<int> offsets(nprocs+1,0);
  for(int i=0; i<nbeta%nprocs; ++i){
    offsets[i+1] = offsets[i] + nbeta/nprocs+1;
  }
  for(int i = nbeta%nprocs; i<nprocs; ++i){
    offsets[i+1] = offsets[i] + nbeta/nprocs;
  }
  assert(offsets[nprocs] == nbeta);
  const int offset = offsets[rank];

  const int nbeta_local = offsets[rank+1]-offsets[rank];

  std::vector<ModelPtr> models;
  if(bIsing){
    for(int i=0; i<nbeta_local; ++i){
      models.push_back(ModelPtr(new potts::Ising<Lattice,RNG01>(h,L)));
    }
  }else{
    for(int i=0; i<nbeta_local; ++i){
      models.push_back(ModelPtr(new potts::Potts<Lattice,RNG01>(q,h,L)));
    }
  }

  std::vector<util::Observable> obs_ene(nbeta);
  std::vector<util::Observable> obs_mag(nbeta);

  for(int mcs = 0; mcs < therm+MCS; ++mcs){
    for(int lm=0; lm < interval; ++lm){
      for(int i=0; i<nbeta_local; ++i){
        models[i]->update(betas[offset+i], rnd);
      }
    }
    std::vector<double> enes_local(nbeta);
    std::vector<double> mags_local(nbeta);
    for(int i=0; i<nbeta_local; ++i){
      enes_local[offset+i] = models[i]->ene();
      mags_local[offset+i] = models[i]->mag();
    }
    std::vector<double> enes(rank==0?nbeta:0);
    std::vector<double> mags(rank==0?nbeta:0);
    MPI_Reduce(&enes_local[0], &enes[0], nbeta, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
    MPI_Reduce(&mags_local[0], &mags[0], nbeta, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);

    if(noEx){
      if(rank == 0){
        for(int i=0; i<nbeta; ++i){
          obs_ene[i] << enes[beta_index[i]];
          obs_mag[i] << mags[beta_index[i]];
        }
        std::clog << "mcs : " << mcs+1 << "/" << MCS+therm << " done." << std::endl;
      }
    }else{
      if(rank == 0){
        for(int i=0; i<nbeta; ++i){
          obs_ene[i] << enes[beta_index[i]];
          obs_mag[i] << mags[beta_index[i]];
        }

        for(int i=nbeta-1; i>0; --i){ // from low-temperature to high-T
          const int ihigh = beta_index[i];
          const int ilow = beta_index[i-1];
          const double p = std::exp( (betas[ihigh] - betas[ilow])*(enes[ihigh]-enes[ilow]));
          if(rnd() < p){
            std::swap(betas[ihigh], betas[ilow]);
            std::swap(beta_index[i], beta_index[i-1]);
            std::swap(inv_beta_index[beta_index[i]], inv_beta_index[beta_index[i-1]]);
          }
        }
      }
      MPI_Bcast(&betas[0], nbeta, MPI_DOUBLE, 0, MPI_COMM_WORLD);
      if(rank==0){
        if(mcs == therm){
          for(int i=0; i<nbeta; ++i){
            obs_ene[i].reset();
            obs_mag[i].reset();
          }
        }
        std::clog << "mcs : " << mcs+1 << "/" << MCS+therm << " done." << std::endl;
      }
    }
  }

  if(rank==0){
    for(int i=0; i<nbeta; ++i){
      std::cout << betas[beta_index[i]]
         << " " << obs_ene[i].mean() << " " << obs_ene[i].error()
         << " " << obs_mag[i].mean() << " " << obs_mag[i].error()
         << std::endl;
    }
  }

  MPI_Barrier(MPI_COMM_WORLD);
  MPI_Finalize();

  return 0;
}


////////////////////////////////////////////////////////////////////////
// Manuel Martinez (manuel.martinez@kit.edu)
//
// license: LGPLv3
// FLAGS: -g -std=c++11 `pkg-config opencv --cflags --libs` -lboost_system -pthread -lturbojpeg  -lboost_system -lboost_program_options -O2 -Wno-inline

#include <opencv2/opencv.hpp>
#include <boost/program_options.hpp>
#include <uSnippets/log.hpp>
#include <dirent.h>

using namespace uSnippets;

struct HeatMap {

	cv::Mat1f H;

	HeatMap() {}

	HeatMap(cv::Size size) : H(size,0.f) {}

	HeatMap &filter(double cooling=0.90, double sigma=3) {

		H *= cooling;
		cv::GaussianBlur(H,H,cv::Size(),sigma,sigma);
		return *this;
	}

	HeatMap &heat(cv::Point2f pt, double temperature) {
/*
		cv::Mat1f row(1,H.cols,0.f);
		for (int j=0; j<H.cols; j++)
			row(j) = std::exp(- std::pow((j-pt.x)/2,2)/(2*sigma*sigma) );

		cv::Mat1f col(H.rows,1,0.f);
		for (int i=0; i<H.rows; i++)
			col(i) = temperature*std::exp(- std::pow((i-pt.y)/2,2)/(2*sigma*sigma) );

		for (int i=0; i<H.rows; i++)
			for (int j=0; j<H.cols; j++)
				H(i,j) += col(i)*row(j);*/
		if (pt.x>=H.cols or pt.y>=H.rows) return *this;
		H(pt)+=temperature;
		return *this;
	}

	cv::Mat3b draw(cv::Mat3b img, cv::Vec3b color, float scale=0) const {

		cv::Mat3b out = img.clone();
		Assert(img.size==H.size) << "Heat map not the same size as the image";

		if (scale==0) {
			for (int i=0; i<H.rows; i++)
				for (int j=0; j<H.cols; j++)
					scale = std::max(scale,H(i,j));
			scale = 1./(scale+1e-10);
		}

		for (int i=0; i<H.rows; i++) {
			for (int j=0; j<H.cols; j++) {
				double h = std::min(std::max(H(i,j)*scale,0.f),1.f);
				out(i,j)/= 1+h;
				out(i,j) += h*color;
			}
		}
		return out;
	}

	operator bool() const { return not H.empty(); }
};

namespace Geometry {

	typedef cv::Vec4f Vec4; // third component should always be 1
	typedef cv::Matx44f Mat44;

/*	struct PinHole { double fx, fy, cx, cy; };

	class Pose {
		mutable Mat44 transformMatrix, iTransformMatrix;
	public:

		const Vec4   translation;
		const Vec4   quaternion;

		Pose( const Vec4 &translation, const Vec4 &rotation ): translation(translation), rotation(rotation), transformMatrix(Mat44::zeros()), iTransformMatrix(Mat44::zeros()) {};

		const Mat44 &getTransformMatrix() const {

			const auto &q = quaternion;
			if (transformMatrix(4,4)==0) transformMatrix = Mat44(
				q[3]*q[3]+q[0]*q[0]-q[1]*q[1]-q[2]*q[2],					2*q[0]*q[1]+2*q[3]*q[2],					2*q[0]*q[2]-2*q[3]*q[1], 	translation[0],
								2*q[0]*q[1]-2*q[3]*q[2], 	q[3]*q[3]-q[0]*q[0]+q[1]*q[1]-q[2]*q[2],				 	2*q[1]*q[2]+2*q[3]*q[0], 	translation[1],
								2*q[0]*q[2]+2*q[3]*q[1], 					2*q[1]*q[2]-2*q[3]*q[0], 	q[3]*q[3]-q[0]*q[0]-q[1]*q[1]+q[2]*q[2], 	translation[2],
													0.f, 										0.f,										0.f, 	  			1.);
			return transformMatrix;
		}

		const Mat44 &getInvTransformMatrix(const Vec4 &t, const Vec4 &q) const {

			getTransformMatrix();
			const auto &tm = transformMatrix;
			if (iTransformMatrix(4,4)==0) iTransformMatrix = Mat44(
				tm(0,0), tm(1,0), tm(2,0), -(tm(0,0)*tm(0,3) + tm(1,0)*tm(1,3) + tm(2,0)*tm(2,3)),
				tm(0,1), tm(1,1), tm(2,1), -(tm(0,1)*tm(0,3) + tm(1,1)*tm(1,3) + tm(2,1)*tm(2,3)),
				tm(0,2), tm(1,2), tm(2,2), -(tm(0,2)*tm(0,3) + tm(1,2)*tm(1,3) + tm(2,2)*tm(2,3)),
					  0,	   0, 	    0,													 	1);
			return iTransformMatrix;
		}

		bool operator< (const Pose &p) const { return translation=p.translation?rotation<p.rotation:translation<p.translation; }

		friend std::ostream& operator<< (std::ostream &out, const Pose &p) { out << pt.translation << " " << pt.rotation; return out; }
	};

	// Transform a point in Object Coordinates to World Coordinates
	inline Vec4 tObjectToWorld( const Vec4 &p3D, const Pose &objectPose ) { return objectPose.getTransformMatrix()*p3D; }

	// Transform a point in World Coordinates to Camera Coordinates
	inline Vec4 tWorldToCamera( const Vec4 &p3D, const Pose &camPose    ) { return camPose.getInvTransformMatrix()*p3D; }

	// Transform a point in Object Coordinates to Camera Coordinates
	//static inline Vec4 tWorldToCamera( const Vec4 &p3D, const Pose &objectPose, const Pose &camPose ) { return camPose.iTransformMatrix*p3D; }

	inline cv::Vec2f projectToFrame( const Vec4 &p3D, const PinHole &pinhole) {
		return { proj3D[0]/(proj3D[2]+std::numeric_limits<float>::min()) * pinhole.fx + pinhole.cx,
				 proj3D[1]/(proj3D[2]+std::numeric_limits<float>::min()) * pinhole.fy + pinhole.cy };
	}*/
}

struct ORBFeatureEngine {

	cv::ORB orb = cv::ORB(2000,1.15f,12);

	struct Descriptor {

		const cv::Mat1b d;

		Descriptor(const cv::Mat1b &d) : d(d.clone()) {}

		uint dist(const Descriptor &desc) const {
			const uint64_t *v0 = reinterpret_cast<const uint64_t*>(d.data);
			const uint64_t *v1 = reinterpret_cast<const uint64_t*>(desc.d.data);
			return _popcnt64(v0[0]^v1[0])+_popcnt64(v0[1]^v1[1])+_popcnt64(v0[2]^v1[2])+_popcnt64(v0[2]^v1[2]);
		}
	};

	std::vector<std::pair<cv::KeyPoint,Descriptor>> extract(cv::Mat3b img) {

		std::vector<std::pair<cv::KeyPoint,Descriptor>> desc;

		std::vector<cv::KeyPoint> keyPoints;
		cv::Mat1b descriptors;
		orb(img, cv::Mat(), keyPoints, descriptors);
		for (size_t i=0; i<keyPoints.size(); i++)
			desc.emplace_back(keyPoints[i], descriptors.row(i));

		return desc;
	}

};

/*
struct SURFFeatureEngine {

	cv::SURF surf(500);

	struct Descriptor {

		const cv::Mat1f d;

		Descriptor(const cv::Mat1f &d) : d(d.clone()) {}

		float dist(const Descriptor &desc) const {
			return cv::norm(d-desc.d);
		}
	};

	std::vector<std::pair<cv::KeyPoint,Descriptor>> extract(cv::Mat3b img) {

		std::vector<std::pair<cv::KeyPoint,Descriptor>> desc;

		std::vector<cv::KeyPoint> keyPoints;
		cv::Mat1f descriptors;
		surf(img, cv::Mat(), keyPoints, descriptors);
		for (size_t i=0; i<keyPoints.size(); i++)
			desc.emplace_back(keyPoints[i], descriptors.row(i));

		return desc;
	}

};*/

template<typename FeatureEngine>
struct MopedLt {

	static inline std::vector<std::string> getAllFilenames(std::string path, std::string type="") {

		std::vector<std::string> r;
		DIR *dir;
		struct dirent *ent;
		if ((dir = opendir (path.c_str())) == NULL)
			return r;

		while ((ent = readdir (dir)) != NULL)
			if (std::string(ent->d_name).size()>=type.size() and std::string(&ent->d_name[std::string(ent->d_name).size()-type.size()])==type)
				r.push_back(path+"/"+ent->d_name);

		closedir (dir);
		std::sort(r.begin(), r.end());
		return r;
	}

	FeatureEngine FE;
	typedef typename FeatureEngine::Descriptor Descriptor;

	struct ImageKeyPoint {

		cv::KeyPoint kp;
		Descriptor desc;
		cv::Mat3b img;
		ImageKeyPoint(const cv::KeyPoint &kp, const Descriptor &desc, const cv::Mat3b &img) : kp(kp), desc(desc), img(img) {}
	};

	struct ModelKeyPoint {

		Geometry::Vec4 pt;
		std::vector<ImageKeyPoint> ikps;
		ModelKeyPoint(const cv::KeyPoint &kp, const Descriptor &desc, const cv::Mat3b &img) { ikps.emplace_back(kp,desc,img); }
	};

	typedef std::vector<ModelKeyPoint> Model;

	std::vector<std::pair<std::string, Model>> models;

	MopedLt &add(std::string path) {

		for (auto &model : getAllFilenames(path)) {

			std::vector<cv::Mat3b> images;
			for (auto &imageName : getAllFilenames(model)) {
				cv::Mat3b image = cv::imread(imageName);
				while (image.cols>1024) cv::resize(image,image,cv::Size(),.5,.5);
				if (not image.empty())
					images.emplace_back(image);
			}
			if (images.empty()) continue;

			std::string name = model.substr(model.find_last_of("/\\")+1);
			Log(0) << name << ": " << images.size();

			add(name, images);
		}
		return *this;
	}

	MopedLt &add(std::string name, std::vector<cv::Mat3b> images) {

		models.emplace_back(name, Model());

		Model &model = models.back().second;

		Assert(images.size()>2) << "Not enough images for SFM";

		// We assume images are correlated, therefore an almost-planar transformation must exist between consecutive images
		Log(0) << "KeyPoint Extraction";
		{
			for (auto &p : FE.extract(images[0]))
				model.emplace_back(p.first, p.second, images[0]);

			for (size_t ii = 1; ii<images.size(); ii++) {
				Log(0) << "Image " << ii << " of " << images.size();

				auto feats = FE.extract(images[ii]);
				Assert(feats.size()>4) << "Not enough features in image" << ii;

				std::vector<size_t> bestMatchA(model.size()), bestMatchB(feats.size());
				for (size_t i=0; i<model.size(); i++) {
					for (size_t j=0; j<feats.size(); j++) {

						if (model[i].ikps.back().desc.dist(feats[j].second) < model[i].ikps.back().desc.dist(feats[bestMatchA[i]].second))
							bestMatchA[i] = j;

						if (model[i].ikps.back().desc.dist(feats[j].second) < model[bestMatchB[j]].ikps.back().desc.dist(feats[j].second))
							bestMatchB[j] = i;
					}
				}

				std::vector<cv::Point2f> pointsA, pointsB;
				for (size_t j=0; j<feats.size(); j++) {
					if (bestMatchA[bestMatchB[j]]==j) {
						pointsA.push_back(model[bestMatchB[j]].ikps.back().kp.pt);
						pointsB.push_back(feats[j].first.pt);
					}
				}

				cv::Mat maskG;
				cv::Mat H = findHomography( pointsA, pointsB, cv::RANSAC, 5, maskG );
				cv::Mat1b mask = maskG;
				if (H.empty()) Log(0) << "Homography found: " << H.empty();

				{

					std::vector<cv::KeyPoint> kp;
					for (size_t j=0,k=0; j<feats.size(); j++) {
						if (bestMatchA[bestMatchB[j]]!=j) continue;
						if (mask(k))
							kp.push_back(feats[j].first);
						k++;
					}

					cv::Mat3b displayImg = images[ii].clone();
					cv::drawKeypoints(displayImg, kp, displayImg);
					cv::imshow("kk",displayImg);
					cv::waitKey(10);
				}

				for (size_t j=0,k=0; j<feats.size(); j++) {
					if (bestMatchA[bestMatchB[j]]!=j) continue;
					if (mask(k)) {
						model[bestMatchB[j]].ikps.push_back({feats[j].first, feats[j].second, images[ii]});
					} else {
						model.emplace_back(feats[j].first, feats[j].second, images[ii]);
					}
					k++;
				}
			}
		}
		Log(0) << "KeyPoint Extraction Done: " <<model.size() << " current points";

		Log(0) << "Removing low cardinality points";
		{
			std::vector<ModelKeyPoint> modelTmp;
			std::swap(model, modelTmp);
			for (auto &m : modelTmp)
				if (m.ikps.size()>2)
					model.push_back(m);
		}
		Log(0) << "Removing low cardinality points: Done, " <<model.size() << " current points";
		return *this;
	}

//	Point *query(const Descriptor &) {}

//	std::vector<std::pair<cv::Point2f,Point *>> queryN(cv::Mat3b img) {
//	}

	std::string queryLargestObject(cv::Mat3b image, HeatMap &heatMap) {

		std::vector<cv::KeyPoint> kps;
		std::map<std::pair<std::string,Model>*,double> mp;

		for (auto &f : FE.extract(image)) {

			struct MatchInfo { double distance; std::pair<std::string,Model> *m; ModelKeyPoint *k; };
			MatchInfo M[2]; M[0].distance = M[1].distance = 1e10;
			for (auto &m : models) {
				for (auto &k : m.second) {
					for (auto &ikp : k.ikps) {
						double d = ikp.desc.dist(f.second);
						if (d>=M[1].distance) continue;
						M[1].distance = d;
						M[1].m = &m;
						M[1].k = &k;
						if (M[0].distance > M[1].distance) std::swap(M[0],M[1]);
					}
				}
			}
			Log(-2) << M[0].distance << " " << M[1].distance;
			if (M[0].k != M[1].k) continue;
			//if (M[0].distance > 40) continue; //Magic number!
			kps.push_back(f.first);
			mp[M[0].m] += std::pow(1/M[0].distance,2);

			if (heatMap)
				heatMap.heat(f.first.pt,.2);
		}
		heatMap.filter();

		Log(0) << kps.size();
		if (0) {
			cv::Mat3b displayImg = image.clone();
			cv::drawKeypoints(displayImg, kps, displayImg);
			cv::imshow("kk",displayImg);
			cv::waitKey(10);
		}

		std::pair<std::string,Model> *best=&models.front();
		for (auto &m : mp)
			if (m.second>mp[best])
				best = m.first;

		for (auto &m : mp)
			Log(0) << m.first->first << ": " << m.second;

		return "";
	}
};

/*
class MonoSFM {

public:

	std::map<std::string, std::vector<cv::KeyPoint>> imageKeypoints;
	std::map<std::string, cv::Mat1b> imageDescriptors;
	std::map<std::string, cv::Mat1f> imageFundamentals;

	cv::Mat3b panorama;
	std::vector<cv::KeyPoint> panoramaKeypoints;
	cv::Mat1b panoramaDescriptors;

	MonoSFM(const std::Mat3b &pattern, double ppm, const std::vector<std::string> &imageFiles) {

		std::vector<cv::KeyPoint> lastKeypoints;
		cv::Mat1b lastDescriptors;

		orb(pattern, cv::Mat(), lastKeypoints, lastDescriptors);

		Log(0) << "Extracting all descriptors";
		for (auto &imageFile : imageFiles) {

			cv::Mat3b image = cv::imread(imageFile);

			std::vector<cv::KeyPoint> keypoints;
			cv::Mat1b descriptors;
			orb(image, cv::Mat(), keypoints, descriptors);

			if (descriptors.rows<8) throw(std::string("Not enough keypoints"));

			for (int i=0; i<descriptors.rows; i++) {
				std::vector<std::pair<int, uint>> dists;
				for (int j=0; j<lastDescriptors.rows; j++)
					dists.emplace_back(j,dist(descriptors.row(i), lastDescriptors.row(j)));

				std::partial_sort(dists.begin(), dists.begin()+3, dists.end());

				Log(0) << dists[0].second << " " << dists[1].second;
			}

			lastKeypoints = keypoints;
			lastDescriptors = descriptors;
		}
	}
};*/

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

	// PARSING PROGRAM OPTIONS
	namespace po = boost::program_options;
	po::options_description pod("BlindSLAM Tool");

	po::positional_options_description p;
	p.add("path", 1);

	pod.add_options()
		("help,h", "produce this help message")
		("log,l", po::value<int>()->default_value(0), "set log level")
		("path", po::value<std::string>()->default_value("data/objects"), "path to SFM files");

	po::variables_map pom;
	po::store( po::command_line_parser( argc, argv).options(pod).positional(p).run(), pom);
	po::notify(pom);

	if (pom.count("help")) {
		std::cout << "Usage:" << std::endl <<  pod << "\n";
		return 0;
	}

	Log::reportLevel(pom["log"].as<int>());

	MopedLt<ORBFeatureEngine> moped;
	moped.add(pom["path"].as<std::string>());

	cv::VideoCapture cap(0);
    if (cap.isOpened()) {
		while (cv::waitKey(10)!='q') {
			cv::Mat3b image;
			cap >> image;
			HeatMap heatMap(image.size());
			moped.queryLargestObject(image,heatMap);
			cv::imshow("kk",heatMap.draw(image,{0,0,255}));
		}
	} else {
		for (auto &model : moped.getAllFilenames(pom["path"].as<std::string>())) {

			for (auto &imageName : moped.getAllFilenames(model)) {
				cv::Mat3b image = cv::imread(imageName);
				while (image.cols>1024) cv::resize(image,image,cv::Size(),.5,.5);
				if (image.empty()) continue;
				HeatMap heatMap(image.size());
				moped.queryLargestObject(image,heatMap);
				cv::imshow("kk",heatMap.draw(image,{0,0,255}));
				cv::waitKey(10);
			}
		}
	}
}


#include "batoid.h"
#include "ray.h"
#include "surface.h"
#include "medium.h"
#include "utils.h"
#include <cmath>
#include <numeric>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using Eigen::Vector3d;
using Eigen::Matrix3d;
using Eigen::AngleAxisd;

namespace batoid{
    RayVector rayGrid(double dist, double length,
                      double xcos, double ycos, double zcos,
                      int nside, double wavelength, double flux,
                      const Medium& m, bool lattice=false) {
        double n = m.getN(wavelength);
    // `dist` is the distance from the center of the pupil to the center of the rayGrid.
    // `length` is the length of one side of the rayGrid square.
    // `xcos`, `ycos`, `zcos` are the direction cosines of the ray velocities
    // `nside` is the number of rays on a side of the rayGrid.
    // `wavelength` is the wavelength assigned to the rays
    // `m` is the medium (from which we get the refractive index) at the position of the rays.
    // (Needed to properly normalize the ray magnitudes).
        std::vector<Ray> result;
        result.reserve(nside*nside);

        // The "velocities" of all the rays in the grid are the same.
        Vector3d v(xcos, ycos, zcos);
        v.normalize();
        v /= n;

        double dy;
        if (lattice)
            dy = length/nside;
        else
            dy = length/(nside-1);
        double y0 = -length/2;
        double y = y0;
        for(int iy=0; iy<nside; iy++) {
            double x = y0;
            for(int ix=0; ix<nside; ix++) {
                // Start with the position of the ray when it intersects the pupil
                Vector3d r(x,y,0);
                // We know that the position of the ray that goes through the origin
                // (which is also the center of the pupil), is given by
                //   a = -dist * vhat = -dist * v * n
                // We want to find the position r0 that satisfies
                // 1) r0 - a is perpendicular to v
                // 2) r = r0 + v t
                // The first equation can be rewritten as
                // (r0 - a) . v = 0
                // some algebra reveals
                // (r + v n d) . v - t v . v = 0
                // => t = (r + v n d) . v / v . v
                //      = (r + v n d) . v n^2
                // => r0 = r - v t
                double t = (r + v*n*dist).dot(v) * n * n;
                result.emplace_back(r-v*t, v, 0, wavelength, flux, false);
                x += dy;
            }
            y += dy;
        }
        return RayVector(std::move(result), wavelength);
    }

    RayVector circularGrid(double dist, double outer, double inner,
                           double xcos, double ycos, double zcos,
                           int nradii, int naz, double wavelength, double flux, const Medium& m) {
        double n = m.getN(wavelength);

        // Determine number of rays at each radius
        std::vector<int> nphis(nradii);
        double drfrac = (outer-inner)/(nradii-1)/outer;
        double rfrac = 1.0;
        for (int i=0; i<nradii; i++) {
            nphis[i] = int(std::ceil(naz*rfrac/6.))*6;
            rfrac -= drfrac;
        }
        // Point in the center is a special case
        if (inner == 0.0)
            nphis[nradii-1] = 1;
        int nray = std::accumulate(nphis.begin(), nphis.end(), 0);

        std::vector<Ray> result;
        result.reserve(nray);

        // The "velocities" of all the rays in the grid are the same.
        Vector3d v(xcos, ycos, zcos);
        v.normalize();
        v /= n;

        rfrac = 1.0;
        for (int i=0; i<nradii; i++) {
            double az = 0.0;
            double daz = 2*M_PI/nphis[i];
            double radius = rfrac*outer;
            for (int j=0; j<nphis[i]; j++) {
                Vector3d r(radius*std::cos(az), radius*std::sin(az), 0);
                double t = (r + v*n*dist).dot(v) * n * n;
                result.emplace_back(r-v*t, v, 0, wavelength, flux, false);
                az += daz;
            }
            rfrac -= drfrac;
        }
        return RayVector(std::move(result), wavelength);
    }

    RayVector pointSourceCircularGrid(const Vector3d& source, double outer, double inner,
                                      int nradii, int naz, double wavelength, double flux,
                                      const Medium& m) {
        double n = m.getN(wavelength);

        // Determine largest and smallest axial angle.
        double dist = source.norm();
        double thetaMax = std::atan(outer/dist);
        double thetaMin = std::atan(inner/dist);

        // Determine number of rays at each angle
        std::vector<int> nphis(nradii);
        double dthetaFrac = (thetaMax-thetaMin)/(nradii-1)/thetaMax;
        double thetaFrac = 1.0;
        for (int i=0; i<nradii; i++) {
            nphis[i] = int(std::ceil(naz*thetaFrac/6.))*6;
            thetaFrac -= dthetaFrac;
        }
        // Point in the center is a special case
        if (inner == 0.0)
            nphis[nradii-1] = 1;
        int nray = std::accumulate(nphis.begin(), nphis.end(), 0);

        std::vector<Ray> result;
        result.reserve(nray);

        // Rotation matrix from z-axis aligned to actual source axis.
        Vector3d axis = -source.cross(Vector3d::UnitZ()).normalized();
        double angle = std::acos(source.normalized().dot(Vector3d::UnitZ()));
        Matrix3d rot2 = AngleAxisd(angle, axis).toRotationMatrix();

        thetaFrac = 1.0;
        for (int i=0; i<nradii; i++) {
            double az = 0.0;
            double daz = 2*M_PI/nphis[i];
            double theta = thetaFrac*thetaMax;
            Vector3d vref(std::sin(theta), 0, -std::cos(theta));
            vref /= n;
            for (int j=0; j<nphis[i]; j++) {
                // Rotate vref around the z axis.
                Matrix3d rot1 = AngleAxisd(az, Vector3d::UnitZ()).toRotationMatrix();
                Vector3d v = rot2*rot1*vref;
                result.emplace_back(source, v, 0, wavelength, flux, false);
                az += daz;
            }
            thetaFrac -= dthetaFrac;
        }
        return RayVector(std::move(result), wavelength);
    }
}


#include <iostream>
#include <vector>
#include <algorithm>

#include <Eigen/Dense>

// Uncomment if you want to use this structure for TripletMatrix
template <class scalar>
struct Triplet {

};

template <class scalar>
struct TripletMatrix {
    // TODO: insert here members and methods to TripletMatrix

    // densify() prototype
    Eigen::Matrix<scalar, -1, -1> densify() const;
};

// Uncomment if you want to use this structure for CRSMatrix
// template <class scalar>
// struct ColValPair {
//
// };

template <class scalar>
struct CRSMatrix {
    // TODO: insert here members and methods to CRSMatrix

    // densify() prototype
    Eigen::Matrix<scalar, -1, -1> densify() const;
};

template <class scalar>
void tripletToCRS(const TripletMatrix<scalar> & T, CRSMatrix<scalar> & C) {
    // TODO: conversion function
}

//! \brief overload of operator << for output of Triplet Matrix (debug).
//! WARNING: uses densify() so there may be a lot of fill-in
//! this allows something like std::cout << S
//! \param o standard output stream
//! \param S matrix in Triplet matrix format
//! \return a ostream o, s.t. you can write o << A << B;
std::ostream & operator<<(std::ostream & o, const TripletMatrix<double> & S) {
    return o << S.densify();
}

//! \brief overload of operator << for output of CRS Matrix (debug).
//! WARNING: uses densify() so there may be a lot of fill-in
//! this allows something like std::cout << S
//! \param o standard output stream
//! \param S matrix in CRS matrix format
//! \return a ostream o, s.t. you can write o << A << B;
std::ostream & operator<<(std::ostream & o, const CRSMatrix<double> & S) {
    return o << S.densify();
}

int main() {
    //// Correctness test
    std::size_t nrows = 7, ncols = 5, ntriplets = 9;

    TripletMatrix<double> T;
    CRSMatrix<double> C;

    // TODO: contrtuct T here
    // TODO: Use this loop to push back triplets in your matrix
    for(auto i = 0u; i < ntriplets; ++i) {
        // TODO: Insert triplet (rand() % nrows, rand() % ncols, rand() % 1000))
    }

    std::cout << "***Test conversion with random matrices***" << std::endl;
    tripletToCRS(T, C);
    // TODO: Uncomment if you implemented densify()
//     std::cout << "--> Frobenius norm of T - C: " << (T.densify()-C.densify()).norm() << std::endl;
    std::cout << "T = " << std::endl << T << std::endl;
    std::cout << "C = " << std::endl << C << std::endl;
}

Nummerical Methods for CSE/solutions/solution_3/tripletToCRS.cpp


#include "smmap/gurobi_solvers.h"
#include <gurobi_c++.h>
#include <iostream>
#include <mutex>
#include <Eigen/Eigenvalues>

using namespace Eigen;

static std::mutex gurobi_env_construct_mtx;

GRBQuadExpr normSquared(const std::vector<GRBLinExpr>& exprs)
{
    GRBQuadExpr vector_norm_squared = 0;

    // TODO: replace with a single call to addTerms?
    for (size_t expr_ind = 0; expr_ind < exprs.size(); expr_ind++)
    {
        vector_norm_squared += exprs[expr_ind] * exprs[expr_ind];
    }

    return vector_norm_squared;
}

GRBQuadExpr normSquared(const std::vector<GRBLinExpr>& exprs, const VectorXd& weights)
{
    assert(exprs.size() == (size_t)weights.rows());
    GRBQuadExpr vector_norm_squared = 0;

    // TODO: replace with a single call to addTerms?
    for (size_t expr_ind = 0; expr_ind < exprs.size(); expr_ind++)
    {
        vector_norm_squared += weights((ssize_t)expr_ind) * exprs[expr_ind] * exprs[expr_ind];
    }

    return vector_norm_squared;
}

GRBQuadExpr normSquared(GRBVar* vars, const ssize_t num_vars)
{
    GRBQuadExpr vector_norm_squared = 0;

    // TODO: replace with a single call to addTerms?
    for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
    {
        vector_norm_squared += vars[var_ind] * vars[var_ind];
    }

    return vector_norm_squared;
}

std::vector<GRBLinExpr> buildVectorOfExperssions(const MatrixXd& A, GRBVar* vars, const VectorXd& b)
{
    const ssize_t num_expr = A.rows();
    const ssize_t num_vars = A.cols();
    std::vector<GRBLinExpr> exprs(num_expr, 0);

    for (ssize_t expr_ind = 0; expr_ind < num_expr; expr_ind++)
    {
        for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
        {
            exprs[expr_ind] += A(expr_ind, var_ind) * vars[var_ind];
        }
        exprs[expr_ind] -= b(expr_ind);
    }

    return exprs;
}

VectorXd smmap::minSquaredNorm(const MatrixXd& A, const VectorXd& b, const double max_x_norm)
{
    VectorXd x;
    GRBVar* vars = nullptr;
    try
    {
        const ssize_t num_vars = A.cols();
        const std::vector<double> lb(num_vars, -max_x_norm);
        const std::vector<double> ub(num_vars, max_x_norm);

        // TODO: Find a way to put a scoped lock here
        gurobi_env_construct_mtx.lock();
        GRBEnv env;
        gurobi_env_construct_mtx.unlock();

        env.set(GRB_IntParam_OutputFlag, 0);
        GRBModel model(env);
        vars = model.addVars(lb.data(), ub.data(), nullptr, nullptr, nullptr, (int)num_vars);
        model.update();

        model.addQConstr(normSquared(vars, num_vars), GRB_LESS_EQUAL, max_x_norm * max_x_norm);
        model.setObjective(normSquared(buildVectorOfExperssions(A, vars, b)), GRB_MINIMIZE);
        model.update();
        model.optimize();

        if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL)
        {
            x.resize(num_vars);
            for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
            {
                x(var_ind) = vars[var_ind].get(GRB_DoubleAttr_X);
            }
        }
        else
        {
            std::cout << "Status: " << model.get(GRB_IntAttr_Status) << std::endl;
            exit(-1);
        }
    }
    catch(GRBException e)
    {
        std::cout << "Error code = " << e.getErrorCode() << std::endl;
        std::cout << e.getMessage() << std::endl;
    }
    catch(...)
    {
        std::cout << "Exception during optimization" << std::endl;
    }

    delete[] vars;
    return x;
}

VectorXd smmap::minSquaredNorm(const MatrixXd& A, const VectorXd& b, const double max_x_norm, const VectorXd& weights)
{
    VectorXd x;
    GRBVar* vars = nullptr;
    try
    {
        const ssize_t num_vars = A.cols();
        const std::vector<double> lb(num_vars, -max_x_norm);
        const std::vector<double> ub(num_vars, max_x_norm);

        // TODO: Find a way to put a scoped lock here
        gurobi_env_construct_mtx.lock();
        GRBEnv env;
        gurobi_env_construct_mtx.unlock();

        env.set(GRB_IntParam_OutputFlag, 0);
        GRBModel model(env);
        vars = model.addVars(lb.data(), ub.data(), nullptr, nullptr, nullptr, (int)num_vars);
        model.update();

        model.addQConstr(normSquared(vars, num_vars), GRB_LESS_EQUAL, max_x_norm * max_x_norm);

        GRBQuadExpr objective_fn = normSquared(buildVectorOfExperssions(A, vars, b), weights);
        // Check if we need to add anything extra to the main diagonal.
        const VectorXd eigenvalues = (A.transpose() * weights.asDiagonal() * A).selfadjointView<Upper>().eigenvalues();
        if ((eigenvalues.array() < 1.1e-4).any())
        {
            const std::vector<double> diagonal(num_vars, 1.1e-4 - eigenvalues.minCoeff());
            objective_fn.addTerms(diagonal.data(), vars, vars, (int)num_vars);
        }
        model.setObjective(objective_fn, GRB_MINIMIZE);

        model.update();
        model.optimize();

        if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL)
        {
            x.resize(num_vars);
            for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
            {
                x(var_ind) = vars[var_ind].get(GRB_DoubleAttr_X);
            }
        }
        else
        {
            std::cout << "Status: " << model.get(GRB_IntAttr_Status) << std::endl;
            exit(-1);
        }
    }
    catch(GRBException e)
    {
        std::cout << "Error code = " << e.getErrorCode() << std::endl;
        std::cout << e.getMessage() << std::endl;
    }
    catch(...)
    {
        std::cout << "Exception during optimization" << std::endl;
    }

    delete[] vars;
    return x;
}

Eigen::VectorXd smmap::minSquaredNormSE3VelocityConstraints(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const double max_se3_velocity, const Eigen::VectorXd& weights)
{
    VectorXd x;
    GRBVar* vars = nullptr;
    try
    {
        const ssize_t num_vars = A.cols();
        assert(num_vars % 6 == 0);

        const std::vector<double> lb(num_vars, -max_se3_velocity);
        const std::vector<double> ub(num_vars, max_se3_velocity);

        // TODO: Find a way to put a scoped lock here
        gurobi_env_construct_mtx.lock();
        GRBEnv env;
        gurobi_env_construct_mtx.unlock();

        env.set(GRB_IntParam_OutputFlag, 0);
        GRBModel model(env);
        vars = model.addVars(lb.data(), ub.data(), nullptr, nullptr, nullptr, (int)num_vars);
        model.update();

        // Add the SE3 velocity constraints
        for (int i = 0; i < num_vars / 6; i++)
        {
            model.addQConstr(normSquared(&vars[i * 6], 6), GRB_LESS_EQUAL, max_se3_velocity * max_se3_velocity);
        }

        GRBQuadExpr objective_fn = normSquared(buildVectorOfExperssions(A, vars, b), weights);
        // Check if we need to add anything extra to the main diagonal.
        const VectorXd eigenvalues = (A.transpose() * weights.asDiagonal() * A).selfadjointView<Upper>().eigenvalues();
        if ((eigenvalues.array() < 1.1e-4).any())
        {
            std::vector<double> diagonal(num_vars, 1.1e-4 - eigenvalues.minCoeff());
            objective_fn.addTerms(diagonal.data(), vars, vars, (int)num_vars);
        }
        model.setObjective(objective_fn, GRB_MINIMIZE);

        model.update();
        model.optimize();

        if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL)
        {
            x.resize(num_vars);
            for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
            {
                x(var_ind) = vars[var_ind].get(GRB_DoubleAttr_X);
            }
        }
        else
        {
            std::cout << "Status: " << model.get(GRB_IntAttr_Status) << std::endl;
            exit(-1);
        }
    }
    catch(GRBException e)
    {
        std::cout << "Error code = " << e.getErrorCode() << std::endl;
        std::cout << e.getMessage() << std::endl;
    }
    catch(...)
    {
        std::cout << "Exception during optimization" << std::endl;
    }

    delete[] vars;
    return x;
}


#include <boost/math/distributions/laplace.hpp>


#include <boost/numeric/odeint/stepper/symplectic_rkn_sb3a_m4_mclachlan.hpp>


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

#include "refill/distributions/gaussian_distribution.h"

namespace refill {

TEST(GaussianDistributionTest, ConstructorTests) {
  GaussianDistribution* dist = new GaussianDistribution();

  ASSERT_EQ(1, dist->dimension())<< "Dimension not correct.";
  ASSERT_EQ(Eigen::VectorXd::Zero(1), dist->mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::MatrixXd::Identity(1, 1), dist->cov())
      << "Covariance not correct";

  delete dist;
  dist = new GaussianDistribution(2);

  ASSERT_EQ(2, dist->dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Zero(), dist->mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity(), dist->cov())
      << "Covariance not correct";

  delete dist;
  dist = new GaussianDistribution(Eigen::Vector2d::Zero(),
                                  Eigen::Matrix2d::Identity());

  ASSERT_EQ(2, dist->dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Zero(), dist->mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity(), dist->cov())
      << "Covariance not correct";

  GaussianDistribution dist_4(*dist);

  ASSERT_EQ(2, dist_4.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Zero(), dist_4.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity(), dist_4.cov())
      << "Covariance not correct";
}

TEST(GaussianDistributionTest, SetterTests) {
  GaussianDistribution dist(2);

  dist.setDistributionParameters(Eigen::Vector2d::Ones(),
                                 Eigen::Matrix2d::Identity() * 2.0);

  ASSERT_EQ(2, dist.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Ones(), dist.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 2.0, dist.cov())
      << "Covariance not correct";

  dist.setMean(Eigen::Vector2d::Constant(2.0));

  ASSERT_EQ(2, dist.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Constant(2.0), dist.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 2.0, dist.cov())
      << "Covariance not correct";

  dist.setCov(Eigen::Matrix2d::Identity() * 3.0);

  ASSERT_EQ(2, dist.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Constant(2.0), dist.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 3.0, dist.cov())
      << "Covariance not correct";
}

TEST(GaussianDistributionTest, OperatorTests) {
  GaussianDistribution dist_1(Eigen::Vector2d::Zero(),
                              Eigen::Matrix2d::Identity());
  GaussianDistribution dist_2(Eigen::Vector2d::Ones(),
                              Eigen::Matrix2d::Identity() * 2.0);

  dist_1 += dist_2;

  ASSERT_EQ(2, dist_1.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Ones(), dist_1.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 3.0, dist_1.cov())
      << "Covariance not correct";

  dist_1 -= dist_2;

  ASSERT_EQ(2, dist_1.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Zero(), dist_1.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 5.0, dist_1.cov())
      << "Covariance not correct";

  GaussianDistribution dist_3 = dist_1 + dist_2;

  ASSERT_EQ(2, dist_3.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Ones(), dist_3.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 7.0, dist_3.cov())
      << "Covariance not correct";

  GaussianDistribution dist_4 = dist_1 - dist_2;

  ASSERT_EQ(2, dist_4.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Constant(-1.0), dist_4.mean())
      << "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 7.0, dist_4.cov())
      << "Covariance not correct";

  dist_4.setDistributionParameters(Eigen::Vector2d::Ones(),
                                   Eigen::Matrix2d::Identity());

  GaussianDistribution dist_5 = Eigen::Matrix2d::Ones() * dist_4;

  ASSERT_EQ(2, dist_5.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Constant(2.0), dist_5.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Constant(2.0), dist_5.cov())
      << "Covariance not correct";

  GaussianDistribution dist_6 = 2.0 * dist_4;

  ASSERT_EQ(2, dist_6.dimension())<< "Dimension not correct";
  ASSERT_EQ(Eigen::Vector2d::Constant(2.0), dist_6.mean())<< "Mean not correct";
  ASSERT_EQ(Eigen::Matrix2d::Identity() * 4.0, dist_6.cov())
      <<"Covariance not correct";
}

TEST(GaussianDistributionTest, PdfEvaluationTest) {
    GaussianDistribution distribution(2);

    double evaluation = distribution.evaluatePdf(Eigen::VectorXd::Zero(2));

    double expected_evaluation = 1 / (2 * M_PI);

    EXPECT_EQ(expected_evaluation, evaluation);
}

TEST(GaussianDistributionTest, VectorizedEvaluationTest) {
    GaussianDistribution distribution(2);

    Eigen::Vector2d evaluation =
        distribution.evaluatePdfVectorized(Eigen::MatrixXd::Zero(2, 2));

    Eigen::Vector2d expected_evaluation =
        Eigen::Vector2d::Constant(1 / (2 * M_PI));

    EXPECT_EQ(expected_evaluation, evaluation);
}

}  // namespace refill


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef NORMAL_FITNESS_METRIC_HPP_
#define NORMAL_FITNESS_METRIC_HPP_

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

#include "clotho/fitness/ifitness.hpp"

extern const std::string NORM_NAME;

/**
 * univariate normal distribution function
 *
 * Note: for k-dimensional trait vector the fitness is based on the phenotype of
 * only the first trait
 */
class normal_fitness_metric : public ifitness {
public:
    typedef double      real_type;
    typedef real_type   result_type;

    normal_fitness_metric( real_type mu = 0., real_type sigma = 1. );

    result_type operator()( double x );
    result_type operator()( float x ) { return operator()( (double) x ); }

    result_type operator()( double x, real_type mu, real_type sigma );
    result_type operator()( float x, real_type mu, real_type sigma ) { return operator()( (double) x, mu, sigma); }

    inline result_type operator()( const std::vector< double > & multi_variate ) {
        return ((multi_variate.empty()) ? operator()( 0. ) : operator()( multi_variate.front() ));
    }

    inline result_type operator()( const std::vector< float > & multi_variate ) {
        return ((multi_variate.empty()) ? operator()( 0. ) : operator()( multi_variate.front() ));
    }

    inline result_type operator()( const std::vector< double > & multi_variate, real_type mu, real_type sigma ) {
        return ((multi_variate.empty()) ? operator()( 0., mu, sigma ) : operator()( multi_variate.front(), mu, sigma ));
    }

    inline result_type operator()( const std::vector< float > & multi_variate, real_type mu, real_type sigma ) {
        return ((multi_variate.empty()) ? operator()( 0., mu, sigma ) : operator()( multi_variate.front(), mu, sigma ));
    }

    inline result_type operator()( double * first, double * last ) {
        return (first == last) ? operator()( 0. ) : operator()( *first );
    }

    inline result_type operator()( float * first, float * last ) {
        return (first == last) ? operator()( 0. ) : operator()( *first );
    }

    const std::string name() const;

    void log( std::ostream & out ) const;

    virtual ~normal_fitness_metric();

protected:
    real_type m_mean;
    real_type m_sigma;

    real_type _coeff, _denom;
};

#endif  // NORMAL_FITNESS_METRIC_HPP_


#include <stdio.h>
#include <omp.h>

#include <iostream>

#include <Eigen/Dense>
#include <Parareal/core.h>
#include <Parareal/backward_euler.h>

#include "rhs_linear1d.h"

typedef Eigen::VectorXd Evec;
typedef Eigen::MatrixXd Emat;

int main(int argc, char **argv)
{
  ode_system ode;
  ode.dimension = 1; ode.t_init = 0; ode.t_final = 1;
  ode.y0 = Evec(1); ode.y0(0) = 1;
  ode.f = std::function<int(double, Evec&, Evec&)>(&linear1d);
  ode.J = std::function<int(double, Evec&, Emat&)>(&linear1d_jac);

  time_stepper solver;

  // Backward Euler
  solver.dt = 0.1;
  solver.F = std::function<int(ode_system&, double, Evec &)>(&backward_euler);
  solver.F_allt = std::function<int(ode_system&, double, Emat &)>(&backward_euler_allt);

  int steps = ode.num_steps(solver.dt);
  double tt = 0.0;

  // Parareal Solver
  Emat yf(steps, ode.dimension);
  tt = omp_get_wtime();
  solver.integrate_allt(ode, yf);
  tt = omp_get_wtime() - tt;

  std::cout << yf << std::endl;
  printf("Time taken %f\n", tt);

  return 0;
}


#include <iostream>
#include <vector>
#include <unordered_map>
#include <fstream>
#include <boost/functional/hash.hpp>
#include <algorithm>
#include <utility>
#include <cassert>
#include <chrono>
#include <iomanip>

using namespace std;
using namespace std::chrono;

#define point pair<double, double>
#define inter pair<bool, point>
const double doublePrecision = 1e-6;
const double delta = 1.0;
const double upperDelta = 1.0;

static void print(point u) {
	cerr << setprecision(15) << u.first << " " << u.second << endl;
}

// Read pair of doubles.
static vector<point> readBinary(char* fileName, int limitSize) {
	ifstream input(fileName, ios::binary);
    auto pos = input.tellg();
    input.seekg( 0, ios::end );
    auto size=input.tellg()-pos;
    input.seekg(0,ios::beg);
/*
	size >>= 1;
	cerr <<"size = " << size << endl;
	int sizeToRead = limitSize == -1 ? size : (limitSize < size ? limitSize : size);
	vector<point> coord(sizeToRead);
	cerr << "sizeToRead = " << sizeToRead << endl;
	for (unsigned index = 0; index < sizeToRead; ++index) {
		double x, y;
		input.read(reinterpret_cast<char*>(&x), sizeof(double));
		input.read(reinterpret_cast<char*>(&y), sizeof(double));
		long double x0 = static_cast<long double>(x);
		long double y0 = static_cast<long double>(y);
		coord[index] = make_pair(x0, y0 + delta);
		if (index < 10)
			print(coord[index]);
	}
	return coord;
*/
    unsigned elements = size / sizeof(point);
    if (limitSize != -1)
    	elements = limitSize < elements ? limitSize : elements;
    vector<point> cdf;
    cdf.resize(elements);
    input.read(reinterpret_cast<char*>(cdf.data()), elements * sizeof(point));
    return cdf;
}

// Write the sample points in a binary file.
static void writeBinary(char* fileName, vector<point>& samplePoints) {
	ofstream output(fileName, ios::binary);
	for (point p: samplePoints) {
		double x = static_cast<double>(p.first);
		double y = static_cast<double>(p.second - delta);
		output.write(reinterpret_cast<char*>(&x), sizeof(double));
		output.write(reinterpret_cast<char*>(&y), sizeof(double));
	}
}

// Read from a .txt file.
static vector<point> readTxt(char* fileName) {
	ifstream input(fileName);
	vector<point> coord;
	double x, y;
	while (input >> x >> y) {
		long double x0 = static_cast<long double>(x);
		long double y0 = static_cast<long double>(y);
		coord.push_back(make_pair(x0, y0 + delta));
	}
	return coord;
}

// Write into a .txt file
static void writeTxt(char* fileName, vector<point>& samplePoints) {
	ofstream output(fileName);
	for (point p: samplePoints)
		output << p.first << " " << (p.second - delta) << "\n";
}

// Computes the position of the triple (u, v, w) in respect to pi.
static int angle(bool counterclockwise, point u, point v, point w) {
	double ret = ((v.first - u.first) * (w.second - u.second) - (v.second - u.second) * (w.first - u.first));
	return counterclockwise ? ret : -ret;
}

static double myabs(long double x) {
	return (x < -doublePrecision) ? -x : x;
}

// Intersection of the lines (u1, u2) and (v1, v2).
static inter intersection(point u1, point u2, point v1, point v2) {
	double a1 = u2.second - u1.second, b1 = u1.first - u2.first, c1 = a1 * u1.first + b1 * u1.second;
	double a2 = v2.second - v1.second, b2 = v1.first - v2.first, c2 = a2 * v1.first + b2 * v1.second;
	double det = a1 * b2 - a2 * b1;

	// compute with long double precision.
	if (abs(det) < doublePrecision) {
		cerr << "ba e determinantul zero!!! Ia sa vedem, totusi" << endl;
		print(u1);
		print(u2);
		print(v1);
		print(v2);
		return make_pair(false, u1);
	}
	return make_pair(true, make_pair((c1 * b2 - c2 * b1) / det, (a1 * c2 - a2 * c1) / det));
	/*if ((min(v1.first, v2.first) <= ret.first) && (ret.first <= max(v1.first, v2.first)) && (min(v1.second, v2.second) <= ret.second) && (ret.second <= max(v1.second, v2.second))) {
		return ret;
	} else {
		print(ret);
		print(u1);
		print(u2);
		print(v1);
		print(v2);
		//cerr << u1.firs << " "  << u2 << " " << v1 << " " << v2 << endl;
		assert(0);
	}*/
}

double globalEpsForChange;
double minusEpsForChange;

point change(bool sign, vector<point>& coord, unsigned index) {
    return sign ? make_pair(coord[index - 1].first, coord[index - 1].second * (1 + globalEpsForChange) + delta + upperDelta)
                : make_pair(coord[index - 1].first, coord[index - 1].second + delta);
}

// Computes the samples points.
vector<point> samplePointsOfApproximateFunction(vector<point>& coord, double eps) {
	unsigned n = coord.size();

    globalEpsForChange = eps;
    minusEpsForChange = 1e-2;

	cerr << "size of coord in funciton = " << n << endl;

	point *windowBound = new point[2], *leftBound = new point[2], *rightBound = new point[2];

	// lists of neighbours.
	unordered_map<point, point, boost::hash<point>> *rightList = new unordered_map<point, point, boost::hash<point>>[2];
	unordered_map<point, point, boost::hash<point>> *leftList = new unordered_map<point, point, boost::hash<point>>[2];

	//map<point, point> *rightList = new map<point, point>[2];
	//map<point, point> *leftList = new map<point, point>[2];

	cerr << "before alloc" << endl;

	vector<point> samplePoints;

	cerr << "wwas???" << endl;

	// Maybe we should reverse the order in points.
	/*for (unsigned index = 1; index <= n; ++index) {
		points[index][0] = make_pair(coord[index - 1].first, coord[index - 1].seconc * (1 - eps) + delta);
		points[index][1] = make_pair(coord[index - 1].first, coord[index - 1].second * (1 + eps) + delta + upperDelta);
	}*/

	cerr << "Umwandeln" << endl;

	for (int sign = 1; sign >= 0; --sign) {
        point p1 = change(sign, coord, 1), p2 = change(sign, coord, 2);

		windowBound[sign] = p1;//points[1][sign];
		leftBound[sign] = p1;//points[1][sign];
		rightBound[sign] = p1;//points[1][sign];

		rightList[sign][p1] = p2;//points[2][sign];
		leftList[sign][p2] = p1;//points[1][sign];
	}

	for (unsigned index = 3; index < n; ++index) {
		//cerr << "now at index = " << index << endl;
		bool nextWindow = false;
		// Updating convex hulls
		for (int sign = 1; sign >= 0; --sign) {
            point indexPoint = change(sign, coord, index);
			point currPoint = change(sign, coord, index - 1);

			while ((currPoint != windowBound[sign]) && (angle(sign, indexPoint, currPoint, leftList[sign][currPoint]) > doublePrecision))
				currPoint = leftList[sign][currPoint];
			rightList[sign][currPoint] = indexPoint; //[index][sign];
			leftList[sign][indexPoint] = currPoint;
		}

		for (int sign = 1; sign >= 0; --sign) {
			int star = sign, diamond = !sign;
            point indexPointStar = change(star, coord, index);
            point indexPointDiamond = change(diamond, coord, index);
            point lastIndexPoint = change(star, coord, index - 1);

			if ((!nextWindow) && (angle(star, indexPointStar, leftBound[star], rightBound[diamond]) < -doublePrecision)) {
				// Get new sample point.
				//cerr << "found sth!" << endl;
				inter value = intersection(leftBound[star], rightBound[diamond], windowBound[star], windowBound[diamond]);
				if (value.first == true) {
					samplePoints.push_back(value.second);
				} else {
					cerr << "For one sample point the determinant is 0!" << endl;
				}
				//cerr << "it's not him" << endl;

				// Update the window.
				windowBound[diamond] = rightBound[diamond];
				value = intersection(leftBound[star], rightBound[diamond], lastIndexPoint, indexPointStar);
				if (value.first == true)
					windowBound[star] = value.second;
				else
					assert(0);

				// Update the lists.
				rightList[star][windowBound[star]] = indexPointStar;
				leftList[star][indexPointStar] = windowBound[star];

				// Update rightBound
				rightBound[star] = indexPointStar;
				rightBound[diamond] = indexPointDiamond;

				// Update leftBound
				leftBound[star] = windowBound[star];
				leftBound[diamond] = windowBound[diamond];

				while (angle(diamond, leftBound[diamond], rightBound[star], rightList[diamond][leftBound[diamond]]) < -doublePrecision)
					leftBound[diamond] = rightList[diamond][leftBound[diamond]];
				nextWindow = true;
			}
		}

		// Updating the supports and separating lines.
		if (!nextWindow) {
			for (int sign = 1; sign >= 0; --sign) {
				int star = sign, diamond = !sign;
                point indexPoint = change(star, coord, index);

				if (angle(star, indexPoint, leftBound[diamond], rightBound[star]) < -doublePrecision) {
					rightBound[star] = indexPoint;
					while (angle(star, indexPoint, leftBound[diamond], rightList[diamond][leftBound[diamond]]) < -doublePrecision)
						leftBound[diamond] = rightList[diamond][leftBound[diamond]];
				}
			}
		}
	}

	// Compute the two last approximated points.
	// Quite weird here!
	cerr << "by now" << samplePoints.size() << endl;
	// I'm still confused if there is need for a condition here.
	cerr << "n-ter Eintrag = " << endl;
	//print(points[n][1]);
	//print(points[n][0]);
	cerr << "leftBound + rightBound + windowBound" << endl;
	for (int sign = 1; sign >= 0; --sign) {
		print(leftBound[sign]);
		print(rightBound[sign]);
		print(windowBound[sign]);
	}

	inter value = intersection(leftBound[0], rightBound[1], windowBound[1], windowBound[0]);
	if (value.first == true)
		samplePoints.push_back(value.second);
	else
		cerr << "Penultimul punct nu exista!" << endl;

	value = intersection(leftBound[0], rightBound[1], change(1, coord, n), change(0, coord, n));
	if (value.first == true)
		samplePoints.push_back(value.second);
	else
		cerr << "Ultimul punct nu exista!" << endl;
	return samplePoints;
}

static double interpolate(vector<point>& spline, double pos, bool dump) {
   if (pos <= spline.front().first)
      return spline.front().second;
   if (pos >= spline.back().first)
      return spline.back().second;

	auto iter=lower_bound(spline.begin(),spline.end(),pos,[](const point& a,double b) { return a.first<b; });

   if (dump) cerr << "(" << (iter-1)->first << "," << (iter-1)->second << ") - " << pos << " - (" << (iter+0)->first << "," << (iter+0)->second << ")" << endl;

   if (iter->first == pos)
      return iter->second;

   double dx = iter->first - (iter - 1)->first;
   double dy = iter->second - (iter - 1)->second;

   double ofs = pos - (iter - 1)->first;
   return (iter - 1)->second + ofs * (dy / dx);
}

/*
static void convertBack(vector<point>& func) {
	for (auto& e: func)
		e.second -= delta;
}
*/

static vector<point> compressFunc(vector<point>& func, unsigned desiredSize)
// Compress to the desired size
{
	// Relax a bit to speed up compression
	unsigned maxSize = desiredSize + (desiredSize / 100), minSize = desiredSize - (desiredSize / 100);

	cerr << maxSize << " and min = " << minSize << endl;

	// Fits?
	if (func.size()<=maxSize)
		return func;

	// No, binary search
	long long capacity = 1e9, left = 1, right = capacity;
	double mul = 1.0 / capacity;
	//long double last = 0;
	while (left < right) {
		long long middle=(left+right)/2;

		double epsilon = mul * middle;
				cerr << "------------- Binary search for " << epsilon << endl;

		//if (abs(epsilon - last) < doublePrecision)return

		if (myabs(epsilon - 1) < doublePrecision) {
			return samplePointsOfApproximateFunction(func, epsilon);
		}
		if (myabs(epsilon - capacity) < doublePrecision) {
			return samplePointsOfApproximateFunction(func, epsilon);
		}

		vector<point> candidate = samplePointsOfApproximateFunction(func, epsilon);

		cerr << "done sample" << endl;
		cerr << "------------- Binary search for " << epsilon << " has " << candidate.size() << endl;

		cerr << candidate.size() << " " << minSize << " " << maxSize << endl;

		if (candidate.size() < minSize) {
	 		right = middle;
		} else if (candidate.size()>maxSize) {
	 		left = middle;
		} else {
			cerr << "Found already in binary search = " << middle << " and epsilon " << mul * middle << endl;
			//convertBack(candidate);
	 		return candidate;
		}
	}

	cerr << "At the end is left = " << left << " and so epsilon " << (mul * left) << endl;

   // Final call, this is the best we could get
   vector<point> candidate = samplePointsOfApproximateFunction(func, mul * left);
   //convertBack(candidate);
   return candidate;
}

int mainn(int argc, char** argv) {
	if (argc != 7) {
		cout << "Usage: ./a.out inputVariant(1 -> binary, 0 -> txt) inputFileName outputVariant(1 -> binary, 0 -> txt) outputFileName howManySupportingPoints howManyToRead\n";
		return 1;
	}
	int inputVariant = atoi(argv[1]);
	char* inputFileName = argv[2];
	int outputVariant = atoi(argv[3]);
	char* outputFileName = argv[4];
	int countSupportingPoints = atoi(argv[5]);
	int countToRead = atoi(argv[6]);

	cerr << "Begin of reading" << endl;
	auto readingStart = high_resolution_clock::now();

	vector<point> coord;
	if (inputVariant == 1)
		coord = readBinary(inputFileName, countToRead);
	else
		coord = readTxt(inputFileName);

	auto readingStop = high_resolution_clock::now();
	cerr << "Reading done!" << endl;
	/*
	if (useSort) {
		cerr << "Begin of sort" << endl;
		cerr << "wieder" << coord.size() << endl;
		auto startSort = high_resolution_clock::now();

		sort(coord.begin(), coord.end());

		auto sortStop = high_resolution_clock::now();
		cerr << "Sort done!" << endl;

		cerr << "wieder" << coord.size() << endl;

		//writeBinary(outputFileName, coord);
		//return 0;
	}
	*/
	cerr << "test" << coord[0].first << " " << coord[0].second << endl;
	cerr << "test" << coord[1].first << " " << coord[1].second << endl;

	cerr << "Begin of the algorithm" << endl;
	auto startAlg = high_resolution_clock::now();

	cerr << "count = " << countSupportingPoints << endl;
/*
	unsigned n = coord.size();
	points = new point*[n + 1];
	for (unsigned index = 1; index <= n; ++index) {
		points[index] = new point[2];
	}
*/
	//vector<point> samplePoints = compressFunc(coord, countSupportingPoints);
	vector<point> samplePoints = samplePointsOfApproximateFunction(coord, 1e-15);

	cerr << "End of the algorithm" << endl;
	auto algStop = high_resolution_clock::now();

	cerr << "Begin of writing" << endl;
	auto startWriting = high_resolution_clock::now();
/*
	if (outputVariant == 1)
		writeBinary(outputFileName, samplePoints);
	else
		writeTxt(outputFileName, samplePoints);

	auto writingDone = high_resolution_clock::now();
*/
	if (samplePoints.size() == 0) {
		cerr << "alles falsch" << endl;
		return 0;
	}
	  double sumError = 0, maxError = 0;
	  for (auto e: coord) {
          // TODO: I have put here a + delta! Don't forget that!
	     double estimate = interpolate(samplePoints, e.first, false);
	     double real = e.second + delta;
	     double diff = estimate-real;
	     if (diff<0)
	     	diff=-diff;
	     if (diff>maxError)
	        maxError=diff;
	     sumError+=diff;
	  }
	  cerr << samplePoints.size() << " " << maxError << " " << (sumError / coord.size()) << endl;

	//auto duration = duration_cast<milliseconds>(writingDone - readingStart);
	//cerr << "Total time is = " << duration.count() << endl;
	return 0;
}


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

#include "theia/sfm/estimators/estimate_triangulation.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>

#include "theia/solvers/estimator.h"
#include "theia/solvers/ransac.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/types.h"

namespace theia {

namespace {
// The pixel observation and projection matrix needed in order to triangulate a
// 3D point.
struct PointObservation {
  Matrix3x4d projection_matrix;
  Eigen::Vector2d feature;
};

// Returns true if the point is in front of the camera and false if the point is
// behind the camera.
bool IsPointInFrontOfCamera(const Matrix3x4d& projection_matrix,
                            const Eigen::Vector4d& point) {
  return point.dot(projection_matrix.row(2)) > 0;
}

class TriangulationEstimator
    : public Estimator<PointObservation, Eigen::Vector4d> {
 public:
  TriangulationEstimator() {}

  double SampleSize() const { return 2; }

  // Triangulates the 3D point from 2 observations.
  bool EstimateModel(const std::vector<PointObservation>& observations,
                     std::vector<Eigen::Vector4d>* triangulated_points) const {
    // TODO(cmsweeney): We do not check the angle between the two views at the
    // moment. This requires the ray direction of each feature meaning we would
    // have to either decompose the projection matrix or pass in the ray
    // direction as part of the Point Observation. RANSAC should be good enough
    // at filtering out these bad solutions so we ignore this for now.
    triangulated_points->resize(1);
    if (!Triangulate(observations[0].projection_matrix,
                     observations[1].projection_matrix,
                     observations[0].feature,
                     observations[1].feature,
                     &triangulated_points->at(0))) {
      return false;
    }
    // Only return true if the point is in front of both cameras and the
    // triangulation was a success.
    return IsPointInFrontOfCamera(observations[0].projection_matrix,
                                  triangulated_points->at(0)) &&
        IsPointInFrontOfCamera(observations[1].projection_matrix,
                               triangulated_points->at(0));
  }

  double Error(const PointObservation& observation,
               const Eigen::Vector4d& triangulated_point) const {
    if (!IsPointInFrontOfCamera(observation.projection_matrix,
                                triangulated_point)) {
      return std::numeric_limits<double>::max();
    }

    const Eigen::Vector2d reprojection =
        (observation.projection_matrix * triangulated_point).hnormalized();
    return (observation.feature - reprojection).squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(TriangulationEstimator);
};

}  // namespace

bool EstimateTriangulation(const RansacParameters& ransac_params,
                           const std::vector<Matrix3x4d>& projection_matrices,
                           const std::vector<Eigen::Vector2d>& features,
                           Eigen::Vector4d* triangulated_point,
                           RansacSummary* summary) {
  if (projection_matrices.size() < 2) {
    return false;
  }

  // Create point correspondences.
  std::vector<PointObservation> point_observations(
      projection_matrices.size());
  for (int i = 0; i < point_observations.size(); i++) {
    point_observations[i].projection_matrix = projection_matrices[i];
    point_observations[i].feature = features[i];
  }

  // RANSAC triangulation.
  TriangulationEstimator triangulation_estimator;
  Ransac<TriangulationEstimator> ransac(ransac_params,
                                        triangulation_estimator);
  CHECK(ransac.Initialize());
  if (!ransac.Estimate(point_observations, triangulated_point, summary)) {
    return false;
  }

  return true;
}

}  // namespace theia


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include<mitkConnectomicsShortestPathHistogram.h>

#include <boost/graph/dijkstra_shortest_paths.hpp>

#include "mitkConnectomicsConstantsManager.h"

mitk::ConnectomicsShortestPathHistogram::ConnectomicsShortestPathHistogram()
: m_Mode( UnweightedUndirectedMode )
, m_EverythingConnected( true )
{
  m_Subject = "Shortest path";
}

mitk::ConnectomicsShortestPathHistogram::~ConnectomicsShortestPathHistogram()
{
}

void mitk::ConnectomicsShortestPathHistogram::SetShortestPathCalculationMode( const mitk::ConnectomicsShortestPathHistogram::ShortestPathCalculationMode & mode)
{
  m_Mode = mode;
}

mitk::ConnectomicsShortestPathHistogram::ShortestPathCalculationMode mitk::ConnectomicsShortestPathHistogram::GetShortestPathCalculationMode()
{
  return m_Mode;
}

void mitk::ConnectomicsShortestPathHistogram::ComputeFromConnectomicsNetwork( ConnectomicsNetwork* source )
{

  NetworkType* boostGraph = source->GetBoostGraph();

  switch( m_Mode )
  {
  case UnweightedUndirectedMode:
    {
      CalculateUnweightedUndirectedShortestPaths( boostGraph );
      break;
    }
  case WeightedUndirectedMode:
    {
      CalculateWeightedUndirectedShortestPaths( boostGraph );
      break;
    }
  }

    ConvertDistanceMapToHistogram();
}

void mitk::ConnectomicsShortestPathHistogram::CalculateUnweightedUndirectedShortestPaths( NetworkType* boostGraph )
{
  std::vector< DescriptorType > predecessorMap( boost::num_vertices( *boostGraph ) );
  int numberOfNodes( boost::num_vertices( *boostGraph ) );

  m_DistanceMatrix.resize( numberOfNodes );
  for( int index(0); index < m_DistanceMatrix.size(); index++ )
  {
    m_DistanceMatrix[ index ].resize( numberOfNodes );
  }

  IteratorType iterator, end;
  boost::tie(iterator, end) = boost::vertices( *boostGraph );

  for ( int index(0) ; iterator != end; ++iterator, index++)
  {
          boost::dijkstra_shortest_paths(*boostGraph, *iterator, boost::predecessor_map(&predecessorMap[ 0 ]).distance_map(&m_DistanceMatrix[ index ][ 0 ]).weight_map( boost::get( &mitk::ConnectomicsNetwork::NetworkEdge::edge_weight ,*boostGraph ) ) ) ;
  }
}

void mitk::ConnectomicsShortestPathHistogram::CalculateWeightedUndirectedShortestPaths( NetworkType* boostGraph )
{
  MBI_WARN << mitk::ConnectomicsConstantsManager::CONNECTOMICS_WARNING_UNIMPLEMENTED_FEATURE;
}

void mitk::ConnectomicsShortestPathHistogram::ConvertDistanceMapToHistogram()
{
  // get the longest path between any two nodes in the network
  // we assume that no nodes are farther apart than there are nodes,
  // this is to filter unconnected nodes
  int longestPath( 0 );
  int numberOfNodes( m_DistanceMatrix.size() );
  m_EverythingConnected = true;

  for( int index(0); index < m_DistanceMatrix.size(); index++ )
  {
    for( int innerIndex(0); innerIndex < m_DistanceMatrix[ index ].size(); innerIndex++ )
    {
      if( m_DistanceMatrix[ index ][ innerIndex ] > longestPath )
      {
        if( m_DistanceMatrix[ index ][ innerIndex ] < numberOfNodes )
        {
          longestPath = m_DistanceMatrix[ index ][ innerIndex ];
        }
        else
        {
          // these nodes are not connected
          m_EverythingConnected = false;
        }
      }
    }
  }

  m_HistogramVector.resize( longestPath + 1 );

  for( int index(0); index < m_DistanceMatrix.size(); index++ )
  {
    for( int innerIndex(0); innerIndex < m_DistanceMatrix[ index ].size(); innerIndex++ )
    {
      if( m_DistanceMatrix[ index ][ innerIndex ] < numberOfNodes )
      {
        m_HistogramVector[ m_DistanceMatrix[ index ][ innerIndex ] ]++;
      }
    }
  }

  // correct for every path being counted twice

  for( int index(1); index < m_HistogramVector.size(); index++ )
  {
    m_HistogramVector[ index ] = m_HistogramVector[ index ] / 2;
  }

    // correct for every node being distance zero to itself
  if( m_HistogramVector[ 0 ] >= numberOfNodes )
  {
    m_HistogramVector[ 0 ] = m_HistogramVector[ 0 ] - numberOfNodes;
  }
  else
  {
    MBI_WARN << mitk::ConnectomicsConstantsManager::CONNECTOMICS_WARNING_ZERO_DISTANCE_NODES;
  }

  UpdateYMax();

  this->m_Valid = true;
}

double mitk::ConnectomicsShortestPathHistogram::GetEfficiency()
{
  if( !this->m_Valid )
  {
    MBI_INFO << mitk::ConnectomicsConstantsManager::CONNECTOMICS_WARNING_CAN_NOT_COMPUTE_EFFICIENCY << mitk::ConnectomicsConstantsManager::CONNECTOMICS_WARNING_NETWORK_NOT_VALID;
    return 0.0;
  }

  if( !m_EverythingConnected )
  { // efficiency of disconnected graphs is 0
        MBI_INFO << mitk::ConnectomicsConstantsManager::CONNECTOMICS_WARNING_NETWORK_DISCONNECTED;
    return 0.0;
  }

  double efficiency( 0.0 );

  double overallDistance( 0.0 );
  double numberOfPairs( 0.0 );
  // add up all distances
  for( int index(0); index < m_HistogramVector.size(); index++ )
  {
    overallDistance = overallDistance + m_HistogramVector[ index ] * index;
    numberOfPairs = numberOfPairs + m_HistogramVector[ index ];
  }

  // efficiency = 1 / averageDistance = 1 / ( overallDistance / numberofPairs )
  efficiency = numberOfPairs / overallDistance;

  return efficiency;
}


/*
 * Copyright 2017-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#define BOOST_TEST_MODULE TestSpatialFilters
#include <boost/test/unit_test.hpp>
#include <gram_savitzky_golay/spatial_filters.h>

#ifndef M_PI
#  include <boost/math/constants/constants.hpp>
#  define M_PI boost::math::constants::pi<double>()
#endif

using namespace gram_sg;

BOOST_AUTO_TEST_CASE(test_transform_filter)
{
  /** Filtering **/
  {
    gram_sg::SavitzkyGolayFilterConfig sg_conf(50, 50, 2, 0);
    TransformFilter filter(sg_conf);

    Eigen::Matrix3d rot_sg;
    rot_sg = Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(1.9, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(2.3, Eigen::Vector3d::UnitZ());
    Eigen::Affine3d X_init = Eigen::Affine3d::Identity();
    X_init.matrix().block<3, 3>(0, 0) = rot_sg;
    X_init.matrix().block<3, 1>(0, 3) = Eigen::Vector3d{0.1, 5, 0.3};
    filter.reset(X_init);
    const auto & res_init = filter.filter();
    BOOST_CHECK_MESSAGE(X_init.matrix().isApprox(res_init.matrix()), "\n" << X_init.matrix() << "\n-----\n"
                                                                          << res_init.matrix());
  }

  {
    gram_sg::SavitzkyGolayFilterConfig sg_conf(50, 50, 2, 0);
    TransformFilter filter(sg_conf);
    Eigen::Matrix3d rot_sg;
    rot_sg = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ());
    Eigen::Affine3d X_init = Eigen::Affine3d::Identity();
    X_init.matrix().block<3, 3>(0, 0) = rot_sg;
    X_init.matrix().block<3, 1>(0, 3) = Eigen::Vector3d{0.1, 5, 0.3};
    filter.reset(X_init);

    const auto & res_init = filter.filter();
    BOOST_CHECK_MESSAGE(X_init.matrix().isApprox(res_init.matrix()), "\n" << X_init.matrix() << "\n-----\n"
                                                                          << res_init.matrix());
  }
}

BOOST_AUTO_TEST_CASE(test_rotation_filter)
{
  Eigen::Matrix3d rot;
  rot = Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ());

  gram_sg::SavitzkyGolayFilterConfig sg_conf(50, 50, 2, 0);
  RotationFilter filter(sg_conf);

  filter.reset(rot);

  const Eigen::Matrix3d res = filter.filter();
  BOOST_CHECK(rot.isApprox(res));
}


#define CATCH_CONFIG_MAIN
#include <Eigen/Dense>
#include <catch.hpp>
#include <memory>
#include <random>

#include "EDP/ConstructSparseMat.hpp"
#include "EDP/LocalHamiltonian.hpp"

#include "yavque/Operators/DiagonalHamEvol.hpp"
#include "yavque/utils.hpp"

#include "common.hpp"

TEST_CASE("test random diagonal", "[random-diagonal]")
{
	constexpr uint32_t N = 8;
	constexpr yavque::cx_double I(0., 1.);
	std::random_device rd;
	std::default_random_engine re{rd()};

	std::normal_distribution<> nd;

	Eigen::VectorXd ham = Eigen::VectorXd::Random(1 << N);
	yavque::DiagonalOperator diag_op(ham);
	yavque::DiagonalHamEvol diag_ham_evol(diag_op);

	for(uint32_t k = 0; k < 100; ++k) // instance
	{
		Eigen::VectorXcd vec = Eigen::VectorXcd::Random(1 << N);
		vec.normalize();

		double t = nd(re);
		diag_ham_evol.set_variable_value(t);

		Eigen::VectorXcd out1 = diag_ham_evol.apply_right(vec);
		Eigen::VectorXcd out2 = exp(-I * ham.array() * t) * vec.array();

		REQUIRE((out1 - out2).norm() < 1e-6);

		Eigen::VectorXcd grad1 = diag_ham_evol.log_deriv()->apply_right(out1);
		Eigen::VectorXcd grad2 = -I * ham.cwiseProduct(out1);
		REQUIRE((grad1 - grad2).norm() < 1e-6);
	}

	diag_ham_evol.dagger_in_place();
	for(uint32_t k = 0; k < 100; ++k) // instance
	{
		Eigen::VectorXcd vec = Eigen::VectorXcd::Random(1 << N);
		vec.normalize();

		double t = nd(re);
		diag_ham_evol.set_variable_value(t);

		Eigen::VectorXcd out1 = diag_ham_evol.apply_right(vec);
		Eigen::VectorXcd out2 = exp(I * ham.array() * t) * vec.array();

		REQUIRE((out1 - out2).norm() < 1e-6);
		Eigen::VectorXcd grad1 = diag_ham_evol.log_deriv()->apply_right(out1);
		Eigen::VectorXcd grad2 = I * ham.cwiseProduct(out1);
		REQUIRE((grad1 - grad2).norm() < 1e-6);
	}
}

TEST_CASE("test basic operations", "[basic-operation]")
{
	constexpr uint32_t N = 8;
	std::random_device rd;
	std::default_random_engine re{rd()};

	std::normal_distribution<> nd;

	Eigen::VectorXd ham = Eigen::VectorXd::Random(1 << N);
	yavque::DiagonalOperator diag_op(ham);
	yavque::DiagonalHamEvol diag_ham_evol(diag_op);

	diag_ham_evol.set_variable_value(1.0);

	auto clonned = diag_ham_evol.clone();

	std::cout << clonned->desc() << std::endl;
	auto* p = dynamic_cast<yavque::Univariate*>(clonned.get());
	p->set_variable_value(-1.0);

	REQUIRE(diag_ham_evol.get_variable_value() == 1.0);
}


///////////////////////////////////////////////////////////////
//  Copyright 2019 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include "../performance_test.hpp"
#if defined(TEST_CPP_INT)
#include <boost/multiprecision/cpp_int.hpp>
#endif

void test11()
{
#ifdef TEST_CPP_INT
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<512, 512, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(fixed)", 512);
#endif
}


/**
 * @file random.cpp
 *
 * Declarations of global Boost random number generators.
 *
 * This file is part of mlpack 1.0.12.
 *
 * mlpack is free software; you may redstribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <boost/random.hpp>
#include <boost/version.hpp>

namespace mips {
namespace math {

#if BOOST_VERSION >= 104700
  // Global random object.
  boost::random::mt19937 randGen;
  // Global uniform distribution.
  boost::random::uniform_01<> randUniformDist;
  // Global normal distribution.
  boost::random::normal_distribution<> randNormalDist;
#else
  // Global random object.
  boost::mt19937 randGen;

  #if BOOST_VERSION >= 103900
    // Global uniform distribution.
    boost::uniform_01<> randUniformDist;
  #else
    // Pre-1.39 Boost.Random did not give default template parameter values.
    boost::uniform_01<boost::mt19937, double> randUniformDist(randGen);
  #endif

  // Global normal distribution.
  boost::normal_distribution<> randNormalDist;
#endif

}; // namespace math
}; // namespace mlpack


/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <boost/intrusive_ptr.hpp>

#include "formula.hpp"
#include "formula_callable.hpp"
#include "unit_test.hpp"

namespace {
using namespace game_logic;
class mock_char : public formula_callable {
	variant get_value(const std::string& key) const {
		if(key == "strength") {
			return variant(15);
		} else if(key == "agility") {
			return variant(12);
		}

		return variant(10);
	}
};
class mock_party : public formula_callable {
	variant get_value(const std::string& key) const {
		c_.add_ref();
		i_[0].add_ref();
		i_[1].add_ref();
		i_[2].add_ref();
		if(key == "members") {
			i_[0].add("strength",variant(12));
			i_[1].add("strength",variant(16));
			i_[2].add("strength",variant(14));
			std::vector<variant> members;
			for(int n = 0; n != 3; ++n) {
				members.push_back(variant(&i_[n]));
			}

			return variant(&members);
		} else if(key == "char") {
			return variant(&c_);
		} else {
			return variant(0);
		}
	}

	mock_char c_;
	mutable map_formula_callable i_[3];

};

}

UNIT_TEST(formula)
{
	boost::intrusive_ptr<mock_char> cp(new mock_char);
	boost::intrusive_ptr<mock_party> pp(new mock_party);
#define FML(a) formula(variant(a))
	mock_char& c = *cp;
	mock_party& p = *pp;
	CHECK_EQ(FML("strength").execute(c).as_int(), 15);
	CHECK_EQ(FML("17").execute(c).as_int(), 17);
	CHECK_EQ(FML("strength/2 + agility").execute(c).as_int(), 19);
	CHECK_EQ(FML("(strength+agility)/2").execute(c).as_int(), 13);
	CHECK_EQ(FML("strength > 12").execute(c).as_int(), 1);
	CHECK_EQ(FML("strength > 18").execute(c).as_int(), 0);
	CHECK_EQ(FML("if(strength > 12, 7, 2)").execute(c).as_int(), 7);
	CHECK_EQ(FML("if(strength > 18, 7, 2)").execute(c).as_int(), 2);
	CHECK_EQ(FML("2 and 1").execute(c).as_int(), 1);
	CHECK_EQ(FML("2 and 0").execute(c).as_int(), 0);
	CHECK_EQ(FML("2 or 0").execute(c).as_int(), 2);
	CHECK_EQ(FML("-5").execute(c).as_int(),-5);
	CHECK_EQ(FML("not 5").execute(c).as_int(), 0);
	CHECK_EQ(FML("not 0").execute(c).as_int(), 1);
	CHECK_EQ(FML("abs(5)").execute(c).as_int(), 5);
	CHECK_EQ(FML("abs(-5)").execute(c).as_int(), 5);
	CHECK_EQ(FML("sign(5)").execute(c).as_int(), 1);
	CHECK_EQ(FML("sign(0)").execute(c).as_int(), 0);
	CHECK_EQ(FML("sign(-5)").execute(c).as_int(), -1);
	CHECK_EQ(FML("min(3,5)").execute(c).as_int(), 3);
	CHECK_EQ(FML("min(5,2)").execute(c).as_int(), 2);
	CHECK_EQ(FML("max(3,5)").execute(c).as_int(), 5);
	CHECK_EQ(FML("max(5,2)").execute(c).as_int(), 5);
	CHECK_EQ(FML("char.strength").execute(p).as_int(), 15);
	CHECK_EQ(FML("choose(members,value.strength).strength").execute(p).as_int(), 16);
	CHECK_EQ(FML("4^2").execute().as_int(), 16);
	CHECK_EQ(FML("2+3^3").execute().as_int(), 29);
	CHECK_EQ(FML("2*3^3+2").execute().as_int(), 56);
	CHECK_EQ(FML("9^3").execute().as_int(), 729);
	CHECK_EQ(FML("x*5 where x=1").execute().as_int(), 5);
	CHECK_EQ(FML("x*(a*b where a=2,b=1) where x=5").execute().as_int(), 10);
	CHECK_EQ(FML("char.strength * ability where ability=3").execute(p).as_int(), 45);
	CHECK_EQ(FML("'abcd' = 'abcd'").execute(p).as_bool(), true);
	CHECK_EQ(FML("'abcd' = 'acd'").execute(p).as_bool(), false);
	CHECK_EQ(FML("~strength, agility: ${strength}, ${agility}~").execute(c).as_string(),
	               "strength, agility: 15, 12");
	for(int n = 0; n != 128; ++n) {
		const int dice_roll = FML("3d6").execute().as_int();
		CHECK_GE(dice_roll, 3);
   		CHECK_LE(dice_roll, 18);
	}

	variant myarray = FML("[1,2,3]").execute();
	CHECK_EQ(myarray.num_elements(), 3);
	CHECK_EQ(myarray[0].as_int(), 1);
	CHECK_EQ(myarray[1].as_int(), 2);
	CHECK_EQ(myarray[2].as_int(), 3);

}

BENCHMARK(construct_int_variant)
{
	BENCHMARK_LOOP {
		variant v(0);
	}
}

BENCHMARK_ARG(formula, const std::string& fm)
{
	static mock_party p;
	formula f = formula(variant(fm));
	BENCHMARK_LOOP {
		f.execute(p);
	}
}

BENCHMARK_ARG_CALL(formula, integer, "0");
BENCHMARK_ARG_CALL(formula, where, "x where x = 5");
BENCHMARK_ARG_CALL(formula, add, "5 + 4");
BENCHMARK_ARG_CALL(formula, arithmetic, "(5 + 4)*17 + 12*9 - 5/2");
BENCHMARK_ARG_CALL(formula, read_input, "char");
BENCHMARK_ARG_CALL(formula, read_input_sub, "char.strength");
BENCHMARK_ARG_CALL(formula, array, "[4, 5, 8, 12, 17, 0, 19]");
BENCHMARK_ARG_CALL(formula, array_str, "['stand', 'walk', 'run', 'jump']");
BENCHMARK_ARG_CALL(formula, string, "'blah'");
BENCHMARK_ARG_CALL(formula, null_function, "null()");
BENCHMARK_ARG_CALL(formula, if_function, "if(4 > 5, 7, 8)");


///////////////////////////////////////////////////////////////
//  Copyright 2012-2021 John Maddock.
//  Copyright 2021 Matt Borland. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/math/special_functions/prime.hpp>
#include <boost/random.hpp>
#include <random>
#include <iostream>
#include <iomanip>
#include "test.hpp"

#define BOOST_MP_STANDALONE
#include <boost/multiprecision/miller_rabin.hpp>

template <class I>
void test()
{
   //
   // Very simple test program to verify that the GMP's Miller-Rabin
   // implementation and this one agree on whether some random numbers
   // are prime or not.  Of course these are probabilistic tests so there's
   // no reason why they should actually agree - except the probability of
   // disagreement for 25 trials is almost infinitely small.
   //
   using namespace boost::random;
   using namespace boost::multiprecision;

   typedef I test_type;

   static const unsigned test_bits =
      std::numeric_limits<test_type>::digits && (std::numeric_limits<test_type>::digits <= 256)
         ? std::numeric_limits<test_type>::digits
         : 128;

   independent_bits_engine<mt11213b, test_bits, test_type> gen;
   //
   // We must use a different generator for the tests and number generation, otherwise
   // we get false positives.  Further we use the same random number engine for the
   // Miller Rabin test as GMP uses internally:
   //
   mt19937 gen2;

   //
   // Begin by testing the primes in our table as all these should return true:
   //
   for (unsigned i = 1; i < boost::math::max_prime; ++i)
   {
      BOOST_TEST(miller_rabin_test(test_type(boost::math::prime(i)), 25, gen));
      BOOST_TEST(mpz_probab_prime_p(mpz_int(boost::math::prime(i)).backend().data(), 25));
   }
   //
   // Now test some random values and compare GMP's native routine with ours.
   //
   for (unsigned i = 0; i < 10000; ++i)
   {
      test_type n              = gen();
      bool      is_prime_boost = miller_rabin_test(n, 25, gen2);
      bool      is_gmp_prime   = mpz_probab_prime_p(mpz_int(n).backend().data(), 25) ? true : false;
      if (is_prime_boost && is_gmp_prime)
      {
         std::cout << "We have a prime: " << std::hex << std::showbase << n << std::endl;
      }
      if (is_prime_boost != is_gmp_prime)
         std::cout << std::hex << std::showbase << "n = " << n << std::endl;
      BOOST_CHECK_EQUAL(is_prime_boost, is_gmp_prime);
   }
}

int main()
{
   using namespace boost::multiprecision;

   test<mpz_int>();
   test<number<gmp_int, et_off> >();
   test<std::uint64_t>();
   test<std::uint32_t>();

   test<cpp_int>();
   test<number<cpp_int_backend<64, 64, unsigned_magnitude, checked, void>, et_off> >();
   test<checked_uint128_t>();
   test<checked_uint1024_t>();

   return boost::report_errors();
}