#define _USE_MATH_DEFINES
#include <cmath>
#include <algorithm>
#include <unistd.h>
#include <fstream>
#include <iomanip>
#include <map>
#include <Eigen/Dense>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include "bayesopt/bayesopt.hpp"
#include "specialtypes.hpp"
#include "param_loader.hpp"
#include "read_para_bayes.h"
#include "read_para_vehicle.h"
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/Float64.h>

using namespace std;
using namespace Eigen;

enum optimizationChoice{
  processNoise, //0
  observationNoise, //1
  all //2
};

class Robot1D: public bayesopt::ContinuousModel
{
public:
    Robot1D(bayesopt::Parameters par, int dim, ros::NodeHandle& nh, ReadParaVehicle& rv):
      ContinuousModel(dim,par) {
          rv_        = rv;
          dim_       = dim;
          nh_        = nh;
          pub_noise_ = nh.advertise<std_msgs::Float64MultiArray>("noise", 10);
          pub_sg_ac_  = nh.advertise<std_msgs::Float64MultiArray>("sgAc", 10);
          //subscribe in a class. If outside of a class, we need use object to take the place this pointer
          sub_cost_  = nh.subscribe<std_msgs::Float64>("cost", 10, &Robot1D::CostCallback,this);

          //to use switch case to decide what parameters to optimize
          choice_.insert(std::pair<std::string,optimizationChoice>("processNoise",processNoise));
          choice_.insert(std::pair<std::string,optimizationChoice>("observationNoise",observationNoise));
          choice_.insert(std::pair<std::string,optimizationChoice>("all",all));
      }

    double evaluateSample(const vectord& xin)
    {
      if(!ros::ok())
        exit(0);
      cost_ = -1;
      it_counter_++;
      std_msgs::Float64MultiArray noise_msgs;

      //to assign that layout has two dimensions
      noise_msgs.layout.dim.push_back(std_msgs::MultiArrayDimension());
      noise_msgs.layout.dim.push_back(std_msgs::MultiArrayDimension());
      noise_msgs.layout.dim[0].size = rv_.pnoise_.size();
      noise_msgs.layout.dim[0].label = "processNoiseDimension";
      noise_msgs.layout.dim[1].size = rv_.onoise_.size();
      noise_msgs.layout.dim[1].label = "measurmentNoiseDimension";

      std::vector<double> data;//data to publish, including process noise and observation
      std::vector<double> dataOn;//data of observationNoise

      //switch case cannot be used for string so I use map
      switch(choice_.find(rv_.optimization_choice_)->second){
        case processNoise:
          data.insert(data.end(),xin.begin(),xin.end());
          dataOn = rv_.onoise_; //if just optimize the process noise, we use the default observation noise in the yaml file
          data.insert(data.end(),dataOn.begin(),dataOn.end());
          break;
        case observationNoise:
          data = rv_.pnoise_;
          dataOn.insert(dataOn.end(),xin.begin(),xin.end()); //optimize observation noise, process noise will use default ones
          data.insert(data.end(),dataOn.begin(),dataOn.end());
          break;
        case all:
          data.insert(data.end(),xin.begin(),xin.end());
          break;
        default:
          std::cout<<"Please choose optimization method!!!"<<std::endl;
          ros::shutdown();
          break;
      }

      //the vector data is always like this, std::vector<double> data = {DataOfprocessNoise,DataOfObservationNoise}
      noise_msgs.data = data;

      //maybe ros system cannot response that fast. We need publish one message once or twice then we need sleep for a short time
      usleep(300000);
      pub_noise_.publish(noise_msgs);

      std::cout<<"publish the noise........................."<<std::endl;

      usleep(200000);
      get_pub_sg_ac_data();//this is the data from last iteration

      /*when a message is published, the corresponding call back function will be stored into a queue.
      when a ros spin is called, it will call the first one in the queue.
      ros spinOnce will only check the if the queue has call back function for several hundred msecs
      if the cost function costs too much time(publish to fast), then the ros spinOnce will just return because there is not enough
      time for it to wait and check. Then when the cost function is finished, the callback function will not be executed
      Here I add one more condition, when cost is -1 we continue check if there is callback function, the callback function
      can change the cost so that we can jump out from the will loop. This can make us not lost one call back function
      when the cost function is too long.*/
      while(cost_ == -1 && ros::ok()){
        ros::spinOnce();
      }
      if(cost_< min_cost_)
        min_cost_ = cost_;
      return cost_;
    };

    void CostCallback(const std_msgs::Float64::ConstPtr& msg){
      cost_ = msg->data;
      std::cout<<"get the cost, now change it to "<<cost_<<"..........................\n \n"<<std::endl;
    }

    bool checkReachability(const vectord &query)
    {return true;}

    /*This function is for bayesopt plot, work with `rosrun robot_kf_bayesopt plot_surrogate_acquisition.py`*/
    void get_pub_sg_ac_data(){
    std_msgs::Float64MultiArray sg_ac_msgs;
    sg_ac_msgs.layout.dim.push_back(std_msgs::MultiArrayDimension());//to assign that layout has two dimensions
    sg_ac_msgs.layout.dim[0].size = dim_;
    sg_ac_msgs.layout.dim[0].label = "dimension to plot";

    if(dim_ == 1){
      std::vector<double> x       = get1Dx();
      std::vector<double> y       = get1Dmean();
      std::vector<double> sl      = get1DStdLowerBound();
      std::vector<double> su      = get1DStdUpperBound();
      std::vector<double> c       = get1DAcquisitionFunctionValue();
      std::vector<double> sampleX = getSampleX(); //points that are really sampled(run the cost function and get the cost)
      std::vector<double> sampleY = getSampleY();

      sg_ac_msgs.layout.dim.push_back(std_msgs::MultiArrayDimension());//to assign that layout has two dimensions
      sg_ac_msgs.layout.dim[1].size = x.size();
      sg_ac_msgs.layout.dim[1].label = "size";
      //data structure is x axis value, mean, lower bound, upper bound, and criteria
      sg_ac_msgs.data.insert(sg_ac_msgs.data.end(),  x.begin(),      x.end());
      sg_ac_msgs.data.insert(sg_ac_msgs.data.end(),  y.begin(),      y.end());
      sg_ac_msgs.data.insert(sg_ac_msgs.data.end(), sl.begin(),      sl.end());
      sg_ac_msgs.data.insert(sg_ac_msgs.data.end(), su.begin(),      su.end());
      sg_ac_msgs.data.insert(sg_ac_msgs.data.end(),  c.begin(),      c.end());
      sg_ac_msgs.data.insert(sg_ac_msgs.data.end(), sampleX.begin(), sampleX.end());
      sg_ac_msgs.data.insert(sg_ac_msgs.data.end(), sampleY.begin(), sampleY.end());

      //There is a very strange phenomenon. After the last iteration I publish the data, the matplot figure window will
      //freeze(no response). I think it is because ros spin. Loop is in ros spin so I cannot do anything operating the figure.
      //The way I solve this is by passing a last iteration symbol to
      //python code and tell it this is the last iteration(dim[2] == 1). Then you can see in the python code
      //I'll have a 9999...(very long pause) to make the figure window not freeze.
      sg_ac_msgs.layout.dim.push_back(std_msgs::MultiArrayDimension());//to assign that layout has two dimensions
      if(it_counter_ == mParameters.n_init_samples + mParameters.n_iterations -1) //mParameter is a public member of BayesoptBase class, which has been inherited
        sg_ac_msgs.layout.dim[2].size = 1;
      else
        sg_ac_msgs.layout.dim[2].size = 0;
      sg_ac_msgs.layout.dim[2].label = "keep active";

      pub_sg_ac_.publish(sg_ac_msgs);
    }
    else if(dim_ == 2){
      std::vector<double> xx  = get2Dx(); //we just need 1D x because the coordinate is x square
      std::vector<std::vector<double> > yy   = get2Dmean();
      std::vector<std::vector<double> > slsl = get2DStdLowerBound();
      std::vector<std::vector<double> > susu = get2DStdUpperBound();
      std::vector<std::vector<double> > cc   = get2DAcquisitionFunctionValue();
      std::vector<double> sampleX = getSampleX(); //points that are really sampled(run the cost function and get the cost)
      std::vector<double> sampleY = getSampleY();

      if(xx.size() != 0){
        sg_ac_msgs.layout.dim.push_back(std_msgs::MultiArrayDimension());//to assign that layout has two dimensions
        sg_ac_msgs.layout.dim[1].size = xx.size()/2;
        sg_ac_msgs.layout.dim[1].label = "size";

        sg_ac_msgs.data.insert(sg_ac_msgs.data.end(),  xx.begin(),      xx.begin()+xx.size()/2);//in fact we just need one row of x becasue the coordinate is x square

        //copy all the other data to msgs
        //yy is like yy[i][j] is the mean value whose coordinate is (x[i], x[j]). Similar for the others
        //Normally we don't plot upper bound and lower bound for 2D or it will be hard to visualize
        for(int i = 0; i < xx.size()/2; i++){
          sg_ac_msgs.data.insert(sg_ac_msgs.data.end(),  yy[i].begin(),      yy[i].end());
        }

        for(int i = 0; i < xx.size()/2; i++){
          sg_ac_msgs.data.insert(sg_ac_msgs.data.end(),  cc[i].begin(),      cc[i].end());
        }

        sg_ac_msgs.data.insert(sg_ac_msgs.data.end(), sampleX.begin(), sampleX.end());
        sg_ac_msgs.data.insert(sg_ac_msgs.data.end(), sampleY.begin(), sampleY.end());

        sg_ac_msgs.layout.dim.push_back(std_msgs::MultiArrayDimension());//to assign that layout has two dimensions

        //The matplot will freeze after the last iteration, so we need give a sigal to let it keep active(not exit the program)
        if(it_counter_ == mParameters.n_init_samples + mParameters.n_iterations -1){
          sg_ac_msgs.layout.dim[2].size = 1;
          std::cout<<"should keep active"<<std::endl;
        }
        else
          sg_ac_msgs.layout.dim[2].size = 0;
        sg_ac_msgs.layout.dim[2].label = "keep active";

        pub_sg_ac_.publish(sg_ac_msgs);
      }
    }
  }

  double min_cost_ = 9999999;
private:
    ros::NodeHandle nh_;
    ros::Publisher pub_noise_;
    ros::Publisher pub_sg_ac_; //publish surrogate model value and acuiqsition function value
    ros::Subscriber sub_cost_;
    ReadParaVehicle rv_;
    std::map<std::string, optimizationChoice> choice_;
    int dim_;//dimention of optimized parameters;
    double cost_;
    int it_counter_ = 0; //iteration number
};

int main(int nargs, char *args[])
{
  ros::init(nargs,args,"robot1d_bayes");

  ros::NodeHandle n;

  ReadParaBayes re(n);
  ReadParaVehicle rv(n,false); //false: won't show the parameters that are read

  bayesopt::Parameters par;
  if(nargs > 1){
      if(!bayesopt::utils::ParamLoader::load(args[1], par)){
          std::cout << "ERROR: provided file \"" << args[1] << "\" does not exist" << std::endl;
          return -1;
      }
  }
  else{
        par = initialize_parameters_to_default();
        par.n_init_samples = re.n_init_samples_;
        par.n_iterations = re.n_iterations_;
        par.random_seed = re.random_seed_;
        par.verbose_level = re.verbose_level_;
        par.surr_name = re.surr_name_;
        par.noise = re.noise_;
        par.force_jump = re.force_jump_;
      }

  int opt_dim = re.lower_bound_.size();
  Robot1D sc(par,opt_dim,n, rv);
  vectord result(opt_dim);

  boost::numeric::ublas::vector<double> lower_bound(opt_dim);
  boost::numeric::ublas::vector<double> upper_bound(opt_dim);
  for(int i = 0; i<opt_dim; i++){
    lower_bound(i) = re.lower_bound_[i];
    upper_bound(i) = re.upper_bound_[i];
  }
  sc.setBoundingBox(lower_bound,upper_bound);

  sc.optimize(result);

  std::cout<<"final optimization result ";
  for(auto r:result)
    std::cout<<r<<",";
  std::cout<<std::endl;
  std::ofstream ofs;

return 0;
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

template <typename T>
constexpr T diff(T a, T b) {
	return max(a, b) - min(a, b);
}

template <typename T>
constexpr T is_in_range(T a, T v, T b) {
	return a <= v && v <= b;
}

template <typename T, typename U, typename F>
std::vector<U> mapcar(std::vector<T>& v, F proc) {
	std::vector<U> ret(v.size());
	std::transform(std::begin(v), std::end(v), std::begin(ret), proc);
	return ret;
}

template <typename T, typename F>
std::vector<T> filter(std::vector<T>& v, F proc) {
	std::vector<T> ret;
	std::copy_if(std::begin(v), std::end(v), std::back_inserter(ret), proc);
	return ret;
}

// 終了判定
template <typename T,
		  typename enable_if<is_integral<T>::value>::type* = nullptr>
bool finds(T left, T right) {
	return abs(right - left) > 1;
}

// 二分探索
template <typename T>
T bin_search(T left, T right, auto pred) {
	while(finds<T>(left, right)) {
		T middle = (left + right) / 2;
		if(pred(middle)) {
			left = middle;
		} else {
			right = middle;
		}
	}
	return left;
}

int main() {
	int n, k;
	cin >> n >> k;
	VLL x(n), y(n);
	REP(i, n) { cin >> x[i] >> y[i]; }

	VLL sorted_x(x);
	sort(ALL(sorted_x));
	VLL sorted_y(y);
	sort(ALL(sorted_y));
	ll result = static_cast<ll>(INF) * INF;
	REP(i, n) {			// x right index
		REP(j, i) {		// x left index
			REP(l, n) { // y bottom index
				int y_top_index = bin_search(-1, l + 1, [&](int m) {
					if(m < 0) {
						return true;
					}
					if(m > l) {
						return false;
					}
					ll x_right = sorted_x[i];
					ll x_left = sorted_x[j];
					ll y_bottom = sorted_y[l];
					ll y_top = sorted_y[m];
					ll c = 0;
					REP(ii, n) {
						if(is_in_range(x_left, x[ii], x_right) &&
						   is_in_range(y_top, y[ii], y_bottom)) {
							c++;
						}
					}
					return c >= k;
				});
				if(y_top_index < 0 || l < y_top_index) {
					continue;
				}
				chmin(result,
					  diff(sorted_x[i], sorted_x[j]) *
						  diff(sorted_y[l], sorted_y[y_top_index]));
			}
		}
	}

	cout << result << endl;
	return 0;
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(ray_test)
{

  {
    geometry::Ray<V> A;
    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.origin()(0), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.origin()(1), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.origin()(2), 0.0, 0.01 );

    BOOST_CHECK_CLOSE(A.direction()(0), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.direction()(1), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.direction()(2), 1.0, 0.01 );
  }
  {
    V const o = V::make(1.0, 2.0, 3.0);
    V const d = V::make(4.0, 0.0, 0.0);

    geometry::Ray<V> A = geometry::make_ray(o, d);

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.origin()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.origin()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(A.origin()(2), 3.0, 0.01 );

    BOOST_CHECK_CLOSE(A.direction()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.direction()(1), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.direction()(2), 0.0, 0.01 );
  }
  {
    V const o = V::make(1.0, 2.0, 3.0);
    V const d = V::make(4.0, 0.0, 0.0);

    geometry::Ray<V> A;

    A.origin() = o;
    A.set_direction(d);

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.origin()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.origin()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(A.origin()(2), 3.0, 0.01 );

    BOOST_CHECK_CLOSE(A.direction()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.direction()(1), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.direction()(2), 0.0, 0.01 );
  }
  {
    V const o = V::make(1.0, 2.0, 3.0);
    V const d = V::make(4.0, 0.0, 0.0);

    geometry::Ray<V> A = geometry::make_ray(o, d);

    geometry::Ray<V> B = A;
    geometry::Ray<V> C(A);

    BOOST_CHECK( geometry::is_valid(B) == true );
    BOOST_CHECK( geometry::is_valid(C) == true );

    BOOST_CHECK_CLOSE(B.origin()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(B.origin()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(B.origin()(2), 3.0, 0.01 );

    BOOST_CHECK_CLOSE(B.direction()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(B.direction()(1), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(B.direction()(2), 0.0, 0.01 );

    BOOST_CHECK_CLOSE(C.origin()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(C.origin()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(C.origin()(2), 3.0, 0.01 );

    BOOST_CHECK_CLOSE(C.direction()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(C.direction()(1), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(C.direction()(2), 0.0, 0.01 );

  }

}

BOOST_AUTO_TEST_SUITE_END();


#include "cunp.hpp"
#include "np_inspect.hpp"
#include <Python.h>
#include <numpy/arrayobject.h>
#include <numpy/arrayscalars.h>
#include <stdexcept>
#include "monotype.hpp"
#include "type_printer.hpp"
#include <boost/python/extract.hpp>

using std::shared_ptr;
using namespace backend;
using std::make_tuple;
using std::make_shared;

extern "C" {
void initialize_cunp() {
    //This initializes Numpy
    import_array();
}
}

shared_ptr<const type_t> np_to_cu(NPY_TYPES n) {
    switch(n) {
    case NPY_BOOL:
        return bool_mt;
    case NPY_INT:
        return int32_mt;
    case NPY_LONG:
        return int64_mt;
    case NPY_FLOAT:
        return float32_mt;
    case NPY_DOUBLE:
        return float64_mt;
    default:
        return void_mt;
    }
}

bool isnumpyarray(PyObject* in) {
    return PyArray_Check(in);
}

boost::python::object convert_to_array(PyObject* in) {
    PyObject* array = PyArray_FROM_OTF(in, NPY_NOTYPE, NPY_IN_ARRAY);

    //Record that we have a reference to the result of PyArray_FROM_OTF
    //Putting it in a boost::python::object ensures that reference counting
    //happens automatically for this object, even in the presence of exceptions
    boost::python::handle<> array_handle(array);
    return boost::python::object(array_handle);
}

np_array_info inspect_array(PyObject* in) {
    boost::python::object bp_object = convert_to_array(in);
    PyObject* input_array = bp_object.ptr();

    if (!(PyArray_Check(input_array))) {
        return make_tuple((void*)NULL, 0, void_mt, boost::python::object());
    }

    PyArrayObject* input_as_array = (PyArrayObject*)input_array;

    ssize_t n = input_as_array->dimensions[0];
    void* d = input_as_array->data;
    NPY_TYPES dtype = NPY_TYPES(PyArray_TYPE(input_as_array));
    shared_ptr<const type_t> t = np_to_cu(dtype);

    return make_tuple(d, n, make_shared<const sequence_t>(t), bp_object);
}

//Instantiate scalar packings
PyObject* make_scalar(const float& s) {
    PyObject* result = PyArrayScalar_New(Float);
    PyArrayScalar_ASSIGN(result, Float, s);
    return result;
}

PyObject* make_scalar(const double& s) {
    PyObject* result = PyArrayScalar_New(Double);
    PyArrayScalar_ASSIGN(result, Double, s);
    return result;
}

PyObject* make_scalar(const int& s) {
    PyObject* result = PyArrayScalar_New(Int);
    PyArrayScalar_ASSIGN(result, Int, s);
    return result;
}

PyObject* make_scalar(const long& s) {
    PyObject* result = PyArrayScalar_New(Long);
    PyArrayScalar_ASSIGN(result, Long, s);
    return result;
}

PyObject* make_scalar(const bool& s) {
    if (s) {
        PyArrayScalar_RETURN_TRUE;
    } else {
        PyArrayScalar_RETURN_FALSE;
    }
}

float unpack_scalar_float(PyObject* s) {
    return PyArrayScalar_VAL(s, Float);
}

double unpack_scalar_double(PyObject* s) {
    return PyArrayScalar_VAL(s, Double);
}

int unpack_scalar_int(PyObject* s) {
    return PyArrayScalar_VAL(s, Int);
}

long unpack_scalar_long(PyObject* s) {
    return PyArrayScalar_VAL(s, Long);
}

bool unpack_scalar_bool(PyObject* s) {
    return PyArrayScalar_VAL(s, Bool);
}

copperhead::sp_cuarray unpack_array(PyObject* a) {
    return boost::python::extract<copperhead::sp_cuarray>(a);
}

PyObject* objectify(const copperhead::sp_cuarray& s) {
    return boost::python::converter::shared_ptr_to_python(s);
}


#pragma once

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/karma.hpp>
#include <cstdlib>
#include <iterator>
#include <algorithm>

namespace StarTape
{
    template <typename IteratorT>
    uint64_t parseOctal(IteratorT begin, IteratorT end)
    {
        namespace qi = boost::spirit::qi;
        uint64_t res = 0;
        qi::parse(begin, end, qi::oct, res);
        return res;
        //return std::strtoull(std::string{begin, end}.c_str(), nullptr, 8);
    }

    template <unsigned Width>
    std::string makeOctal(uint64_t number)
    {
        std::string res;
        boost::spirit::karma::generate(std::back_insert_iterator <std::string> (res),
            boost::spirit::maxwidth(Width)   [boost::spirit::oct(number)]
        );
        std::string padding;
        padding.resize(Width - res.length());
        std::fill(std::begin(padding), std::end(padding), '0');
        return padding + res;
    }
}


// link :: https://www.spoj.com/problems/BISHOPS/
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;

int1024_t boost_sub(int1024_t A, long long B)
{
    int1024_t ans = (int1024_t) A - B;
    return ans;
}

int1024_t boost_mult(int1024_t A, long long B)
{
    int1024_t ans = (int1024_t) A * B;
    return ans;
}
int main()
{
    int1024_t temp;
    int i;
    string str;
    while(cin>>temp){
//    	if(str.length() == 1 && str[0] == '1'){
//    		cout<<1<<endl;
//    	}
        if(temp == 1){
            cout<<1<<endl;
            continue;
        }
        if(temp == 0){
        	cout<<0<<endl;
        	continue;
        }
//    	temp = 0;
//    	for(i=0;i<str.length();i++){
//    		temp = boost_mult(temp,10) + (str[i] - '0');
//    	}
    	temp = boost_mult(temp,2);
    	temp = boost_sub(temp,2);
        cout<<temp<<endl;
    }
    return 0;
}


#ifndef TMC_UNIT_TESTS
#define BOOST_TEST_MODULE lib_test_graph_creator
#endif
#include <boost/test/included/unit_test.hpp>
#include <boost/graph/adjacency_list.hpp>
//#include <boost/graph/graphviz.hpp>

#include "sample_creator.h"
#include "graph_creator.h"

BOOST_AUTO_TEST_SUITE( graph_creator )

BOOST_AUTO_TEST_CASE( twelve_points ) {
    std::string names_buffer("| this is comment\n"
                             "target_feature.| one more comment\n"
                             "\n"
                             "   feature1: continuous.   \n"
                             "feature2: continuous.   | third comment\n"
                             "feature3: ignore.\n"
                             "feature4: ignore.\n"
                             "target_feature: v1, v2.\n"
                             "case weight: continuous.\n"
                             "feature5: ignore.\n"
                             "\n"
                             "  | one more comment here\n"
                             "\n");
    std::stringstream ss_names((std::stringstream(names_buffer)));
    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(ss_names);
    std::string data_buffer("1,8,34,44,v2,1,77\n"
                            "2,2,34,44,v1,2,77\n"
                            "2,5,34,44,v2,3,77\n"
                            "3,7,34,44,v2,1,77\n"
                            "3,9,34,44,v1,2,77\n"
                            "4,1,34,44,v2,3,77\n"
                            "4,5,34,44,v2,1,77\n"
                            "5,8,34,44,v1,2,77\n"
                            "7,1,34,44,v2,3,77\n"
                            "6,0,34,44,v2,3,77\n"
                            "7,5,34,44,v2,3,77\n"
                            "8,2,34,44,v2,3,77\n"
    );
    std::stringstream ss_buffer((std::stringstream(data_buffer)));

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_stream(ss_buffer);
    BOOST_TEST_MESSAGE("Sample: " << *pSample);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;

    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample);

    BOOST_CHECK_EQUAL(pSample->size(), pGraphCreator->size());

    //BOOST_CHECK_EQUAL(pGraphCreator->num_edges(), 28);

    BOOST_TEST_MESSAGE("Full induced graph:");
    pGraphCreator->print();
    // { std::ofstream os("full.dot"); boost::write_graphviz(os, *pGraphCreator->get_graph()); os.close(); }

    //pGraphCreator->do_transitive_reduction();
    BOOST_CHECK_EQUAL(pGraphCreator->num_edges(), 15);

    BOOST_TEST_MESSAGE("TR-Reduced graph:");
    pGraphCreator->print();
    // { std::ofstream os("reduced.dot"); boost::write_graphviz(os, *pGraphCreator->get_graph()); os.close(); }
}

BOOST_AUTO_TEST_CASE( thirtysix_points ) {
    const std::string names_file("data/4layers_36points/4layers_36points.names");
    const std::string data_file("data/4layers_36points/4layers_36points.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_TEST_MESSAGE("Resulting sample: " << *pSample);
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 36);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;

    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample);

    BOOST_CHECK_EQUAL(pSample->size(), pGraphCreator->size());

    BOOST_TEST_MESSAGE("Num edges in the full induced graph: " << pGraphCreator->num_edges());
    //pGraphCreator->print();
    //{ std::ofstream os("full.dot"); boost::write_graphviz(os, *pGraphCreator->get_graph()); os.close(); }

    //pGraphCreator->do_transitive_reduction();

    BOOST_TEST_MESSAGE("Num edges in the TR-reduced graph: " << pGraphCreator->num_edges());

    BOOST_CHECK_EQUAL(pGraphCreator->num_edges(), 69);

    //pGraphCreator->print();
    //{ std::ofstream os("reduced.dot"); boost::write_graphviz(os, *pGraphCreator->get_graph()); os.close(); }
}

BOOST_AUTO_TEST_CASE( from_graph_12points ) {
    std::string names_buffer("| this is comment\n"
                             "target_feature.| one more comment\n"
                             "\n"
                             "   feature1: continuous.   \n"
                             "feature2: continuous.   | third comment\n"
                             "feature3: ignore.\n"
                             "feature4: ignore.\n"
                             "target_feature: v1, v2.\n"
                             "case weight: continuous.\n"
                             "feature5: ignore.\n"
                             "\n"
                             "  | one more comment here\n"
                             "\n");
    std::stringstream ss_names((std::stringstream(names_buffer)));
    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(ss_names);
    std::string data_buffer("1,8,34,44,v2,1,77\n"
                            "2,2,34,44,v1,2,77\n"
                            "2,5,34,44,v2,3,77\n"
                            "3,7,34,44,v2,1,77\n"
                            "3,9,34,44,v1,2,77\n"
                            "4,1,34,44,v2,3,77\n"
                            "4,5,34,44,v2,1,77\n"
                            "5,8,34,44,v1,2,77\n"
                            "7,1,34,44,v2,3,77\n"
                            "6,0,34,44,v2,3,77\n"
                            "7,5,34,44,v2,3,77\n"
                            "8,2,34,44,v2,3,77\n"
    );
    std::stringstream ss_buffer((std::stringstream(data_buffer)));

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_stream(ss_buffer);
    BOOST_TEST_MESSAGE("Sample: " << *pSample);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;

    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample);

    BOOST_CHECK_EQUAL(pSample->size(), pGraphCreator->size());
    //BOOST_CHECK_EQUAL(pGraphCreator->num_edges(), 28);
    //pGraphCreator->do_transitive_reduction();
    BOOST_CHECK_EQUAL(pGraphCreator->num_edges(), 15);

    BOOST_TEST_MESSAGE("Base graph:");
    pGraphCreator->print();

    boost::dynamic_bitset<> bs0(std::string("000010011001"));
    boost::dynamic_bitset<> bs1 = bs0; bs1.flip();

    auto pSubGraph0 = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pGraphCreator->get_graph(), bs0);
    auto pSubGraph1 = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pGraphCreator->get_graph(), bs1);

    BOOST_CHECK_EQUAL(pSubGraph0->size(),4);
    BOOST_CHECK_EQUAL(pSubGraph1->size(),8);

    BOOST_CHECK_EQUAL(pSubGraph0->num_edges(),4);
    BOOST_CHECK_EQUAL(pSubGraph1->num_edges(),9);

    BOOST_TEST_MESSAGE("Subgraph 0:");
    pSubGraph0->print();
    //{ std::ofstream os("subgraph0.dot"); boost::write_graphviz(os, *pSubGraph0->get_graph()); os.close(); }

    BOOST_TEST_MESSAGE("Subgraph 1:");
    pSubGraph1->print();
    //{ std::ofstream os("subgraph1.dot"); boost::write_graphviz(os, *pSubGraph1->get_graph()); os.close(); }
}

BOOST_AUTO_TEST_CASE( tmc_paper_dataset ) {
    const std::string names_file("data/tmc_paper/tmc_paper.names");
    const std::string data_file("data/tmc_paper/tmc_paper.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 992);

    typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS> SampleGraphType;
    typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> AuxTrGraphType;

    auto pGraphCreator = std::make_shared<GraphCreator<SampleGraphType, AuxTrGraphType> >(pSample);

    BOOST_CHECK_EQUAL(pSample->size(), pGraphCreator->size());

    BOOST_TEST_MESSAGE("Num edges in the induced graph: " << pGraphCreator->num_edges());

    BOOST_CHECK_EQUAL(pGraphCreator->num_edges(), 5188);

    //pGraphCreator->print();
    //{ std::ofstream os("reduced.dot"); boost::write_graphviz(os, *pGraphCreator->get_graph()); os.close(); }
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_REDUCTION_FUNCTIONS_SIMD_COMMON_NONE_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_REDUCTION_FUNCTIONS_SIMD_COMMON_NONE_HPP_INCLUDED
#include <boost/simd/toolbox/reduction/functions/none.hpp>
#include <boost/simd/include/constants/true.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::none_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<fundamental_<A0>,X>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      for(size_t i=0; i < boost::simd::meta::cardinal_of<A0>::value; i++)
      {
        if(a0[i]) return False<result_type>();
      }
      return True<result_type>();
    }
  };
} } }

#endif


//////////////////////////////////////////////////////////////////////////////
/// Copyright (C) 2014 Gefu Tang <tanggefu@gmail.com>. All Rights Reserved.
///
/// This file is part of LSHBOX.
///
/// LSHBOX 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 3 of the License, or(at your option)
/// any later version.
///
/// LSHBOX 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 LSHBOX. If not, see <http://www.gnu.org/licenses/>.
///
/// @version 0.1
/// @author Gefu Tang & Zhifeng Xiao
/// @date 2014.6.30
//////////////////////////////////////////////////////////////////////////////
#define BOOST_PYTHON_SOURCE
#include <lshbox.h>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
namespace lshbox
{
boost::python::list reshape(const boost::python::list &source)
{
    boost::python::list vec;
    for (unsigned i = 0; i != unsigned(boost::python::len(source)); ++i)
    {
        vec.extend(source[i]);
    }
    return vec;
}
class pyRbsLsh
{
public:
    typedef unsigned DATATYPE;
    void init_file(
        const std::string &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 20,
        unsigned C = 5)
    {
        data.load(source);
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            rbsLsh::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.C = C;
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    void init_mat(
        const boost::python::list &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 20,
        unsigned C = 5)
    {
        std::vector<DATATYPE> vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(reshape(source)),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        data.load(vec, int(boost::python::len(source)), int(boost::python::len(source[0])));
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            rbsLsh::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.C = C;
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    boost::python::list query(boost::python::list &quy, unsigned type, unsigned K)
    {
        std::vector<DATATYPE> quy_vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(quy),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        Matrix<DATATYPE>::Accessor accessor(data);
        Metric<DATATYPE> metric(data.getDim(), type);
        Scanner<lshbox::Matrix<DATATYPE>::Accessor> scanner(
            accessor,
            metric,
            K
        );
        lsh.query(&quy_vec[0], scanner);
        boost::python::list key;
        boost::python::list dist;
        std::vector<std::pair<float, unsigned> > tmp = scanner.topk().getTopk();
        for (auto it = tmp.begin(); it != tmp.end(); ++it)
        {
            key.append(it->second);
            dist.append(it->first);
        }
        boost::python::list result;
        result.append(key);
        result.append(dist);
        return result;
    }
private:
    Matrix<DATATYPE> data;
    rbsLsh lsh;
};
class pyRhpLsh
{
public:
    typedef float DATATYPE;
    void init_file(
        const std::string &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 6)
    {
        data.load(source);
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            rhpLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    void init_mat(
        const boost::python::list &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 6)
    {
        std::vector<DATATYPE> vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(reshape(source)),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        data.load(vec, int(boost::python::len(source)), int(boost::python::len(source[0])));
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            rhpLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    boost::python::list query(boost::python::list &quy, unsigned type, unsigned K)
    {
        std::vector<DATATYPE> quy_vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(quy),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        Matrix<DATATYPE>::Accessor accessor(data);
        Metric<DATATYPE> metric(data.getDim(), type);
        Scanner<lshbox::Matrix<DATATYPE>::Accessor> scanner(
            accessor,
            metric,
            K
        );
        lsh.query(&quy_vec[0], scanner);
        boost::python::list key;
        boost::python::list dist;
        std::vector<std::pair<float, unsigned> > tmp = scanner.topk().getTopk();
        for (auto it = tmp.begin(); it != tmp.end(); ++it)
        {
            key.append(it->second);
            dist.append(it->first);
        }
        boost::python::list result;
        result.append(key);
        result.append(dist);
        return result;
    }
private:
    Matrix<DATATYPE> data;
    rhpLsh<DATATYPE> lsh;
};
class pyThLsh
{
public:
    typedef float DATATYPE;
    void init_file(
        const std::string &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 12)
    {
        data.load(source);
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            thLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.Max = std::numeric_limits<float>::max();
            param.Min = - std::numeric_limits<float>::max();
            for (unsigned i = 0; i != data.getSize(); ++i)
            {
                for (unsigned j = 0; j != data.getDim(); ++j)
                {
                    if (data[i][j] > param.Max)
                    {
                        param.Max = data[i][j];
                    }
                    if (data[i][j] < param.Min)
                    {
                        param.Min = data[i][j];
                    }
                }
            }
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    void init_mat(
        const boost::python::list &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 12)
    {
        std::vector<DATATYPE> vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(reshape(source)),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        data.load(vec, int(boost::python::len(source)), int(boost::python::len(source[0])));
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            thLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.Max = std::numeric_limits<float>::max();
            param.Min = - std::numeric_limits<float>::max();
            for (unsigned i = 0; i != data.getSize(); ++i)
            {
                for (unsigned j = 0; j != data.getDim(); ++j)
                {
                    if (data[i][j] > param.Max)
                    {
                        param.Max = data[i][j];
                    }
                    if (data[i][j] < param.Min)
                    {
                        param.Min = data[i][j];
                    }
                }
            }
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    boost::python::list query(boost::python::list &quy, unsigned type, unsigned K)
    {
        std::vector<DATATYPE> quy_vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(quy),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        Matrix<DATATYPE>::Accessor accessor(data);
        Metric<DATATYPE> metric(data.getDim(), type);
        Scanner<lshbox::Matrix<DATATYPE>::Accessor> scanner(
            accessor,
            metric,
            K
        );
        lsh.query(&quy_vec[0], scanner);
        boost::python::list key;
        boost::python::list dist;
        std::vector<std::pair<float, unsigned> > tmp = scanner.topk().getTopk();
        for (auto it = tmp.begin(); it != tmp.end(); ++it)
        {
            key.append(it->second);
            dist.append(it->first);
        }
        boost::python::list result;
        result.append(key);
        result.append(dist);
        return result;
    }
private:
    Matrix<DATATYPE> data;
    thLsh<DATATYPE> lsh;
};
class pyPsdLsh
{
public:
    typedef float DATATYPE;
    void init_file(
        const std::string &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned T = GAUSSIAN,
        float W = 0.5)
    {
        data.load(source);
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            psdLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.T = T;
            param.W = W;
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    void init_mat(
        const boost::python::list &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned T = GAUSSIAN,
        float W = 0.5)
    {
        std::vector<DATATYPE> vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(reshape(source)),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        data.load(vec, int(boost::python::len(source)), int(boost::python::len(source[0])));
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            psdLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.T = T;
            param.W = W;
            lsh.reset(param);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    boost::python::list query(boost::python::list &quy, unsigned type, unsigned K)
    {
        std::vector<DATATYPE> quy_vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(quy),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        Matrix<DATATYPE>::Accessor accessor(data);
        Metric<DATATYPE> metric(data.getDim(), type);
        Scanner<lshbox::Matrix<DATATYPE>::Accessor> scanner(
            accessor,
            metric,
            K
        );
        lsh.query(&quy_vec[0], scanner);
        boost::python::list key;
        boost::python::list dist;
        std::vector<std::pair<float, unsigned> > tmp = scanner.topk().getTopk();
        for (auto it = tmp.begin(); it != tmp.end(); ++it)
        {
            key.append(it->second);
            dist.append(it->first);
        }
        boost::python::list result;
        result.append(key);
        result.append(dist);
        return result;
    }
private:
    Matrix<DATATYPE> data;
    psdLsh<DATATYPE> lsh;
};
class pyShLsh
{
public:
    typedef float DATATYPE;
    void init_file(
        const std::string &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 4,
        unsigned S = 100)
    {
        data.load(source);
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            shLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.S = S;
            lsh.reset(param);
            lsh.train(data);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    void init_mat(
        const boost::python::list &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 4,
        unsigned S = 100)
    {
        std::vector<DATATYPE> vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(reshape(source)),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        data.load(vec, int(boost::python::len(source)), int(boost::python::len(source[0])));
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            shLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.S = S;
            lsh.reset(param);
            lsh.train(data);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    boost::python::list query(boost::python::list &quy, unsigned type, unsigned K)
    {
        std::vector<DATATYPE> quy_vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(quy),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        Matrix<DATATYPE>::Accessor accessor(data);
        Metric<DATATYPE> metric(data.getDim(), type);
        Scanner<lshbox::Matrix<DATATYPE>::Accessor> scanner(
            accessor,
            metric,
            K
        );
        lsh.query(&quy_vec[0], scanner);
        boost::python::list key;
        boost::python::list dist;
        std::vector<std::pair<float, unsigned> > tmp = scanner.topk().getTopk();
        for (auto it = tmp.begin(); it != tmp.end(); ++it)
        {
            key.append(it->second);
            dist.append(it->first);
        }
        boost::python::list result;
        result.append(key);
        result.append(dist);
        return result;
    }
private:
    Matrix<DATATYPE> data;
    shLsh<DATATYPE> lsh;
};
class pyItqLsh
{
public:
    typedef float DATATYPE;
    void init_file(
        const std::string &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 8,
        unsigned S = 100,
        unsigned I = 50)
    {
        data.load(source);
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            itqLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.S = S;
            param.I = I;
            lsh.reset(param);
            lsh.train(data);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    void init_mat(
        const boost::python::list &source,
        const std::string &index,
        unsigned M = 521,
        unsigned L = 5,
        unsigned N = 8,
        unsigned S = 100,
        unsigned I = 50)
    {
        std::vector<DATATYPE> vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(reshape(source)),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        data.load(vec, int(boost::python::len(source)), int(boost::python::len(source[0])));
        std::ifstream is(index.c_str(), std::ios_base::binary);
        lshbox::timer timer;
        if (is)
        {
            std::cout << "LOADING INDEX ..." << std::endl;
            lsh.load(index);
            std::cout << "LOADING TIME: " << timer.elapsed() << "s." << std::endl;
        }
        else
        {
            std::cout << "CONSTRUCTING INDEX ..." << std::endl;
            itqLsh<DATATYPE>::Parameter param;
            param.M = M;
            param.L = L;
            param.D = data.getDim();
            param.N = N;
            param.S = S;
            param.I = I;
            lsh.reset(param);
            lsh.train(data);
            lsh.hash(data);
            lsh.save(index);
            std::cout << "CONSTRUCTING TIME: " << timer.elapsed() << "s." << std::endl;
        }
    }
    boost::python::list query(boost::python::list &quy, unsigned type, unsigned K)
    {
        std::vector<DATATYPE> quy_vec = std::vector<DATATYPE>(
            boost::python::stl_input_iterator<DATATYPE>(quy),
            boost::python::stl_input_iterator<DATATYPE>()
        );
        Matrix<DATATYPE>::Accessor accessor(data);
        Metric<DATATYPE> metric(data.getDim(), type);
        Scanner<lshbox::Matrix<DATATYPE>::Accessor> scanner(
            accessor,
            metric,
            K
        );
        lsh.query(&quy_vec[0], scanner);
        boost::python::list key;
        boost::python::list dist;
        std::vector<std::pair<float, unsigned> > tmp = scanner.topk().getTopk();
        for (auto it = tmp.begin(); it != tmp.end(); ++it)
        {
            key.append(it->second);
            dist.append(it->first);
        }
        boost::python::list result;
        result.append(key);
        result.append(dist);
        return result;
    }
private:
    Matrix<DATATYPE> data;
    itqLsh<DATATYPE> lsh;
};
}

#ifdef WIN32
    BOOST_PYTHON_MODULE(pylshbox)
#else
    BOOST_PYTHON_MODULE(libpylshbox)
#endif
{
    using namespace boost::python;
    class_<lshbox::pyRbsLsh>("rbslsh", "Locality-Sensitive Hashing Scheme Based on Random Bits Sampling.")
        .def("init_file", &lshbox::pyRbsLsh::init_file, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 20, arg("C") = 5))
        .def("init_mat", &lshbox::pyRbsLsh::init_mat, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 20, arg("C") = 5))
        .def("query", &lshbox::pyRbsLsh::query, (arg("quy"), arg("type") = 2, arg("K") = 10));
    class_<lshbox::pyRhpLsh>("rhplsh", "Locality-Sensitive Hashing Scheme Based on Random Hyperplane.")
        .def("init_file", &lshbox::pyRhpLsh::init_file, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 6))
        .def("init_mat", &lshbox::pyRhpLsh::init_mat, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 6))
        .def("query", &lshbox::pyRhpLsh::query, (arg("quy"), arg("type") = 2, arg("K") = 10));
    class_<lshbox::pyThLsh>("thlsh", "Locality-Sensitive Hashing Scheme Based on Thresholding.")
        .def("init_file", &lshbox::pyThLsh::init_file, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 12))
        .def("init_mat", &lshbox::pyThLsh::init_mat, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 12))
        .def("query", &lshbox::pyThLsh::query, (arg("quy"), arg("type") = 2, arg("K") = 10));
    class_<lshbox::pyPsdLsh>("psdlsh", "Locality-Sensitive Hashing Scheme Based on p-Stable Distributions.")
        .def("init_file", &lshbox::pyPsdLsh::init_file, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("T") = 2, arg("W") = 0.5))
        .def("init_mat", &lshbox::pyPsdLsh::init_mat, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("T") = 2, arg("W") = 0.5))
        .def("query", &lshbox::pyPsdLsh::query, (arg("quy"), arg("type") = 2, arg("K") = 10));
    class_<lshbox::pyShLsh>("shlsh", "Locality-Sensitive Hashing Scheme Based on Spectral Hashing.")
        .def("init_file", &lshbox::pyShLsh::init_file, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 4, arg("S") = 100))
        .def("init_mat", &lshbox::pyShLsh::init_mat, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 4, arg("S") = 100))
        .def("query", &lshbox::pyShLsh::query, (arg("quy"), arg("type") = 2, arg("K") = 10));
    class_<lshbox::pyItqLsh>("itqlsh", "Locality-Sensitive Hashing Scheme Based on Iterative Quantization.")
		.def("init_file", &lshbox::pyItqLsh::init_file, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 8, arg("S") = 100, arg("I") = 50))
        .def("init_mat", &lshbox::pyItqLsh::init_mat, (arg("source"), arg("index"), arg("M") = 521, arg("L") = 5, arg("N") = 8, arg("S") = 100, arg("I") = 50))
		.def("query", &lshbox::pyItqLsh::query, (arg("quy"), arg("type") = 2, arg("K") = 10));
}


#include <boost/python/numpy.hpp>
#include "outbreak.cpp"

namespace p = boost::python;
namespace np = boost::python::numpy;
typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrixXi;

// simulate a batch of outbreaks each with a different R0
np::ndarray simulateR0(np::ndarray &py_R0, uint batch_size, uint seed)
{
    std::mt19937_64 prng(seed);
    params_struct params;

    // convert input R0 to Eigen
    Eigen::Map<Eigen::VectorXd> R0((double *) py_R0.get_data(), batch_size);

    // setup output matrix
    uint n_output = lrint(1. * params.max_time / params.output_interval);
    RowMatrixXi output(batch_size, n_output);

    // mean infectious period
    double mean_inf_period = params.infect_period_shape * params.infect_period_scale;

    Eigen::MatrixXi c;

    // loop over the batch
    for (uint i=0; i<batch_size; ++i)
    {
        // setup simulation-specific params
        params.infect_delta = mean_inf_period / R0[i];

        while (true)
        {
            Outbreak ob(prng, params);
            c = ob.getCounters();
            output.row(i) = c.rowwise().sum() - c.col(0) - c.col(2);

            // Consider only "exploding" outbreak simulations (note effect on prng)
            if ((uint) output.row(i).sum() > 10 * n_output)
                break;
        }
    }

    // convert output to numpy array
    // https://github.com/boostorg/python/issues/97 -> need to copy!
    np::ndarray py_output = np::from_data(output.data(), np::dtype::get_builtin<int>(),
                                          p::make_tuple(batch_size, n_output),
                                          p::make_tuple(sizeof(int)*n_output, sizeof(int)),
                                          p::object());

    // std::cout << "Final: " << std::endl << output << std::endl;
    // std::cout << "Py: " << p::extract<char const*>(p::str(py_output)) << std::endl;
    return py_output.copy();
}

BOOST_PYTHON_MODULE(outbreak4elfi)
{
    Py_Initialize();
    np::initialize();
    boost::python::def("simulateR0", &simulateR0);
}


#include <puppet/compiler/evaluation/operators/binary/multiply.hpp>
#include <puppet/compiler/evaluation/operators/binary/call_context.hpp>
#include <puppet/compiler/exceptions.hpp>
#include <boost/format.hpp>
#include <limits>
#include <cfenv>

using namespace std;
using namespace puppet::runtime::values;

namespace puppet { namespace compiler { namespace evaluation { namespace operators { namespace binary {

    static value multiply_by(call_context& context, int64_t left, int64_t right)
    {
        auto& evaluation_context = context.context();

        bool overflow = false;
        bool underflow = false;
        if (right > 0) {
            if (left > (numeric_limits<int64_t>::max() / right)) {
                overflow = true;
            } else if (left < (numeric_limits<int64_t>::min() / right)) {
                underflow = true;
            }
        } else if (right < -1) {
            if (left > (numeric_limits<int64_t>::min() / right)) {
                underflow = true;
            } else if (left < (numeric_limits<int64_t>::max() / right)) {
                overflow = true;
            }
        } else if (right == -1) {
            if (left == numeric_limits<int64_t>::min()) {
                overflow = true;
            }
        }
        if (overflow) {
            throw evaluation_exception(
                (boost::format("multiplication of %1% and %2% results in an arithmetic overflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        }
        if (underflow) {
            throw evaluation_exception(
                (boost::format("multiplication of %1% and %2% results in an arithmetic underflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        }
        return left * right;
    }

    static value multiply_by(call_context& context, double left, double right)
    {
        auto& evaluation_context = context.context();

        feclearexcept(FE_OVERFLOW | FE_UNDERFLOW);
        double result = left * right;
        if (fetestexcept(FE_OVERFLOW)) {
            throw evaluation_exception(
                (boost::format("multiplication of %1% and %2% results in an arithmetic overflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        } else if (fetestexcept(FE_UNDERFLOW)) {
            throw evaluation_exception(
                (boost::format("multiplication of %1% and %2% results in an arithmetic underflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        }
        return result;
    }

    static value multiply_by(call_context& context, value const& left, value const& right)
    {
        if (left.as<int64_t>() && right.as<int64_t>()) {
            return multiply_by(context, left.require<int64_t>(), right.require<int64_t>());
        }
        if (left.as<int64_t>()) {
            return multiply_by(context, static_cast<double>(left.require<int64_t>()), right.require<double>());
        }
        if (right.as<int64_t>()) {
            return multiply_by(context, left.require<double>(), static_cast<double>(right.require<int64_t>()));
        }
        return multiply_by(context, left.require<double>(), right.require<double>());
    }

    // Declared in divide.cc
    value arithmetic_string_conversion(call_context& context, bool left = true);

    descriptor multiply::create_descriptor()
    {
        binary::descriptor descriptor{ ast::binary_operator::multiply };

        descriptor.add("Numeric", "Numeric", [](call_context& context) {
            return multiply_by(context, context.left(), context.right());
        });
        descriptor.add("String", "Numeric", [](call_context& context) {
            return multiply_by(context, arithmetic_string_conversion(context), context.right());
        });
        descriptor.add("Numeric", "String", [](call_context& context) {
            return multiply_by(context, context.left(), arithmetic_string_conversion(context, false));
        });
        descriptor.add("String", "String", [](call_context& context) {
            return multiply_by(context, arithmetic_string_conversion(context), arithmetic_string_conversion(context, false));
        });
        return descriptor;
    }

}}}}}  // namespace puppet::compiler::evaluation::operators::binary


#pragma once

#include "value/keyword.hpp"
#include "value/numerical.hpp"
#include "value/dimensionless.hpp"
#include "value/string.hpp"
#include "value/url.hpp"
#include "value/point.hpp"
#include "value/color.hpp"

#include <boost/lexical_cast.hpp>

#include <vector>
#include <string>

namespace WretchedCss
{
    using namespace ValueTypes;

    template <typename T>
    struct Valueifier
    {};

    template <>
    struct Valueifier <DimensionlessValue>
    {
        static DimensionlessValue* create(std::vector <RawValue>::const_iterator& begin,
                                          std::vector <RawValue>::const_iterator const&)
        {
            try
            {
                auto* num = new DimensionlessValue (boost::lexical_cast <double> (begin->data));
                ++begin;
                return num;
            }
            catch (boost::bad_lexical_cast const&)
            {
                return nullptr;
            }
        }
    };

    template <>
    struct Valueifier <Color>
    {
        static Color* create(std::vector <RawValue>::const_iterator& begin,
                             std::vector <RawValue>::const_iterator const&)
        {
            auto maybeColor = tryGetColorFromString(begin->data);
            if (!maybeColor)
                return nullptr;

            ++begin;
            return new Color(maybeColor.get());
        }
    };

    template <>
    struct Valueifier <Url>
    {
        static Url* create(std::vector <RawValue>::const_iterator& begin,
                           std::vector <RawValue>::const_iterator const&)
        {
            auto maybeUrl = tryGetUrlFromString(begin->data);
            if (!maybeUrl)
                return nullptr;

            ++begin;
            return new Url(maybeUrl.get());
        }
    };

    template <>
    struct Valueifier <NumericValue>
    {
        static NumericValue* create(std::vector <RawValue>::const_iterator& begin,
                            std::vector <RawValue>::const_iterator const&)
        {
            auto maybeNumericValue = tryGetNumericValueFromString(begin->data);
            if (!maybeNumericValue)
                return nullptr;

            ++begin;
            return new NumericValue(maybeNumericValue.get());
        }
    };

    template <typename... List>
    struct Valueifier <Keyword <List...> >
    {
        static /* Keyword <List...>* */ StringValue* create(std::vector <RawValue>::const_iterator& begin,
                                         std::vector <RawValue>::const_iterator const&)
        {
            Keyword <List...> kw(begin->data);
            if (kw.verify())
			{
				/*
				auto* kw = new Keyword <List...> (begin->data);
				++begin;
				return kw;
				*/
				auto* sv = new StringValue(begin->data);
				++begin;
				return sv;
            }
            else
                return nullptr;
        }
    };

    template <>
    struct Valueifier <StringValue>
    {
        static StringValue* create(std::vector <RawValue>::const_iterator& begin,
                                   std::vector <RawValue>::const_iterator const& end)
        {
            if (begin != end)
			{
                auto* sv = new StringValue(begin->data);
                ++begin;
                return sv;
            }
            return nullptr;
        }
    };

    template <>
    struct Valueifier <Point>
    {
        static Point* create(std::vector <RawValue>::const_iterator&,
                             std::vector <RawValue>::const_iterator const&)
        {
            return nullptr;
            /*
                auto iter = begin;

                if (iter == end)
                    return nullptr;

                auto left = tryGetNumericValueFromString(iter->data);
                if (!left)
                    return nullptr;
                else
                    ++iter;

                boost::optional <NumericValue> top;
                if (iter != end)
                    top = tryGetNumericValueFromString(iter->data);

                if (!top)
                {
                    if (left.get().unit == Unit)
                        top = boost::optional <NumericValue> {NumericValue{Unit::keyword, 0., "center"}};
                    else
                        top = boost::optional <NumericValue> {NumericValue{Unit::percent, 50., ""}};
                }
                else
                    ++iter;

                if (left.get().unit == Unit)
                    if (left.get().strValue == "bottom" || left.get().strValue == "top")
                        std::swap(left, top);

                begin = iter;
                return new Point (left.get(), top.get());
            */
        }
    };

} // namespace WretchedCss


#ifndef STAN_MATH_TORSTEN_PK_NEUT_MPI_TEST_FIXTURE_HPP
#define STAN_MATH_TORSTEN_PK_NEUT_MPI_TEST_FIXTURE_HPP

#include <stan/math/rev/mat.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <test/unit/math/torsten/test_util.hpp>
#include <test/unit/math/torsten/pmx_ode_test_fixture.hpp>
#include <stan/math/torsten/dsolve/pmx_cvodes_fwd_system.hpp>
#include <stan/math/torsten/dsolve/pmx_cvodes_integrator.hpp>
#include <test/unit/math/prim/arr/functor/harmonic_oscillator.hpp>
#include <test/unit/math/prim/arr/functor/lorenz.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

class TorstenPopulationNeutropeniaTest : public testing::Test {
  void SetUp() {
    // make sure memory's clean before starting each test
    stan::math::recover_memory();
    torsten::mpi::Envionment::init();
  }
public:
  TorstenPopulationNeutropeniaTest() :
    f(),
    nCmt(8),
    nt(27),
    time{ 0.000,  0.000,  0.083,  0.167,  0.250,  0.500,  0.750,  1.000,  1.500,  2.000,
      3.000,  4.000,  6.000,  8.000, 12.000, 12.083, 12.167, 12.250, 12.500, 12.750,
      13.000, 13.500, 14.000, 15.000, 16.000, 18.000, 20.000},
    amt(nt, 0),
    rate(nt, 0),
    cmt(nt, 2),
    evid(nt, 0),
    ii(nt, 0),
    addl(nt, 0),
    ss(nt, 0),
    theta(1, {10.74924, 16.83211, 37.33329, 98.06352, 2.001674, 119.6034, 4.787322, 0.201094, 0.000278} ),
    biovar(1, std::vector<double>(nCmt, 1.0)),
    tlag(1, std::vector<double>(nCmt, 0.0)),
    np(8),
    time_m   (np * nt),
    amt_m    (np * nt),
    rate_m   (np * nt),
    cmt_m    (np * nt),
    evid_m   (np * nt),
    ii_m     (np * nt),
    addl_m   (np * nt),
    ss_m     (np * nt),
    theta_m  (np * theta.size()),
    biovar_m (np * biovar.size()),
    tlag_m   (np * tlag.size()),
    len(np)
  {
    SetUp();

    amt[1] = 80000;
    cmt[1] = 1;
    evid[1] = 1;
    ii[1] = 12;
    addl[1] = 1;

    setup_population();
  }

  void setup_population() {
    // population data
    for (int i = 0; i < np; ++i) {
      for (int j = 0; j < nt; ++j) {
        time_m[i * nt + j] = time[j];
        amt_m [i * nt + j] = amt[j];
        rate_m[i * nt + j] = rate[j];
        cmt_m [i * nt + j] = cmt[j];
        evid_m[i * nt + j] = evid[j];
        ii_m  [i * nt + j] = ii[j];
        addl_m[i * nt + j] = addl[j];
        ss_m  [i * nt + j] = ss[j];
      }
    }

    // population param
    for (int i = 0; i < np; ++i) {
      theta_m[i] = theta[0];
      biovar_m[i] = biovar[0];
      tlag_m[i] = tlag[0];
    }

    // population length
    for (int i = 0; i < np; ++i) {
      len[i] = nt;
    }
  }

  /*
   * setup population given length of each individual,
   * so that individual data that are greater than
   * given length is modified by @c std::vector::resize()
   */
  void setup_population(const std::vector<int>& length) {
    time_m.clear();
    amt_m.clear();
    rate_m.clear();
    cmt_m.clear();
    evid_m.clear();
    ii_m.clear();
    addl_m.clear();
    ss_m.clear();
    theta_m.clear();
    biovar_m.clear();
    tlag_m.clear();

    // population data
    for (size_t i = 0; i < length.size(); ++i) {
      assert(length[i] <= nt);
      for (int j = 0; j < length[i]; ++j) {
        time_m.push_back(time[j]);
        amt_m .push_back(amt[j]);
        rate_m.push_back(rate[j]);
        cmt_m .push_back(cmt[j]);
        evid_m.push_back(evid[j]);
        ii_m  .push_back(ii[j]);
        addl_m.push_back(addl[j]);
        ss_m  .push_back(ss[j]);
      }
    }

    // population param
    for (size_t i = 0; i < length.size(); ++i) {
      for (int j = 0; j < length[i]; ++j) {
        theta_m.push_back(theta[0]);
        biovar_m.push_back(biovar[0]);
        tlag_m.push_back(tlag[0]);
      }
    }

    // population length
    len.resize(length.size());
    for (size_t i = 0; i < length.size(); ++i) {
      len[i] = length[i];
    }

    np = length.size();
  }

  const TwoCptNeutModelODE f;
  const int nCmt;
  const int nt;
  std::vector<double> time;
  std::vector<double> amt;
  std::vector<double> rate;
  std::vector<int> cmt;
  std::vector<int> evid;
  std::vector<double> ii;
  std::vector<int> addl;
  std::vector<int> ss;
  std::vector<std::vector<double> > theta;
  std::vector<std::vector<double> > biovar;
  std::vector<std::vector<double> > tlag;
  int np;
  std::vector<double> time_m   ;
  std::vector<double> amt_m    ;
  std::vector<double> rate_m   ;
  std::vector<int   > cmt_m    ;
  std::vector<int   > evid_m   ;
  std::vector<double> ii_m     ;
  std::vector<int   > addl_m   ;
  std::vector<int   > ss_m     ;
  std::vector<std::vector<double> > theta_m;
  std::vector<std::vector<double> > biovar_m ;
  std::vector<std::vector<double> > tlag_m   ;
  std::vector<int> len;
};

#endif


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef IMAGE_UTIL_DETAIL_HPP
#define IMAGE_UTIL_DETAIL_HPP

#include <image/io.hpp>
#ifndef png_infopp_NULL
#define png_infopp_NULL (png_infopp)NULL
#endif

#ifndef int_p_NULL
#define int_p_NULL (int*)NULL
#endif

#include <boost/gil/gil_all.hpp>
#include <image/numeric_extension/sampler.hpp>
#include <image/numeric_extension/resample.hpp>

namespace turi {

namespace image_util_detail {

using namespace boost::gil;

template<typename current_pixel_type, typename new_pixel_type>
void resize_image_detail(const char* data, size_t width, size_t height, size_t channels, size_t resized_width, size_t resized_height, size_t resized_channels, char** resized_data){
  if (data == NULL){
    log_and_throw("Trying to resize image with NULL data pointer");
  }
  size_t len = resized_height * resized_width * resized_channels;
  char* buf = new char[len];
  // Fast path when the sizes are equal.
  if ((width == resized_width) && (height == resized_height) && (channels == resized_channels)) {
    memcpy(buf, data, len);
  } else {
    auto view = interleaved_view(width, height, (current_pixel_type*)data, width * channels * sizeof(char));
    auto resized_view = interleaved_view(resized_width, resized_height, (new_pixel_type*)buf,
                                         resized_width * resized_channels * sizeof(char));
    resize_view(color_converted_view<new_pixel_type>(view), (resized_view), nearest_neighbor_sampler());
  }
  *resized_data = buf;
}

/**
 * Resize the image, and set resized_data to resized image data.
 */
void resize_image_impl(const char* data, size_t width, size_t height, size_t channels, size_t resized_width, size_t resized_height, size_t resized_channels, char** resized_data) {
  // This code should be simplified
  if (channels == 1) {
    if (resized_channels == 1){
      resize_image_detail<gray8_pixel_t, gray8_pixel_t>(data, width, height, channels,
                                                      resized_width, resized_height,
                                                      resized_channels, resized_data);
    } else if (resized_channels == 3){
      resize_image_detail<gray8_pixel_t, rgb8_pixel_t>(data, width, height, channels,
                                                     resized_width, resized_height,
                                                     resized_channels, resized_data);
    } else if (resized_channels == 4){
      resize_image_detail<gray8_pixel_t, rgba8_pixel_t>(data, width, height, channels,
                                                      resized_width, resized_height,
                                                      resized_channels, resized_data);
    } else {
      log_and_throw (std::string("Unsupported channel size ") + std::to_string(channels));
    }
  } else if (channels == 3) {
    if (resized_channels == 1){
      resize_image_detail<rgb8_pixel_t, gray8_pixel_t>(data, width, height, channels,
                                                      resized_width, resized_height,
                                                      resized_channels, resized_data);
    } else if (resized_channels == 3){
      resize_image_detail<rgb8_pixel_t, rgb8_pixel_t>(data, width, height, channels,
                                                     resized_width, resized_height,
                                                     resized_channels, resized_data);
    } else if (resized_channels == 4){
      resize_image_detail<rgb8_pixel_t, rgba8_pixel_t>(data, width, height, channels,
                                                      resized_width, resized_height,
                                                      resized_channels, resized_data);
    } else {
      log_and_throw (std::string("Unsupported channel size ") + std::to_string(channels));
    }
  } else if (channels == 4) {
    if (resized_channels == 1){
      resize_image_detail<rgba8_pixel_t, gray8_pixel_t>(data, width, height, channels,
                                                      resized_width, resized_height,
                                                      resized_channels, resized_data);
    } else if (resized_channels == 3){
      resize_image_detail<rgba8_pixel_t, rgb8_pixel_t>(data, width, height, channels,
                                                     resized_width, resized_height,
                                                     resized_channels, resized_data);
    } else if (resized_channels == 4){
      resize_image_detail<rgba8_pixel_t, rgba8_pixel_t>(data, width, height, channels,
                                                      resized_width, resized_height,
                                                      resized_channels, resized_data);
    } else {
      log_and_throw (std::string("Unsupported channel size ") + std::to_string(channels));
    }
  }
}

void decode_image_impl(image_type& image) {
  if (image.m_format == Format::RAW_ARRAY) {
    return;
  }

  char* buf = NULL;
  size_t length = 0;

  if (image.m_format == Format::JPG) {
    decode_jpeg((const char*)image.get_image_data(), image.m_image_data_size,
                &buf, length);
  } else if (image.m_format == Format::PNG) {
    decode_png((const char*)image.get_image_data(), image.m_image_data_size,
                &buf, length);
  } else {
    log_and_throw(std::string("Cannot decode image. Unknown format."));
  };
  image.m_image_data.reset(buf);
  image.m_image_data_size = length;
  image.m_format = Format::RAW_ARRAY;
}

void encode_image_impl(image_type& image) {
  if (image.m_format != Format::RAW_ARRAY){
    return;
  }

  char* buf = NULL;
  size_t length = 0;

  encode_png((const char*)image.get_image_data(), image.m_width, image.m_height, image.m_channels, &buf, length);
  image.m_image_data.reset(buf);
  image.m_image_data_size = length;
  image.m_format = Format::PNG;
}

} // end of image_util_detail
} // end of turicreate
#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_COMMON_IS_EQUAL_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_COMMON_IS_EQUAL_HPP_INCLUDED

#include <boost/simd/operator/functions/is_equal.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/meta/as_arithmetic.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/sizeof.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF         ( is_equal_
                                      , boost::simd::tag::cpu_
                                      , (A0)(X)
                                      , (mpl::equal_to< mpl::sizeof_<A0>
                                                      , mpl::sizeof_<typename A0::type>
                                                      >
                                        )
                                      , ((simd_<logical_<A0>,X>))
                                        ((simd_<logical_<A0>,X>))
                                      )
  {
    typedef A0                                                result_type;
    typedef typename meta::as_arithmetic<A0>::type            base_t;
    typedef typename dispatch::meta::as_integer<base_t>::type cast_t;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return bitwise_cast<A0> ( eq( bitwise_cast<cast_t>(a0)
                                  , bitwise_cast<cast_t>(a1)
                                  )
                              );
    }
  };
} } }

#endif


#include <ros/ros.h>
#include <memory>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
#include <moveit/robot_state/conversions.h>

#include <geometry_msgs/PointStamped.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>

#include <stdlib.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <random>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "moveit_demo");

  ros::AsyncSpinner spinner(8);
  spinner.start();

  std::vector<double> joint_values;
  moveit::planning_interface::MoveGroupInterface rng_group_interface("rocket_and_groot");
  moveit::planning_interface::MoveGroupInterface rocket_group_interface("rocket");
  moveit::planning_interface::MoveGroupInterface groot_group_interface("groot");

  rng_group_interface.setMaxVelocityScalingFactor(1.0);
  rng_group_interface.setMaxAccelerationScalingFactor(1.0);
  rng_group_interface.setPlanningTime(15.0);
  rng_group_interface.setNumPlanningAttempts(20.0);

  moveit::core::RobotModelConstPtr kinematic_model = rocket_group_interface.getRobotModel();
  moveit::core::RobotStatePtr kinematic_state = rng_group_interface.getCurrentState();
  const moveit::core::JointModelGroup* rocket_joint_model_group = kinematic_model->getJointModelGroup("rocket");
  const moveit::core::JointModelGroup* groot_joint_model_group = kinematic_model->getJointModelGroup("groot");

  const std::vector<std::string>& rocket_joint_names = rocket_joint_model_group->getVariableNames();
  const std::vector<std::string>& groot_joint_names = groot_joint_model_group->getVariableNames();
  std::vector<double> rocket_joint_values;
  std::vector<double> groot_joint_values;

  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<> distr(-10, 10);
  std::uniform_int_distribution<> rad_distr(-30, 30);

  geometry_msgs::Pose rocket_pose;
  geometry_msgs::Pose groot_pose;

  // w x y z
  Eigen::Quaternionf rocket_q = Eigen::Quaternionf(0.0044319521005895665 , -0.0018064082028716572, 0.714190127940822, -0.6999353940485185);
  Eigen::Quaternionf groot_q = Eigen::Quaternionf(0.7171097271676862 , -0.6959453209354478, -0.029260144371181365, -0.02361341612136324);

  for(int i; i < 100; i++){

    float random_x = ( ((float) distr(gen)) * 0.01);
    float random_y = ( ((float) distr(gen)) * 0.01);
    float random_z = ( ((float) distr(gen)) * 0.01);

    rocket_pose.position.x = -0.015463195119993365 + random_x;
    rocket_pose.position.y = 0.02029402510664674 + random_y;
    rocket_pose.position.z = 1.658157440477098 + random_z;
    groot_pose.position.x = -0.01565011581780207 + random_x;
    groot_pose.position.y = -0.019683543216663102 + random_y;
    groot_pose.position.z = 1.657396455658871 + random_z;

    float x_rotation = rad_distr(gen) * 0.01;
    float y_rotation = rad_distr(gen) * 0.01;
    float z_rotation = rad_distr(gen) * 0.01;

    rocket_q = Eigen::AngleAxisf(x_rotation, Eigen::Vector3f::UnitX()) *
                Eigen::AngleAxisf(y_rotation, Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(z_rotation, Eigen::Vector3f::UnitZ()) *
                rocket_q;

    groot_q = Eigen::AngleAxisf(x_rotation, Eigen::Vector3f::UnitX()) *
                Eigen::AngleAxisf(y_rotation, Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(z_rotation, Eigen::Vector3f::UnitZ()) *
                groot_q;

    rocket_pose.orientation.w = rocket_q.w();
    rocket_pose.orientation.x = rocket_q.x();
    rocket_pose.orientation.y = rocket_q.y();
    rocket_pose.orientation.z = rocket_q.z();
    groot_pose.orientation.w = groot_q.w();
    groot_pose.orientation.x = groot_q.x();
    groot_pose.orientation.y = groot_q.y();
    groot_pose.orientation.z = groot_q.z();

    double timeout = 0.1;
    bool rocket_found_ik = kinematic_state->setFromIK(rocket_joint_model_group, rocket_pose, timeout);
    bool groot_found_ik = kinematic_state->setFromIK(groot_joint_model_group, groot_pose, timeout);

    if (rocket_found_ik && groot_found_ik)
    {
      kinematic_state->copyJointGroupPositions(rocket_joint_model_group, rocket_joint_values);
      kinematic_state->copyJointGroupPositions(groot_joint_model_group, groot_joint_values);

      for (std::size_t i = 0; i < rocket_joint_names.size(); ++i)
      {
        ROS_INFO("Joint %s: %f", rocket_joint_names[i].c_str(), rocket_joint_values[i]);
        ROS_INFO("Joint %s: %f", groot_joint_names[i].c_str(), groot_joint_values[i]);
      }
    }
    else
    {
      ROS_INFO("Did not find IK solution");
    }
    rng_group_interface.setJointValueTarget(rocket_joint_names, rocket_joint_values);
    rng_group_interface.setJointValueTarget(groot_joint_names, groot_joint_values);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (rng_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if(!success){
      ROS_INFO("Plan did not successed");
    }
    rng_group_interface.execute(my_plan);
  }

  ros::shutdown();
}