#ifndef IMAGE_REPROJECTION_PLUGINS_MESH_SURFACE_MODEL_HPP
#define IMAGE_REPROJECTION_PLUGINS_MESH_SURFACE_MODEL_HPP

#include <string>
#include <vector>

#include <image_reprojection/surface_model.hpp>
#include <image_reprojection_plugins/MeshStamped.h>
#include <topic_tools/shape_shifter.h>

#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <opencv2/core/core.hpp>

namespace image_reprojection_plugins {

class MeshSurfaceModel : public image_reprojection::SurfaceModel {
public:
  MeshSurfaceModel() {}

  virtual ~MeshSurfaceModel() {}

  virtual void update(const topic_tools::ShapeShifter &surface) override {
    const MeshStampedConstPtr mesh = surface.instantiate<MeshStamped>();
    CV_Assert(mesh);
    update(*mesh);
  }

  void update(const MeshStamped &mesh) {
    boost::unique_lock<boost::shared_mutex> write_lock(mutex_);

    frame_id_ = mesh.header.frame_id;

    vertices_.clear();
    for (const geometry_msgs::Point &v : mesh.mesh.vertices) {
      vertices_.push_back(cv::Vec3f(v.x, v.y, v.z));
    }

    triangles_.clear();
    for (const shape_msgs::MeshTriangle &t : mesh.mesh.triangles) {
      triangles_.push_back(
          cv::Vec3i(t.vertex_indices[0], t.vertex_indices[1], t.vertex_indices[2]));
    }
  }

  virtual std::string getFrameId() const override {
    boost::shared_lock<boost::shared_mutex> read_lock(mutex_);
    return frame_id_;
  }

private:
  virtual void onInit() override {}

  virtual void onIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                              cv::Mat &dst, cv::Mat &mask) const override {
    boost::shared_lock<boost::shared_mutex> read_lock(mutex_);
    multirayMeshIntersection(src_origin, src_direction, dst, mask);
  }

  void multirayMeshIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                                cv::Mat &dst, cv::Mat &mask) const {
    dst.create(src_direction.size(), CV_32FC3);
    mask.forEach<uchar>([this, &src_origin, &src_direction, &dst](uchar &m, const int *const pos) {
      if (m != 0) {
        const cv::Vec3f &sd = *src_direction.ptr<cv::Vec3f>(pos[0], pos[1]);
        cv::Vec3f &d = *dst.ptr<cv::Vec3f>(pos[0], pos[1]);
        m = rayMeshIntersection(src_origin, sd, d) ? 1 : 0;
      }
    });
  }

  bool rayMeshIntersection(const cv::Vec3f &src_origin, const cv::Vec3f &src_direction,
                           cv::Vec3f &dst) const {
    bool intersects = false;
    double min_distance;
    for (const cv::Vec3i &triangle : triangles_) {
      cv::Vec3f this_intersection;
      if (rayTriangleIntersection(src_origin, src_direction, triangle, this_intersection)) {
        const double this_distance = cv::norm(src_origin, this_intersection);
        if (!intersects || (intersects && this_distance < min_distance)) {
          intersects = true;
          min_distance = this_distance;
          dst = this_intersection;
        }
      }
    }

    return intersects;
  }

  bool rayTriangleIntersection(const cv::Vec3f &src_origin, const cv::Vec3f &src_direction,
                               const cv::Vec3i &triangle, cv::Vec3f &dst) const {
    // primitive vectors with respect to 0th vertex of triangle
    const cv::Vec3f r = src_origin - vertices_[triangle[0]];              // position of ray origin
    const cv::Vec3f e1 = vertices_[triangle[1]] - vertices_[triangle[0]]; // 1st edge of triangle
    const cv::Vec3f e2 = vertices_[triangle[2]] - vertices_[triangle[0]]; // 2nd edge of triangle
    const cv::Vec3f n = e1.cross(e2);                                     // normal of triangle

    // intersection point (p) can be described as
    //    p = r + t * d = b1 * e1 + b2 * e2
    //    (t >= 0, b1 >= 0, b2 >= 0, b1 + b2 <= 1)
    // where d is ray direction

    // calculate dot(d, n) for convenience
    const double ddn = src_direction.dot(n);
    if (ddn == 0.) { // means ray and triangle are parallel
      return false;
    }
    const double ddn_sign = ddn > 0. ? 1. : -1.;
    const double ddn_norm = ddn > 0. ? ddn : -ddn;

    // t = -dot(r, n) / dot(d, n)
    const double srdn = ddn_sign * r.dot(n);
    if (srdn > 0.) { // means t < 0
      return false;
    }

    // b1 = dot(d, cross(r, e2)) / dot(d, n)
    const double sddrce2 = ddn_sign * src_direction.dot(r.cross(e2));
    if (sddrce2 < 0. || sddrce2 > ddn_norm) { // means b1 < 0 or b1 > 1
      return false;
    }

    // b2 = dot(d, cross(e1, r)) / dot(d, n)
    const double sdde1cr = ddn_sign * src_direction.dot(e1.cross(r));
    if (sdde1cr < 0. || sdde1cr > ddn_norm) { // means b2 < 0 or b2 > 1
      return false;
    }

    if (sddrce2 + sdde1cr > ddn_norm) { // means b1 + b2 > 1
      return false;
    }

    // intersection point with respect to mesh coordinate frame
    dst = src_origin - srdn * src_direction / ddn_norm;
    return true;
  }

private:
  mutable boost::shared_mutex mutex_;
  std::string frame_id_;
  std::vector<cv::Vec3f> vertices_;
  std::vector<cv::Vec3i> triangles_;
};

} // namespace image_reprojection_plugins

#endif


//
//  CHOLMODSolver.hpp
//  OptCuts
//
//  Created by Minchen Li on 6/22/18.
//

#ifndef CHOLMODSolver_hpp
#define CHOLMODSolver_hpp

#include "LinSysSolver.hpp"

#include "cholmod.h"

#include <Eigen/Eigen>

#include <vector>
#include <set>

namespace OptCuts {

    template <typename vectorTypeI, typename vectorTypeS>
    class CHOLMODSolver : public LinSysSolver<vectorTypeI, vectorTypeS>
    {
        typedef LinSysSolver<vectorTypeI, vectorTypeS> Base;

    protected:
        cholmod_common cm;
        cholmod_sparse *A;
        cholmod_factor *L;
        cholmod_dense *b;

    public:
        CHOLMODSolver(void);
        ~CHOLMODSolver(void);

        void set_type(int threadAmt, int _mtype, bool is_upper_half = false);

        void set_pattern(const std::vector<std::set<int>>& vNeighbor,
                         const std::set<int>& fixedVert);
        void set_pattern(const Eigen::SparseMatrix<double>& mtr); //NOTE: mtr must be SPD

        void update_a(const vectorTypeI &II,
                      const vectorTypeI &JJ,
                      const vectorTypeS &SS);
        void update_a(const Eigen::SparseMatrix<double>& mtr);

        void analyze_pattern(void);

        bool factorize(void);

        void solve(Eigen::VectorXd &rhs,
                   Eigen::VectorXd &result);

        virtual void multiply(const Eigen::VectorXd& x,
                              Eigen::VectorXd& Ax);

        void setZero(void);

        virtual void setCoeff(int rowI, int colI, double val);

        virtual void addCoeff(int rowI, int colI, double val);
    };

}

#endif /* CHOLMODSolver_hpp */


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * planeRegistration.cpp
 *
 *  Created on: Jan 28, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/pc_registration.hpp"
#include "mrob/plane_registration.hpp"
#include <Eigen/LU> // for inverse and determinant
#include <Eigen/Eigenvalues>
#include <iostream>

#include <chrono>

using namespace mrob;

PlaneRegistration::PlaneRegistration():
        numberPlanes_(0), numberPoses_(0),isSolved_(0), trajectory_(new std::vector<SE3>(8,SE3())),
        solveMode_(SolveMode::GRADIENT),
        c1_(1e-4), c2_(0.9), alpha_(0.75), beta_(0.1)
{
    // Optmizer does not establish the size for this matrices and thus it is required
    gradient_.resize(6);
    hessian_.resize(6,6);
}

PlaneRegistration::~PlaneRegistration()
{

}

void PlaneRegistration::set_number_planes_and_poses(uint_t numberPlanes, uint_t numberPoses)
{
    planes_.clear();
    planes_.reserve(numberPlanes);
    trajectory_->clear();
    trajectory_->resize(numberPoses, SE3());
    numberPlanes_ = numberPlanes;
    numberPoses_ = numberPoses;

    previousState_.clear();
    previousState_.resize(numberPoses, Mat61::Zero());
}

void PlaneRegistration::reset_solution()
{
    // trajectory is reset
    trajectory_->clear();
    trajectory_->resize(numberPoses_, SE3());
    previousState_.clear();
    previousState_.resize(numberPoses_, Mat61::Zero());
}

uint_t PlaneRegistration::solve(SolveMode mode, bool singleIteration)
{
    // just in case some methods, such as gradient, has several modes
    solveMode_ = mode;
    switch(mode)
    {
        case SolveMode::INITIALIZE:
            return solve_initialize();
        case SolveMode::GRADIENT:
        case SolveMode::GRADIENT_BENGIOS_NAG:
            return solve_interpolate_gradient(singleIteration);
        case SolveMode::GN_HESSIAN:
            return optimize(NEWTON_RAPHSON);
        case SolveMode::GN_CLAMPED_HESSIAN:
            return solve_interpolate_hessian(singleIteration);
        case SolveMode::LM_SPHER:
            return optimize(LEVENBERG_MARQUARDT_SPHER);
        case SolveMode::LM_ELLIP:
            return optimize(LEVENBERG_MARQUARDT_ELLIP);
        default:
            return 0;
    }
}

uint_t PlaneRegistration::solve_interpolate_gradient(bool singleIteration)
{
    // This function is from the first implementation, is just kept for comparisons (but should not be used to solve the problem)
    // iterative process, on convergence basis | error_k - error_k-1| < tol
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;
    do
    {
        // 1) calculate plane estimation given the current trajectory
        double  initialError = 0.0;
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            initialError += it->second->estimate_plane();
        }
        diffError = previousError - initialError;
        previousError = initialError;

        std::cout << "current error iteration " << solveIters_ << " = "<< initialError << std::endl;

        // 2) calculate Gradient = Jacobian^T. We maintain the nomenclature Jacobian for coherence on the project,
        //    but actually this Jacobian should be transposed.
        Mat61 jacobian = Mat61::Zero(), accumulatedJacobian = Mat61::Zero();
        double  numberPoints, tau = 1.0 / (double)(numberPoses_-1);
        for (uint_t t = 1 ; t < numberPoses_; ++t)
        {
            numberPoints = 0.0;
            jacobian.setZero();
            for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            {
                jacobian += it->second->calculate_gradient(t);
                numberPoints += it->second->get_number_points(t);
            }
            // XXX this could be changed to time stamps later
            accumulatedJacobian +=  (tau *  t  / numberPoints / numberPoses_) * jacobian;

        }
        // 3) update results Tf = exp(-dxi) * Tf (our convention, we expanded from the left)
        // 3-1)
        Mat61 dxi, xiFinal;
        if (solveMode_ == SolveMode::GRADIENT)
        {
            double alpha = alpha_;
            dxi = -alpha * accumulatedJacobian;
            //std::cout << "\nINterpolate jacobian : = " << accumulatedJacobian.transpose() << ", and increment update = " << dxi << std::endl;
        }
        // 3.4-B) Bengio's NAG: a modification to NAG as proposed in Bengio-2013. Fixed parameters
        //          1) momentum or velocity  v_k = beta_k-1 v _k-1 - alpha_k-1 Grad f (x_k-1)
        //          2) x_k+1 = x_k + beta_k+1 beta_k * v_k - (1 + beta_k+1)*alpha_k * Grad f(x_k)
        if (solveMode_ == SolveMode::GRADIENT_BENGIOS_NAG)
        {
            double alpha = alpha_;
            double beta = beta_;
            // x update
            dxi = beta * beta * previousState_.back() - (1 + beta) * alpha * accumulatedJacobian;

            // momentum
            previousState_.back() = beta * previousState_.back() - alpha * accumulatedJacobian;
        }
        trajectory_->back().update_lhs(dxi);
        xiFinal = trajectory_->back().ln_vee();
        for (uint_t t = 1 ; t < numberPoses_-1; ++t)
        {
            dxi = tau * t * xiFinal;// SE3 does not like all derived classes TODO
            trajectory_->at(t) = SE3(dxi);
        }
        ++solveIters_;
    }while(fabs(diffError) > 1e-4 && !singleIteration && solveIters_ < 1e4);
    return solveIters_;
}

// TO BE DEPRECATED. Only used for clamped Hessian, and that is shown NOT to work.
uint_t PlaneRegistration::solve_interpolate_hessian(bool singleIteration)
{
    // iterative process, on convergence basis | error_k - error_k-1| < tol
    // For now, only 1 iteration
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;

    do
    {

        // 1) calculate plane estimation given the current trajectory. Same as solve_interpolate
        double  initialError = get_current_error();

        diffError = previousError - initialError;
        previousError = initialError;

        solveIters_++;
        std::cout << "current error iteration " << solveIters_ << " = "<< initialError << std::endl;

        // 2) calculate Gradient and Hessian
        Mat61 gradient = Mat61::Zero();
        Mat6 hessian = Mat6::Zero();
        gradient__.setZero();
        hessian__.setZero();
        double  tau = 1.0 / (double)(numberPoses_-1);
        for (uint_t t = 1 ; t < numberPoses_; ++t)
        {
            gradient.setZero();
            hessian.setZero();
            for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            {
                gradient += it->second->calculate_gradient(t);
                hessian += it->second->calculate_hessian(t);
            }
            // TODO this should be changed to time stamps later
            gradient__ +=  (tau *  t)  * gradient;
            hessian__ += (tau *  t) * hessian.selfadjointView<Eigen::Upper>();
        }
        // 3) calculate update Tf = exp(-dxi) * Tf (our convention, we expanded from the left)
        Mat61 dxi;
        if (solveMode_ == SolveMode::GN_CLAMPED_HESSIAN)
        {
            // we clamp the vector spaces corresponding to negative eigenvals
            Mat6 pseudoInv = Mat6::Zero();
            Eigen::SelfAdjointEigenSolver<Mat6> eigs(hessian__);
            for (uint_t i = 0; i < 6 ; ++i)
            {
                if(eigs.eigenvalues()[i] > 1e-4 || true ) //TODO set tolerance
                {
                    std::cout << "POSITIVE. cos distance to grad = " << eigs.eigenvectors().col(i).dot(gradient)/gradient.norm()
                              << ", eigs = " << eigs.eigenvalues()[i] << std::endl;
                    pseudoInv += (1.0/eigs.eigenvalues()(i)) * eigs.eigenvectors().col(i) * eigs.eigenvectors().col(i).transpose();
                    // XXX why is this function not monotonically decreasing? this is annoying, but makes clamping a bad idea: LM!
                }
                else
                {
                    std::cout << "NEGATIVE. cos distance to grad = " << eigs.eigenvectors().col(i).dot(gradient)/gradient.norm()
                              << ", eigs = " << eigs.eigenvalues()[i] << std::endl;

                }
            }
            dxi = - pseudoInv * gradient__;
        }
        else
            dxi = - hessian__.inverse() * gradient__;
        trajectory_->back().update_lhs(dxi);

        // 4) update full trajectory. Here we assume a full rank matrix TODO check for degenerate cases
        Mat61 xiFinal = trajectory_->back().ln_vee();
        for (uint_t t = 1 ; t < numberPoses_-1; ++t)
        {
            dxi = tau * t * xiFinal;
            trajectory_->at(t) = SE3(dxi);
        }
    }while(fabs(diffError) > 1e-4 && !singleIteration && solveIters_ < 1e4);
    return solveIters_;
}

uint_t PlaneRegistration::solve_quaternion_plane()
{
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;

    // TODO create factor graph or call dense solver?

    return solveIters_;
}

uint_t PlaneRegistration::solve_initialize()
{
    // TODO Maybe solve this as a plane-to-point alignment wrt T0
    // Initialize matrices of points
    MatX X(numberPlanes_,3), Y(numberPlanes_,3);

    // create points Y (from frame t =0), minimum 3 planes per pose
    uint_t t = 0;
    for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
    {
        it->second->calculate_all_matrices_S();
        Y.row(t) = it->second->get_mean_point(0);
        ++t;
    }

    // create points X, for t = 1, ... T, minimum 3 planes per pose
    for (t = 1; t < numberPoses_; ++t)
    {
        uint_t cont = 0;
        X.setZero();
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            X.row(cont) = it->second->get_mean_point(t);
            ++cont;
        }
        // Arun solver
        SE3 estimatedPose;
        if (!PCRegistration::arun(X,Y,estimatedPose))
            return 0;
        trajectory_->at(t) = estimatedPose;
    }

    // update current trajectory
    return 1;
}

double PlaneRegistration::get_current_error() const
{
    double  currentError = 0.0;
    for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        currentError += it->second->estimate_plane();
    return currentError;
}

void PlaneRegistration::add_plane(uint_t id, std::shared_ptr<Plane> &plane)
{
    plane->set_trajectory(trajectory_);
    planes_.emplace(id, plane);
}

double PlaneRegistration::calculate_poses_rmse(std::vector<SE3> & groundTruth) const
{
    assert(groundTruth.size() >= numberPoses_ && "PlaneRegistration::calculate_poses_rmse: number of poses from GT is incorrect\n");
    double rmse= 0.0;
    uint_t t = 0;
    for (auto &pose: groundTruth)
    {
        Mat61 dxi = (trajectory_->at(t) * pose.inv()).ln_vee();
        //std::cout << pose.ln_vee().transpose() << " and solution inv first pose \n" << (invFirstPose * trajectory_->at(t) * pose.inv()).ln_vee().transpose() <<std::endl;
        rmse += dxi.dot(dxi)/(double)numberPoses_;
        ++t;
    }
    return std::sqrt(rmse);
}

//XXX this can be a reference, who does this interface with pybinds?
std::vector<Mat31> PlaneRegistration::get_point_cloud(uint_t time)
{
	std::vector<Mat31> aggregated_pc;
	for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
    {
        std::vector<Mat31>& plane_pc = it->second->get_points(time);
        // aggregating elements
        aggregated_pc.insert(aggregated_pc.end(), plane_pc.begin(), plane_pc.end());
	}
	return aggregated_pc;
}

Mat4 PlaneRegistration::get_trajectory(uint_t time)
{
    assert(time < numberPoses_ && "CreatePoints::getPointCloud: temporal index larger than number of calculated poses\n");
    if (time < numberPoses_ )
        return trajectory_->at(time).T();
    return Mat4::Identity();
}

void PlaneRegistration::print(bool plotPlanes) const
{
    std::cout << "Printing plane registration data :"<< std::endl;
    for (SE3 &transf : *trajectory_)
        transf.print();
    if (plotPlanes)
    {
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            it->second->print();
    }
}

// resturns: [0]error, [1]iters, hessdet[2], conditioningNumber[3]
std::vector<double> PlaneRegistration::print_evaluate()
{
    std::vector<double> result(6,0.0);

    result[0] = get_current_error();
    result[1] = solveIters_;

    MatX allPlanes(numberPlanes_,4);
    MatX allNormals(numberPlanes_,3);
    uint_t i = 0;

    switch(solveMode_)
    {
        case SolveMode::GRADIENT:
        case SolveMode::GRADIENT_BENGIOS_NAG:
            hessian__.setZero();
            break;
        case SolveMode::GN_HESSIAN:
        case SolveMode::LM_SPHER:
        case SolveMode::LM_ELLIP:
            gradient__ = gradient_;
            hessian__ = hessian_;
    }

    // Normals on planes, check for rank
    for (auto plane : planes_)
    {
        Mat41 pi = plane.second->get_plane();
        //std::cout << "plane : \n" << pi << std::endl;
        allPlanes.row(i) = pi;
        allNormals.row(i) = pi.head(3)/(pi.head(3).norm());
        ++i;
    }
    // Orthogonality between planes (4 dim)
    std::cout << "current gradient \n" << gradient__ << std::endl;
    // XXX : NO, Orthogonolaity was not an issue
    //std::cout << "solution\n" << allPlanes << "\nOrthogonality between planes: \n" << allPlanes * allPlanes.transpose() <<
    //              "\n and det  = \n" << allPlanes.determinant() << std::endl;

    // XXX No, Orthogonality between normals
    //std::cout << "Orthogonality between normals: \n" << allNormals * allNormals.transpose() <<
    //             "\n and det = \n" << allNormals.determinant() << std::endl;

    // Hessian rank and eigen, look for negative vaps. Lasta hessina calculateds
    Eigen::EigenSolver<MatX> eigs(hessian__);
    std::cout << "eigen values are: \n" << eigs.eigenvalues() << std::endl;
    // Determinant of stacked normals
    std::cout << "det(Hessian) = \n" << hessian__ << std::endl;
    // Hessian conditioning number
    Eigen::JacobiSVD<Mat6> svd(hessian__, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // TODO remove
    //std::cout << "SVD decomposition : \n" << svd.singularValues() <<
    //              "\n vectors :\n" << svd.matrixU() <<
    //             "\n and conditioning number = " << svd.singularValues()(0)/svd.singularValues()(5) << std::endl;

    result[2] = hessian__.determinant();
    //TODO count this and optimze
    result[3] = svd.singularValues()(0)/svd.singularValues()(5);

    return result;
}

// From parent class Optimizer:
// TOOD for now replicated, later we will substitute
matData_t PlaneRegistration::calculate_error()
{
    return get_current_error();
}

void PlaneRegistration::calculate_gradient_hessian()
{
    Mat61 gradient = Mat61::Zero();
    Mat6 hessian = Mat6::Zero();
    gradient_.setZero();
    hessian_.setZero();
    double  tau = 1.0 / (double)(numberPoses_-1);
    for (uint_t t = 1 ; t < numberPoses_; ++t)
    {
        gradient.setZero();
        hessian.setZero();
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            gradient += it->second->calculate_gradient(t);
            hessian += it->second->calculate_hessian(t);
        }
        // TODO this should be changed to time stamps later
        gradient_ +=  (tau *  t)  * gradient;
        hessian_ += (tau *  t) * hessian.selfadjointView<Eigen::Upper>();
    }
}

void PlaneRegistration::update_state(const MatX1 &dx)
{
    trajectory_->back().update_lhs(dx);
    Mat61 xiFinal = trajectory_->back().ln_vee();
    double  tau = 1.0 / (double)(numberPoses_-1);
    Mat61 dxi;
    for (uint_t t = 1 ; t < numberPoses_-1; ++t)
    {
        dxi = tau * t * xiFinal;
        trajectory_->at(t) = SE3(dxi);
    }
}

void PlaneRegistration::bookkeep_state()
{
    bookept_trajectory_ = trajectory_->back();
}

void PlaneRegistration::update_state_from_bookkeep()
{
    trajectory_->back() = bookept_trajectory_;
    Mat61 xiFinal = bookept_trajectory_.ln_vee();
    double  tau = 1.0 / (double)(numberPoses_-1);
    Mat61 dxi;
    for (uint_t t = 1 ; t < numberPoses_-1; ++t)
    {
        dxi = tau * t * xiFinal;
        trajectory_->at(t) = SE3(dxi);
    }
    calculate_error();// planes get recalculated, which is a requisite for later
    //(this class construction, in general grad should be self-contained...)
}


#include <iostream>
#include <list>
#include <fstream>
#include <random>
#include <vector>
#include <string>
#include <limits.h>
#include <float.h>
#include <assert.h>
#include <sstream>
#include <iomanip>
#include <algorithm>
#include <map>
#include <queue>
#include <ctime>
#include <boost/algorithm/string.hpp>
#include <boost/tokenizer.hpp>
#include <signal.h>

#include "RNG_wrapper.h"
#include "RNG_wrapper.cpp"
#include "Walker.h"
#include "Walker.cpp"

int main ()
{
	RNG_wrapper rngw(12);

	std::vector<double> cumSumFits = {0.8, 1.6, 1.7, 1.8, 5};
	Walker walker(cumSumFits);

	std::vector<uint32_t> counts(5,0);
	for (int i = 0 ; i < 1e5; ++i)
	{
		counts[walker(rngw)]++;
	}

	for (auto& elem : counts)
		std::cout << elem << ":";
	std::cout << "\n";

}


#include <stdexcept>
#include <boost/test/unit_test.hpp>
#include <cmath>
#include "rectangle.hpp"
#include "base-types.hpp"

const double epsilon = 0.0000001;

BOOST_AUTO_TEST_SUITE(rectangle_constructor)

BOOST_AUTO_TEST_CASE(ValidParametersWithRotateAndWithout_ValidShapewithValidParametersConstructed)
{
  gadzhiev::Rectangle rectangle{ 10, 15, {3, 8} };
  BOOST_CHECK_CLOSE(rectangle.getHeight(), 15, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getWidth(), 10, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().x, 3, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().y, 8, epsilon);

  gadzhiev::Rectangle rectangleTwo(4, 2, { 2, 1 }, 45);

  BOOST_CHECK_CLOSE(rectangleTwo.getWidth(), 4, epsilon);
  BOOST_CHECK_CLOSE(rectangleTwo.getHeight(), 2, epsilon);
  BOOST_CHECK_CLOSE(rectangleTwo.getFrameRect().height, 6 * sqrt(2) / 2, epsilon);
  BOOST_CHECK_CLOSE(rectangleTwo.getFrameRect().width, 6 * sqrt(2) / 2, epsilon);
}

BOOST_AUTO_TEST_CASE(InvalidParameters_ThrownInvalidArument)
{
  BOOST_CHECK_THROW(gadzhiev::Rectangle rectangle(-1, 8, { 1, 8 }), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(InvalidParameterZeroWidth_ThrownInvalidArument)
{
  BOOST_CHECK_THROW(gadzhiev::Rectangle rectangle(0, 8, { 1, 8 }), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(InvalidParameterHeight_ThrownInvalidArument)
{
  BOOST_CHECK_THROW(gadzhiev::Rectangle rectangle(71, -25, { 1, 89 }), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(InvalidParameterZerHeight_ThrownInvalidArument)
{
  BOOST_CHECK_THROW(gadzhiev::Rectangle rectangle(7, 0, { 12, 8 }), std::invalid_argument);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(rectangle_move)

BOOST_AUTO_TEST_CASE(ValidMoveDxDy_RectangleCenterValidMoved)
{
  gadzhiev::Rectangle rectangle(7, 8, { 12, 11 });
  const double dx = 1;
  const double dy = 43;
  rectangle.move(dx, dy);
  BOOST_CHECK_CLOSE(12 + dx, rectangle.getCenter().x, epsilon);
  BOOST_CHECK_CLOSE(11 + dy, rectangle.getCenter().y, epsilon);
}

BOOST_AUTO_TEST_CASE(ValidMoveDxDyZero_RectangleCenterValidMoved)
{
  gadzhiev::Rectangle rectangle(7, 8, { 12, 8 });
  const double dx = 0;
  const double dy = 0;
  rectangle.move(dx, dy);
  BOOST_CHECK_CLOSE(12, rectangle.getCenter().x, epsilon);
  BOOST_CHECK_CLOSE(8, rectangle.getCenter().y, epsilon);
}

BOOST_AUTO_TEST_CASE(ValidMoveToPoint_RectangleCenterValidMoved)
{
  gadzhiev::Rectangle rectangle(7, 8, { 12, 11 });
  const gadzhiev::point_t point{ -23, 5 };
  rectangle.move(point);
  BOOST_CHECK_CLOSE(rectangle.getCenter().x, point.x, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().y, point.y, epsilon);
}

BOOST_AUTO_TEST_CASE(ValidMoveToZeroPoint_RectangleCenterValidMoved)
{
  gadzhiev::Rectangle rectangle(7, 8, { 12, 11 });
  const gadzhiev::point_t point{ 0, 0 };
  rectangle.move(point);
  BOOST_CHECK_CLOSE(rectangle.getCenter().x, point.x, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().y, point.y, epsilon);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(rectangle_frameRect)

BOOST_AUTO_TEST_CASE(FrameRectangle_ValidWidthHeightCenterOfRectangle)
{
  gadzhiev::Rectangle rectangle(10, 15, {1, 4});
  gadzhiev::rectangle_t rectangleStruct = rectangle.getFrameRect();
  BOOST_CHECK_CLOSE(rectangleStruct.width, 10, epsilon);
  BOOST_CHECK_CLOSE(rectangleStruct.height, 15, epsilon);
  BOOST_CHECK_CLOSE(rectangleStruct.pos.x, 1, epsilon);
  BOOST_CHECK_CLOSE(rectangleStruct.pos.y, 4, epsilon);
}

BOOST_AUTO_TEST_CASE(FrameRectangleAfterScale_ValidWidthHeightCenterOfRectangle)
{
  gadzhiev::Rectangle rectangle(10, 15, { 1, 4 });
  const double scaleCoef = 0.4;
  rectangle.scale(scaleCoef);

  gadzhiev::rectangle_t rectangleStruct = rectangle.getFrameRect();
  BOOST_CHECK_CLOSE(rectangleStruct.width, 10 * scaleCoef, epsilon);
  BOOST_CHECK_CLOSE(rectangleStruct.height, 15 * scaleCoef, epsilon);
  BOOST_CHECK_CLOSE(rectangleStruct.pos.x, 1, epsilon);
  BOOST_CHECK_CLOSE(rectangleStruct.pos.y, 4, epsilon);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(rectangle_scale)

BOOST_AUTO_TEST_CASE(ValidScale_CoefMoreThatOne_CmpShapeValidScaled)
{
  gadzhiev::Rectangle rectangle(12, 10, { 12, 8 });
  const double scaleCoef = 3.3;
  rectangle.scale(scaleCoef);
  BOOST_CHECK_CLOSE(rectangle.getWidth(), 12 * scaleCoef, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getHeight(), 10 * scaleCoef, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().x, 12, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().y, 8, epsilon);
}

BOOST_AUTO_TEST_CASE(ValidScale_CoefOne_CmpShapeValidScaledParametersNotChanged)
{
  gadzhiev::Rectangle rectangle(12, 10, { 12, 8 });
  const double scaleCoef = 1;
  rectangle.scale(scaleCoef);
  BOOST_CHECK_CLOSE(rectangle.getWidth(), 12, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getHeight(), 10, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().x, 12, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().y, 8, epsilon);
}

BOOST_AUTO_TEST_CASE(rectangle_scale_by_valid_coef_less_than_1)
{
  gadzhiev::Rectangle rectangle(12, 10, { 12, 8 });
  const double scaleCoef = 0.4;
  rectangle.scale(scaleCoef);
  BOOST_CHECK_CLOSE(rectangle.getWidth(), 12 * scaleCoef, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getHeight(), 10 * scaleCoef, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().x, 12, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getCenter().y, 8, epsilon);
}

BOOST_AUTO_TEST_CASE(InvalidScale_CoefLessThanZero_ThrownInvalidArgument)
{
  gadzhiev::Rectangle rectangle(12, 10, { 12, 8 });
  BOOST_CHECK_THROW(rectangle.scale(-64), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(InvalidScale_CoefZero_ThrownInvalidArgument)
{
  gadzhiev::Rectangle rectangle(12, 10, { 12, 8 });
  BOOST_CHECK_THROW(rectangle.scale(0), std::invalid_argument);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(rectangle_area)

BOOST_AUTO_TEST_CASE(DefaultArea_ValidGetArea)
{
  gadzhiev::Rectangle rectangle(7, 8, { 12, 8 });
  BOOST_CHECK_CLOSE(rectangle.getArea(), 7 * 8, epsilon);
}

BOOST_AUTO_TEST_CASE(AreaAfterMoving_AreaNotChanged)
{
  gadzhiev::Rectangle rectangle(70, 80, { 12, 8 });
  const double dx = -23;
  const double dy = 65;
  rectangle.move(dx, dy);
  BOOST_CHECK_CLOSE(rectangle.getArea(), 70 * 80, epsilon);
}

BOOST_AUTO_TEST_CASE(AreaAfterMovingToPoint_AreaNotChanged)
{
  gadzhiev::Rectangle rectangle(7, 10, { 12, 8 });
  const gadzhiev::point_t NewCenter{ 62, -8 };
  rectangle.move(NewCenter);
  BOOST_CHECK_CLOSE(rectangle.getArea(), 7 * 10, epsilon);
}

BOOST_AUTO_TEST_CASE(AreaAfterScale_AreaNotChanged)
{
  gadzhiev::Rectangle rectangle(7, 10, { 12, 8 });
  const double scaleCoef = 2.4;
  rectangle.scale(scaleCoef);
  BOOST_CHECK_CLOSE(rectangle.getArea(), 7 * 10 * scaleCoef * scaleCoef, epsilon);
}

BOOST_AUTO_TEST_CASE(AreaAfterRotate_AreaNotChanged)
{
  gadzhiev::Rectangle rectangle(4, 2, { 2, 1 });

  double rotateCoef = 45;

  rectangle.rotate(rotateCoef);

  BOOST_CHECK_CLOSE(rectangle.getArea(), 4 * 2, epsilon);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(rectangle_rotate)

BOOST_AUTO_TEST_CASE(RectangleRotate_ValidParametersOfWidthAndHeightAndValidFrameRect)
{
  gadzhiev::Rectangle rectangle(4, 2, { 2, 1 });

  double rotateCoef = 45;

  rectangle.rotate(rotateCoef);
  BOOST_CHECK_CLOSE(rectangle.getWidth(), 4, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getHeight(), 2, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getFrameRect().height, 6 * sqrt(2) / 2, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getFrameRect().width, 6 * sqrt(2) / 2, epsilon);

  rotateCoef = 0;

  rectangle.rotate(rotateCoef);

  BOOST_CHECK_CLOSE(rectangle.getWidth(), 4, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getHeight(), 2, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getFrameRect().height, 6 * sqrt(2) / 2, epsilon);
  BOOST_CHECK_CLOSE(rectangle.getFrameRect().width, 6 * sqrt(2) / 2, epsilon);

}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>
#include <crave/experimental/Variable.hpp>

#include <set>
#include <iostream>

using namespace crave;

BOOST_FIXTURE_TEST_SUITE(Ex_Operators_t, Context_Fixture)

struct logical_not_s1 : crv_sequence_item {
  logical_not_s1(crv_object_name) {}

  crv_variable<unsigned> a;
  crv_constraint con{!(a() != 0)};
};

BOOST_AUTO_TEST_CASE(logical_not_t1) {
  logical_not_s1 item("Item");
  BOOST_REQUIRE(item.con.active());
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.a, 0);
}

struct logical_not_s2 : crv_sequence_item {
  logical_not_s2(crv_object_name) {}

  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_constraint con{if_then_else(!(a() % 2 == 0), b() > 0 && b() <= 50, b() > 50 && b() <= 100)};
};

BOOST_AUTO_TEST_CASE(logical_not_t2) {
  logical_not_s2 item("item");

  BOOST_REQUIRE(item.randomize());
  std::cout << "a =" << item.a << ", b = " << item.b << std::endl;
  if (item.a % 2 != 0) {
    BOOST_REQUIRE_GT(item.b, 0u);
    BOOST_REQUIRE_LE(item.b, 50u);
  } else {
    BOOST_REQUIRE_GT(item.b, 50u);
    BOOST_REQUIRE_LE(item.b, 100u);
  }
}

struct logical_and_s1 : crv_sequence_item {
  crv_variable<bool> a;
  crv_variable<bool> b;
  crv_variable<bool> c;
  crv_constraint con1{a() == true, b() == true};
  crv_constraint con2{c() == (a() && b())};

  logical_and_s1(crv_object_name) { con2.deactivate(); }
};

BOOST_AUTO_TEST_CASE(logical_and_t1) {
  logical_and_s1 item("item");
  BOOST_REQUIRE(item.con1.active());
  BOOST_REQUIRE(!item.con2.active());
  BOOST_REQUIRE(item.randomize());
  item.con2.activate();
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.c, true);
  item.con1 = {item.a() == true, item.b() == false};
  item.con2.deactivate();
  BOOST_REQUIRE(item.randomize());
  item.con2.activate();
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.c, false);
}

struct logical_or_s1 : crv_sequence_item {
  crv_variable<bool> a;
  crv_variable<bool> b;
  crv_variable<bool> c;
  crv_constraint con1{a() == false, b() == false};
  crv_constraint con2{c() == (a() || b())};

  logical_or_s1(crv_object_name) { con2.deactivate(); }
};

BOOST_AUTO_TEST_CASE(logical_or_t1) {
  logical_or_s1 item("item");

  BOOST_REQUIRE(item.randomize());
  item.con2.activate();

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.c, false);

  item.con1 = {item.a() == true, item.b() == false};
  item.con2.deactivate();
  BOOST_REQUIRE(item.randomize());
  item.con2.activate();

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.c, true);
}

struct logical_equal_s1 : crv_sequence_item {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_constraint con{a() == 65535, a() == b()};

  logical_equal_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(equal_t1) {
  logical_equal_s1 item("equal");
  BOOST_REQUIRE(item.con.active());
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.a, item.b);
}

struct not_equal_s1 : crv_sequence_item {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_constraint con{a() < 65535, a() != b()};

  not_equal_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(not_equal_t1) {
  not_equal_s1 item("item");
  for (int i = 0; i < 300; ++i) {
    BOOST_REQUIRE(item.randomize());
    BOOST_REQUIRE_NE(item.a, item.b);
  }
}

struct neg_s1 : crv_sequence_item {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_constraint con{a() == -1337, b() == -a()};

  neg_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(neg_t1) {
  neg_s1 item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.a, -1337);
  BOOST_REQUIRE_EQUAL(item.b, 1337);
}

struct neg_s2 : crv_sequence_item {
  crv_variable<bool> a;
  crv_variable<int> b;
  crv_variable<int> c;
  crv_variable<int> d;
  crv_constraint con1 = {dist(a(), distribution<bool>::create(0.5)) && b() == 1337 && c() == 42 &&
                         if_then_else(a(), d() == -b(), d() == -c())};

  neg_s2(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(neg_t2) {
  neg_s2 item("item");
  for (int i = 0; i < 50; ++i) {
    BOOST_REQUIRE(item.randomize());
    if (item.a)
      BOOST_REQUIRE_EQUAL(item.d, -1337);
    else
      BOOST_REQUIRE_EQUAL(item.d, -42);
  }
}

struct comp_s1 : crv_sequence_item {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_constraint con1 = {a() == 0, b() == ~a()};

  comp_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(complement_t1) {
  comp_s1 item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.a, 0);
  BOOST_REQUIRE_EQUAL(item.b, -1);
}

struct bitwise_and_s1 : crv_sequence_item {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_variable<unsigned> c;
  crv_constraint con1 = {a() == 42, b() == 1337, c() == (a() & b())};

  bitwise_and_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(bitwise_and_t1) {
  bitwise_and_s1 item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.c, 40);
}

struct bitwise_or_s1 : crv_sequence_item {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_variable<unsigned> c;
  crv_constraint con1 = {a() == 42, b() == 1337, c() == (a() | b())};

  bitwise_or_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(bitwise_or_t1) {
  bitwise_or_s1 item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.c, 1339);
}

struct xor_s : crv_sequence_item {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_variable<unsigned> c;
  crv_constraint con = {a() == 65535, b() == 4080, c() == (a() ^ b())};

  xor_s(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(bitwise_xor_t1) {
  xor_s item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.c, 61455);
}

struct element_inside_set_s1 : crv_sequence_item {
  crv_variable<unsigned> a;
  std::set<unsigned> s{1, 7, 9};
  crv_constraint con1{inside(a(), s)};

  element_inside_set_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(element_inside_set_t1) {
  element_inside_set_s1 item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(item.s.find(item.a) != item.s.end());

  unsigned first = item.a;
  item.con1 &= {item.a() != first};

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(item.s.find(item.a) != item.s.end());

  unsigned second = item.a;
  BOOST_REQUIRE_NE(first, second);

  item.con1 &= {item.a() != second};

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(item.s.find(item.a) != item.s.end());

  unsigned third = item.a;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  item.con1 &= {item.a() != third};

  BOOST_REQUIRE(!item.randomize());
}

struct element_inside_vec_s1 : crv_sequence_item {
  crv_variable<unsigned> x;
  std::vector<unsigned> v{1, 7, 9};
  crv_constraint con1 = {inside(x(), v)};

  element_inside_vec_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(element_inside_vec_t1) {
  element_inside_vec_s1 item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(find(item.v.begin(), item.v.end(), item.x) != item.v.end());

  unsigned first = item.x;
  item.con1 &= {item.x() != first};

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(find(item.v.begin(), item.v.end(), item.x) != item.v.end());

  unsigned second = item.x;
  BOOST_REQUIRE_NE(first, second);
  item.con1 &= {item.x() != second};

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(find(item.v.begin(), item.v.end(), item.x) != item.v.end());

  unsigned third = item.x;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  item.con1 &= {item.x() != third};
  BOOST_REQUIRE(!item.randomize());
}

struct element_inside_array_s1 : crv_sequence_item {
  crv_variable<unsigned> x;
  unsigned a[3]{1, 7, 9};
  crv_constraint con1{inside(x(), a)};

  element_inside_array_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(element_inside_array_t) {
  element_inside_array_s1 item("item");
  BOOST_REQUIRE(item.randomize());

  unsigned first = item.x;
  item.con1 &= {item.x() != first};

  BOOST_REQUIRE(item.randomize());

  unsigned second = item.x;
  BOOST_REQUIRE_NE(first, second);
  item.con1 &= {item.x() != second};

  BOOST_REQUIRE(item.randomize());
  unsigned third = item.x;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  item.con1 &= {item.x() != third};

  BOOST_REQUIRE(!item.randomize());
}

struct element_inside_list_s1 : crv_sequence_item {
  crv_variable<unsigned> x;
  std::list<unsigned> l{1, 7, 9};
  crv_constraint con1{inside(x(), l)};

  element_inside_list_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(element_inside_list_t1) {
  element_inside_list_s1 item("item");

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(find(item.l.begin(), item.l.end(), item.x) != item.l.end());

  unsigned first = item.x;
  item.con1 &= {item.x() != first};

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(find(item.l.begin(), item.l.end(), item.x) != item.l.end());

  unsigned second = item.x;
  BOOST_REQUIRE_NE(first, second);
  item.con1 &= {item.x() != second};

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(find(item.l.begin(), item.l.end(), item.x) != item.l.end());

  unsigned third = item.x;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  item.con1 &= {item.x() != third};

  BOOST_REQUIRE(!item.randomize());
}

struct element_not_inside_s1 : crv_sequence_item {
  crv_variable<unsigned> x;
  std::set<unsigned> s;
  crv_constraint con1{inside(x(), s)};

  element_not_inside_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(element_not_inside_t1) {
  element_not_inside_s1 item("item");
  BOOST_REQUIRE(!item.randomize());
}

struct element_not_inside_s2 : crv_sequence_item {
  crv_variable<unsigned> x;
  std::vector<unsigned> s;
  crv_constraint con1{inside(x(), s)};

  element_not_inside_s2(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(element_not_inside_t2) {
  element_not_inside_s2 item("item");
  BOOST_REQUIRE(!item.randomize());
}

struct element_not_inside_s3 : crv_sequence_item {
  crv_variable<unsigned> x;
  std::list<unsigned> s;
  crv_constraint con1{inside(x(), s)};

  element_not_inside_s3(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(element_not_inside_t3) {
  element_not_inside_s3 item("item");
  BOOST_REQUIRE(!item.randomize());
}

struct if_then_else_s1 : crv_sequence_item {
  unsigned a;
  crv_variable<unsigned> b;
  crv_constraint con1{if_then_else(reference(a)<5, b()> 0 && b() <= 50, b() > 50 && b() <= 100)};

  if_then_else_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(if_then_else_t1) {
  if_then_else_s1 item("item");
  for (item.a = 0; item.a < 10; ++item.a) {
    BOOST_REQUIRE(item.randomize());
    std::cout << "a =" << item.a << ", b = " << item.b << std::endl;
    if (item.a < 5) {
      BOOST_REQUIRE_GT(item.b, 0);
      BOOST_REQUIRE_LE(item.b, 50);
    } else {
      BOOST_REQUIRE_GT(item.b, 50);
      BOOST_REQUIRE_LE(item.b, 100);
    }
  }
}

struct if_then_s1 : crv_sequence_item {
  unsigned a;
  crv_variable<unsigned> b;
  crv_constraint con1{if_then(reference(a)<5, b()> 0 && b() <= 100),
                      if_then(reference(a) >= 5, b() > 100 && b() <= 1000)};

  if_then_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(if_then_t1) {
  if_then_s1 item("item");

  for (item.a = 0; item.a < 10; ++item.a) {
    BOOST_REQUIRE(item.randomize());
    std::cout << "a =" << item.a << ", b = " << item.b << std::endl;
    if (item.a < 5) {
      BOOST_REQUIRE_GT(item.b, 0);
      BOOST_REQUIRE_LE(item.b, 100);
    } else {
      BOOST_REQUIRE_GT(item.b, 100);
      BOOST_REQUIRE_LE(item.b, 1000);
    }
  }
}

struct bitslice_s1 : crv_sequence_item {
  crv_variable<short> x;
  crv_constraint con1{bitslice(10, 3, x()) == 0xFF};

  bitslice_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(bitslice_t) {
  bitslice_s1 item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL((item.x >> 3) & 0xFF, 0xFF);

  BOOST_REQUIRE_THROW(item.con1 &= {bitslice(3, 10, item.x()) == 0xFF}, std::runtime_error);
  BOOST_REQUIRE_THROW(item.con1 &= {bitslice(16, 3, item.x()) == 0xFF}, std::runtime_error);
}

struct s_shiftleft : crv_sequence_item {
  s_shiftleft(crv_object_name) {}
  crv_variable<unsigned> a, b, c;
  crv_constraint con = {a() < 256u, b() < ((unsigned)(sizeof(unsigned) * 8u)), c() == (a() << b())};
};
BOOST_AUTO_TEST_CASE(shiftleft) {
  VariableDefaultSolver::bypass_constraint_analysis = true;
  s_shiftleft item("item");
  int count = 0;
  int max_count = 200;
  while (item.randomize() && ++count < max_count) {
    unsigned av = item.a;
    unsigned bv = item.b;
    unsigned r = av << bv;

    BOOST_REQUIRE_EQUAL(r, item.c);
    item.con &= {item.a() != item.a || item.b() != item.b};
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
  BOOST_REQUIRE_EQUAL(count, max_count);
}

struct s_shiftright : public crv_sequence_item {
  s_shiftright(crv_object_name) {}
  crv_variable<unsigned> a, b, c;
  crv_constraint con = {a() > 256u, b() < 8u, c() == (a() >> b())};
};

BOOST_AUTO_TEST_CASE(shiftright) {
  VariableDefaultSolver::bypass_constraint_analysis = true;
  s_shiftright item("shiftright");
  int count = 0;
  int max_count = 300;
  while (item.randomize() && ++count < max_count) {
    unsigned av = item.a;
    unsigned bv = item.b;
    unsigned r = av >> bv;

    BOOST_REQUIRE_EQUAL(r, item.c);
    item.con &= {item.a != item.a() || item.b() != item.b};
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
  BOOST_REQUIRE_EQUAL(count, max_count);
}

struct s_plus_minus : crv_sequence_item {
  s_plus_minus(crv_object_name) {}
  crv_variable<unsigned> a, b, q, r;
  crv_constraint con = {b() != 0u, b() < a(), (q() == a() + b()), r() == a() - b()};
};
BOOST_AUTO_TEST_CASE(plus_minus) {
  VariableDefaultSolver::bypass_constraint_analysis = true;
  s_plus_minus item("item");

  unsigned cnt = 0u;
  while (item.randomize() && cnt < 300) {
    BOOST_REQUIRE_EQUAL(item.a + item.b, item.q);
    BOOST_REQUIRE_EQUAL(item.a - item.b, item.r);

    item.con &= {(item.a() != item.a || item.b() != item.b)};
    std::cout << "#" << cnt++ << ": result: a=" << item.a << ", b=" << item.b << ", q=" << item.q << ", r=" << item.r
              << "\n" << std::endl;
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

struct mult_mod_s1 : crv_sequence_item {
  crv_variable<int> a;
  crv_variable<int> b;
  crv_constraint con1 = {-3 <= a() && a() <= 3, -3 <= b() && b() <= 3, a() * b() % 6 == 0};

  mult_mod_s1(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(mult_mod_t1) {
  mult_mod_s1 item("item");

  int cnt = 0;
  for (int i = -3; i <= 3; i++)
    for (int j = -3; j <= 3; j++)
      if (i * j % 6 == 0) cnt++;

  int cnt1 = 0;

  while (item.randomize()) {
    cnt1++;
    BOOST_REQUIRE_EQUAL(item.a * item.b % 6, 0);
    item.con1 &= {item.a() != item.a || item.b() != item.b};
    std::cout << "result: a1=" << item.a << ", b1=" << item.b << "\n" << std::endl;
    BOOST_REQUIRE_LE(cnt1, cnt);
  }

  BOOST_REQUIRE_EQUAL(cnt, cnt1);
}

struct s_divide : public crv_sequence_item {
  s_divide(crv_object_name) {}
  crv_variable<unsigned char> a, b, q, r;
  crv_constraint con = {b() != (unsigned char)0u, a() < (unsigned char)16u, b() < (unsigned char)16u, q() == a() / b(),
                        r() == a() % b()};
};

BOOST_AUTO_TEST_CASE(divide) {
  s_divide item("item");
  VariableDefaultSolver::bypass_constraint_analysis = true;

  while (item.randomize()) {
    BOOST_REQUIRE_EQUAL(item.a / item.b, item.q);
    BOOST_REQUIRE_EQUAL(item.a % item.b, item.r);
    item.con &= {item.a() != item.a, item.b() != item.b};
    std::cout << "result: a=" << item.a << ", b=" << item.b << ", q=" << item.q << ", r=" << item.r << "\n"
              << std::endl;
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
}
BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 5> test_int_1_5(const Eigen::Matrix<int, 1, 5> & M)
{
	return M;
}
void export_int_1_5()
{
	boost::python::def("test_int_1_5",test_int_1_5);
}


/*! \file
 * \brief Mergesort example.
 *
 * This example shows how to use the threadpool library.
 *
 * Copyright (c) 2005-2006 Philipp Henkel
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * http://threadpool.sourceforge.net
 *
 */

#include <boost/threadpool.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <algorithm>

using namespace std;
using namespace boost::threadpool;

//
// Helpers
boost::mutex m_io_monitor;

void print(string text)
{
  boost::mutex::scoped_lock lock(m_io_monitor);
  cout << text;
}

template<class T>
string to_string(const T& value)
{
  ostringstream ost;
  ost << value;
  ost.flush();
  return ost.str();
}

unsigned long get_ms_diff(boost::xtime& start, boost::xtime& end)
{
  boost::xtime::xtime_sec_t start_ms = start.sec * 1000	+ start.nsec/1000000;
  boost::xtime::xtime_sec_t end_ms = end.sec * 1000	+ end.nsec/1000000;
  return static_cast<unsigned long>(end_ms - start_ms);
}

class image
{
public:
  image() : m_content(0)	{}
  image(int content) : m_content(content)	{}

  image(const image& src)
  {
    m_content = src.m_content;
  }

  bool operator<(const image& l) const
  {
    {	// simulate time needed for image comparision
      boost::xtime xt;
      boost::xtime_get(&xt, boost::TIME_UTC);
      int duration = 1+(m_content % 4);
      xt.nsec += 250 * 1000 * duration;
      boost::thread::sleep(xt);
	    print(".");
    }
    return m_content < l.m_content;
  }

protected:
  int m_content;	// represents image data in this example
};

template<class T>
class merge_job
{
public:
  merge_job(boost::shared_array<T> data, unsigned int position, unsigned int length)
    : m_data(data)
    , m_position(position)
    , m_length(length)
  {
    print("merge job created : " + to_string(m_position) +", "+ to_string(m_length) +"\n");
  }

  void run()
  {
    print("merge job running :   " + to_string(m_position) +", "+ to_string(m_length) +"\n");

    T* begin = m_data.get();
    std::advance(begin, m_position);

    T* mid = m_data.get();
    std::advance(mid, m_position + m_length/2);

    T* end = m_data.get();
    std::advance(end, m_position + m_length);

    std::inplace_merge(begin, mid, end);

    print("\nmerge job finished:     "  + to_string(m_position) +", "+ to_string(m_length) +"\n");
  }

protected:
  boost::shared_array<T> m_data;
  unsigned int m_position;
  unsigned int m_length;
};

//
// A demonstration of the thread_pool class
int main (int argc, char * const argv[])
{
  print("MAIN: construct thread pool\n");

  boost::xtime start;
  boost::xtime_get(&start, boost::TIME_UTC);

  int exponent = 7;
  int data_len = 1 << exponent;  // = pow(2, exponent)

  print("MAIN: sort array with "+ to_string(data_len) + " elements.\n");

  boost::shared_array<image> data(new image[data_len]);

  // fill array with arbitrary values (not sorted ascendingly)
  for(int i = 0; i < data_len; i++)
  {
    data[i] = image((data_len - i - 1) % 23);
  }

  /***************************/
  /* Standard implementation */
  /***************************/

  pool tp;
  tp.size_controller().resize(5);

// merge data array
  for(int step = 1; step <= exponent; step++)
  {
    print("\nMAIN: merge step "+ to_string(step)+"\n");

    // divide array into partitions
    int partition_size = 1 << step;
    for(int partition = 0; partition < data_len/partition_size; partition++)
    {
      // sort partition
      boost::shared_ptr<merge_job<image> > job(new merge_job<image>(data, partition*partition_size, partition_size));
      //tp->schedule(prio_task_func(5, boost::bind(&merge_job<image>::run, job)));
      schedule(tp, boost::bind(&merge_job<image>::run, job));
     // schedule(tp, job);
    }
    tp.wait();	// wait until all partitions are sorted
  }

  boost::xtime end;
  boost::xtime_get(&end, boost::TIME_UTC);

  print("\nMAIN: duration " + to_string(get_ms_diff(start, end)) + " ms \n");

  print("\nMAIN: check if array is sorted... \n");

  // check if array is sorted ascendingly
  bool ascending = true;
  for(int i = 0; i < data_len-1; i++)
  {
    if(data[i+1] < data[i])
    {
      ascending = false;
    }
  }

  if(ascending)
  {
    print("\nMAIN: array is sorted\n");
  }
  else
  {
    print("\nMAIN: array is NOT sorted!\n");
  }

  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_FACT_4_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_FACT_4_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate 4! that is 24

    @par Header <boost/simd/constant/fact_4.hpp>

    @par Semantic:

    @code
    T r = Fact_4<T>();
    @endcode

    is similar to:

    @code
    T r = T(24);
    @endcode

    @return The Fact_4 constant for the proper type
  **/
  template<typename T> T Fact_4();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant fact_4.

      @return The Fact_4 constant for the proper type
    **/
    Value Fact_4<Value>();
  }
} }
#endif

#include <boost/simd/constant/scalar/fact_4.hpp>
#include <boost/simd/constant/simd/fact_4.hpp>

#endif