/*
 * F.cc
 *
 *  Created on: Apr 13, 2014
 *      Author: wilfeli
 */
#include <list>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <math.h>
#include <string>

#include "Tools.h"
#include "F.h"
#include "W.h"
#include "Contract.h"
#include "Market.h"
#include "H.h"

//using std::shared_ptr;
//using std::unique_ptr;
using Eigen::MatrixXd;
using namespace boost;
//using Tools::mgrid;

//will pass w as itself, it will be converted to reference
F::F(W &w_, int id_):w(w_),
					l_bid(0.0, 0.0),
					c_ask(0.0, 0.0),
					id(id_),
					s0(1,19),
					theta_x(1,4){
	w.AddF(this);

	//create storage for goods
	ass_asGC.push_back(new GoodC(this, 0.0));

	type = "FGC";

	l_bid.issuer = this;
	c_ask.issuer = this;

	//create wm
	ExpectationBackward* exp = new ExpectationBackward(1.0, 10, 0.5);
	wm["MasCHK"] = exp;

	//create wm
	exp = new ExpectationBackward(1.0, 10, 0.5);
	wm["MasGC"] = exp;

	//parameters
	param = new Parameters();
	param->init();

	//accounting
	account = new AccountF();

};

void
F::init(){
    //ADP setup
    theta_V = Eigen::MatrixXd::Constant(1, 1, param->ADP_BF_N);
    B_n_1_RLS = Eigen::MatrixXd::Zero(param->ADP_BF_N, param->ADP_BF_N);

    //QL setup
    _set_QL();

    //mRe setup
    _set_mRE();

};

double
F::get_status(){
	return status;
};

void
F::ac_W_begin_step(){
	//begin step - pay off obligations
	//check that has enough money to pay all
	double q_all = 0.0;
	double pq = 0.0;
	MessagePSSendPayment* inf;
	bool PAY_DIV = false;

	for (auto c:ass_asCHK){
		if(c->t_end == w.t - 1){
			q_all += c->q * c->p;
		};
	};

	bool PAY_ALL = false;

	if (q_all <= ass_asCBDt0.front()->q){
		PAY_ALL = true;
	};

	for (auto c:ass_asCHK){

		if(c->t_end == w.t - 1){

			pq = PAY_ALL?c->q * c->p:(c->q * c->p) * ass_asCBDt0.front()->q /q_all;

			//make payment
			inf = new MessagePSSendPayment(c->issuer, pq ,"asCHK_b");
			inf->sender = this;

			ass_asCBDt0.front()->issuer->_PS_accept_PO(inf);

			delete inf;

		};
	};

	if (w.t > 0.0){

		//check last period profit
		q_all = (account->profit.back() > 0.0)? account->profit.back() * param->DIV_SHARE: 0.0;

		if (q_all > 0.0) {
			PAY_DIV = true;
			if (q_all <= ass_asCBDt0.front()->q){
				PAY_ALL = PAY_ALL && true;
			} else{
				PAY_ALL = false;
			};
		};

		//pay off dividends
		if (PAY_ALL  && PAY_DIV){
			pq = q_all/w.NH;
			for (auto a:w.Hs){
				//make payment
				inf = new MessagePSSendPayment(pq ,static_cast<std::string>("FI_div"));
				inf->getter = a; //dynamic_cast<IPSAgent*>(a);
				inf->sender = this;

				ass_asCBDt0.front()->issuer->_PS_accept_PO(inf);

				delete inf;
			};

		};
	};

	//checks for bankruptcy
	if ((ass_asCBDt0.front()->q < 0.0) || !PAY_ALL){
		//calls bankruptcy
		MessageBankruptcy* mes = new MessageBankruptcy(this);
		w.ac_LS_bankruptcy(mes);

		delete mes;

	};

	//clear contracts
	ass_asCHK.erase(std::remove_if(ass_asCHK.begin(), ass_asCHK.end(),
	                       [&](ContractHK* x) -> bool { return (x->t_end <= w.t); }),
			ass_asCHK.end());

//	for (auto c:ass_asCHK){
//		std::cout << true << std::endl;
//	};

	account->ac_W_initialize_step();

};

void
F::ac_W_begin_step(MessageStatus* mes){
	if (mes->status <= 0.0){
		account->ac_W_initialize_step(mes);
	};
};

void
F::ac_W_end_step(){
	// calls accounting to calculate profit
	_s0();
	account->ac_W_end_step(&s0);

};

double
F::_mas_q_sell(MessageMarketCCheckAsk* inf){
	//checks how much to sell
	//return q from ask

	double q_sell = 0.0;

	if (inf->p_eq >= c_ask.p){
		q_sell = c_ask.q;
	};

	return q_sell;
};

double
F::_mas_q_buy(MessageMarketLCheckBid* inf){
	//checks how much to sell
	//return q from ask

	double q_buy = 0.0;

	if (inf->p_eq <= l_bid.p){
		q_buy = l_bid.q;
	};

	return q_buy;
};

void
F::_buy_asCHK(ContractHK* c){
	//add to contract
	ass_asCHK.push_back(c);

	//update ask
	l_bid.q -= c->q;

	//message for accounting
	Message* mes = new Message("buy_asCHK");

	//call accounting
	account->ac_get_inf(mes, c);

	delete mes;

};

void
F::wm_update_k_s(){

	if (w.t > 0.0){

		MatrixXd w0 = wm_w0_tbeg();
		ExpectationBackward* exp_i;
		std::vector<double> s_t_i;

		//update k_s if conditions are met
//		if ((w0(0,1) > 0.0) || ((w0(0,1) == 0.0) && (w0(0,0) != 0.0)) || ((w0(0,4) > 0.0) && (w0(0,0) == 0.0))){
			s_t_i.push_back(w0(0,0));
			s_t_i.push_back(w0(0,1));

			exp_i = wm["MasCHK"];

			exp_i->s_t.push_back(s_t_i);

			wm_update_expectation_backward(exp_i, 1);
//		};

		s_t_i.clear();

		s_t_i.push_back(w0(0,2));
		s_t_i.push_back((w0(0,2) > 0.0)? w0(0,3)/w0(0,2):w0(0,5));

		exp_i = wm["MasGC"];

		exp_i->s_t.push_back(s_t_i);

		wm_update_expectation_backward(exp_i, 1);

		s_t_i.clear();

        if (param->opt_TYPE == "QL"){
            wm_update_Q();

        };

        if (param->opt_TYPE == "mRE"){
            wm_update_mRE();

        };

	};

};

Eigen::MatrixXd
F::wm_w0_tbeg(){
	MatrixXd w0(1,8);

	long life_length = account->asCHK_q.size();

	w0(0,0) = account->asCHK_q.at(life_length - 2);
	w0(0,1) = w.ml->market_price.at(w.t - 1)->p;
	w0(0,2) = account->sales_q.at(life_length - 2);
	w0(0,3) = account->sales_pq.at(life_length - 2);

	w0(0,4) = l_bid.q_t_0;
	w0(0,5) = w.mc->market_price.at(w.t - 1)->p;
	w0(0,6) = c_ask.q_t_0;
	w0(0,7) = account->profit.at(life_length - 2);

//	std::cout << w0 << std::endl;

	return w0;
};

void
F::wm_update_expectation_backward(ExpectationBackward* exp_i, int p_position){

	std::vector<double> p;

	int i_s_t_1_max = 0;
	long i_s_t_max = 0;

	//depending on the length of the wm and accumulated number of prices
	if (std::isinf(param->WM_LENGTH)){
		i_s_t_1_max = exp_i->s_t_1["n"];
		i_s_t_max = exp_i->s_t.size();
	}else{
		i_s_t_1_max = std::min(std::max(param->WM_LENGTH - exp_i->s_t.size(), 0.0), exp_i->s_t_1["n"]);
		i_s_t_max = std::min(param->WM_LENGTH, static_cast<double>(exp_i->s_t.size()));
	};

	for (int i = 0; i < i_s_t_1_max; i++){
		p.push_back(exp_i->s_t_1["mu"]);
	};

	//push other prices
	for (std::size_t i = (exp_i->s_t.size() - i_s_t_max);i < exp_i->s_t.size();i++){
		p.push_back(exp_i->s_t[i][p_position]);
	};

//	Tools::print_vector(p);

	Eigen::MatrixXd mean_variance = wm_mean_variance(p);

	exp_i->mu = mean_variance(0,0);
	exp_i->n += 1.0;
	exp_i->v = mean_variance(1,0);

    if (exp_i->v <= 0.0){
        exp_i->v = exp_i->mu * 0.01;
    };

};

Eigen::MatrixXd
F::wm_mean_variance(std::vector<double> &x){
	MatrixXd mean_variance(2,1);

	//gets mean and variance
	double sum = std::accumulate(x.begin(), x.end(), 0.0);
	double mean = sum / x.size();
	std::vector<double> diff(x.size());
	std::transform(x.begin(), x.end(), diff.begin(),
	               std::bind2nd(std::minus<double>(), mean));
	double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
//	double stdev = std::sqrt(sq_sum / x.size());
	double variance = sq_sum/x.size();

	mean_variance(0,0) = mean;
	mean_variance(1,0) = variance;

	return mean_variance;
};

Eigen::MatrixXd
F::_s0(){
	//money holdings
	s0(0,0) = ass_asCBDt0.front()->q;

	s0(0,1) = 0.0;
	//human capital for production
	for (auto c:ass_asCHK){
		s0(0,1) += c->q;
	};

	//amount of good c
	s0(0,2) = ass_asGC.front()->q;

	//expected price on labor market
	s0(0,3) = wm["MasCHK"]->mu;

	//number of observations
	s0(0,4) = wm["MasCHK"]->n;

	//expected price on goods market
	s0(0,5) = wm["MasGC"]->mu;

	//number of observations
	s0(0,6) = wm["MasGC"]->n;

	//p*q - total costs
	s0(0,7) = account->cost_p * account->cost_q;

	//cost - q
	s0(0,8) = account->cost_q;

	//current period labor contracts
	s0(0,9) = 0.0;

	//current period labor price
	s0(0,10) = 0.0;

	//current period sales q
	s0(0,11) = 0.0;

	//current period sales p
	s0(0,12) = 0.0;

	//current period production
	s0(0,13) = 0.0;

	//current profit
	s0(0,14) = 0.0;

	//state dead/alive
	s0(0,15) = status;

	//share of dividends
	s0(0,16) = param->DIV_SHARE;

	//variance of labor market
	s0(0,17) = wm["MasCHK"]->v;

	//variance of goods market
	s0(0,18) = wm["MasGC"]->v;

	return s0;
};

Bid*
F::ac_L(){
	//update numbers in bid
	l_bid.demand_curve = Eigen::MatrixXd::Zero(1,2);
	l_bid.demand_curve << l_bid.p , l_bid.q;

	l_bid.p_t_0 = l_bid.p;
	l_bid.q_t_0 = l_bid.q;

	//return reference to the bid
	return &l_bid;
};

Ask*
F::ac_C(){
	//update numbers in ask
	c_ask.q = _s0()(0,2) * c_ask.q_share;

	c_ask.p_t_0 = c_ask.p;
	c_ask.q_t_0 = c_ask.q;
	c_ask.q_share_t_0 = c_ask.q_share;

	//update numbers in ask
	c_ask.supply_curve = Eigen::MatrixXd::Zero(1,2);
	c_ask.supply_curve << c_ask.p , c_ask.q;

//	std::cout << ass_asCHK.size() << "\n";
//
//	std::cout << c_ask.supply_curve << "\n";

	//return reference to the bid
	return &c_ask;
};

bool
F::_sell_asGC(MessageMarketCSellGoodC* mes){
	//decrease offer
	c_ask.q -= mes->q;

	//sell actual good
	ass_asGC.back()->q -= mes->q;

	MessageGoodC* mgc = new MessageGoodC(mes->q, mes->p);

	//send goods to the buyer
	mes->buyer->_buy_asGC(mgc);

    //delete message
    delete mgc;

	//account sale
	account->ac_get_inf(mes);

	return true;

};

void
F::ac_W(MessageMakeDec* mes){

    _s0();
	//calls opt choice
    if (param->opt_TYPE == "EO_CS"){
        opt_CS(&s0,&theta_x, param->opt_CS_N);
    };

    if (param->opt_TYPE == "EO_ADP"){
        theta_V = _API_LM(&s0, param->ADP_API_LM_N, param->ADP_API_LM_M);
        Eigen::Block<Eigen::MatrixXd> s = s0.block(0,0,s0.rows(),s0.cols());
        _opt_CS_s_V(s,theta_x, theta_V, param->opt_CS_N);
    };

    if (param->opt_TYPE == "QL"){
        opt_QL();
    };

    if (param->opt_TYPE == "mRE"){
        opt_mRE();

    };

	//transforms it into ask and bid
	l_bid.q = floor(theta_x(0,0));
	l_bid.p = theta_x(0,1) * s0(0,3);

	c_ask.q_share = theta_x(0,2);
	c_ask.p = theta_x(0,3) * s0(0,5);

};

void
F::ac_W(MessageF_F* mes){
	//calls production function
	_s0();
	mes->q = _F_F(&s0);
	ass_asGC.back()->q += mes->q;
	account->ac_get_inf(mes);

};

double
F::_F_F(Eigen::MatrixXd* s){
	double l;

	l = (*s)(0,1);

	return param->F_F_theta(0,0) * pow(l, param->F_F_theta(0,1));
};

void
F::opt_CS(MatrixXd* s0,  MatrixXd* theta_x, int N){

//	N = 1;

	//gets matrix of thetas
	//prepare initial matrix
	MatrixXd m_s0 = MatrixXd::Zero(N, s0->cols());

	m_s0.rowwise() += s0->row(0);

	//allocate space to the matrix
	MatrixXd n_w;
	MatrixXd n_x;
	MatrixXd n_s = MatrixXd::Zero(N, s0->cols());
	MatrixXd n_c;

	//create matrix for the grid - small size here
	MatrixXd grid(4,3);

    create_decision_grid(grid,0);

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(grid, &theta_x_all);

    } else{

        Tools::mgrid(grid, &theta_x_all);
    };

	//create random number generators for the implementation

	boost::normal_distribution<> nd_w1((*s0)(0,3), pow((*s0)(0,17),0.5));
	boost::normal_distribution<> nd_w2((*s0)(0,5), pow((*s0)(0,18),0.5));

	boost::variate_generator<boost::mt19937&,
	                           boost::normal_distribution<>> rng_w1(w.rng, nd_w1);

	boost::variate_generator<boost::mt19937&,
	                           boost::normal_distribution<>> rng_w2(w.rng, nd_w2);

    if (w.param->SIMULATION_MODE != "test"){

        //draw random variables
        n_w = MatrixXd::Zero(N, 2 * param->T_MAX);
        for (int i = 0; i < N; i++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%2 == 0){
                    n_w(i,j) = rng_w1();
                } else {
                    n_w(i,j) = rng_w2();
                };
                n_w(i,j) = std::max(0.0, (double)n_w(i,j));
            };
        };
    };

	//matrix of all
	MatrixXd c_all(theta_x_all.cols(),1);

	for (int i = 0 ; i < theta_x_all.cols(); i++){
        //redraw random in comparative with Python mode
        if (w.param->SIMULATION_MODE == "test"){
            n_w = MatrixXd::Zero(N, 2 * param->T_MAX);
            for (int k = 0; k < N; k++){
                for(int j=0; j< n_w.cols(); j++){
                    if (j%2 == 0){
                        n_w(k,j) = Tools::get_normal((*s0)(0,3), pow((*s0)(0,17),0.5), w.myrng);
                    } else {
                        n_w(k,j) = Tools::get_normal((*s0)(0,5), pow((*s0)(0,18),0.5), w.myrng);
                    };
                    n_w(k,j) = std::max(0.0, (double)n_w(k,j));
                };
            };
        };

		//call estimator of results of steps, given theta_x
		theta_x->row(0) = theta_x_all.col(i).transpose();
		n_x = MatrixXd::Zero(N, 4);
		n_s = m_s0;
		n_c = MatrixXd::Zero(N, param->T_MAX);
		_step_opt_CS(theta_x, &n_w, &n_x, &n_s, &n_c, param->T_MAX);

//        std::cout << n_c << std::endl;
//        std::cout << theta_x->row(0) << std::endl;

		//after n_c
		//average over n runs
		c_all(i,0) = (n_c * param->BETA_T).mean();

	};

//	std::cout << n_c << "\n";

//	std::cout << c_all.transpose() << "\n";

	//find all max elements
	double max = c_all(0,0);
	std::list<int> i_max;
	for (int i=0; i<c_all.rows(); i++){
		if (c_all(i,0) == max){
			i_max.push_back(i);
		}else{
			if (c_all(i,0) > max){
				max = c_all(i,0);
				i_max.clear();
				i_max.push_back(i);
			};
		}
	};

	//if more than 1 max - randomly pick
	long theta_max_i;
	if (i_max.size()>1){
		boost::random::uniform_int_distribution<> i_max_dist(0, i_max.size()-1);
		theta_max_i = i_max_dist(w.rng);
	}else{
		theta_max_i = i_max.front();
	};

	theta_x->row(0) = theta_x_all.col(theta_max_i).transpose();
};

void
F::_step_opt_CS(MatrixXd *theta_x,
		MatrixXd *w,
		MatrixXd *x,
		MatrixXd *s,
		MatrixXd *c,
                int T){

	//sets decisions for labor market
	//zero vector
	auto s_zero = MatrixXd::Zero(s->rows(),1);

	for (int i=0; i< T; i++){

		x->col(0) = Eigen::MatrixXd::Constant(x->rows(), 1, (*theta_x)(0,0));
		x->col(1) = (*theta_x)(0,1) * s->col(3).array();

//        std::cout << *x << std::endl;

		//labor market results
		//if wage is less than realized wage - zero, otherwise hire all desired number

		(*s).col(9) = ((*w).col(i*2).array() <= x->col(1).array()).cast<double>().array() *x->col(0).array();
		(*s).col(10) = (*w).col(i*2);

        //cast to whole numbers
        for (int j=0; j < s->rows(); j++){
            (*s)(j,9) = floor((*s)(j,9));
        };

//        std::cout << *s << std::endl;

		//current production
		s->col(13) = s->col(9).array().pow(param->F_F_theta(0,1))*param->F_F_theta(0,0);
		s->col(2) += s->col(13);

//        std::cout << *s << std::endl;

		//update for cost structure
		s->col(7) = s->col(7).array() + ((*s).col(9).array() * (*s).col(10).array());
		s->col(8) += s->col(13);

//        std::cout << *s << std::endl;

		//sets decision for goods market
		x->col(2) = (*theta_x)(0,2) * s->col(2).array();
		x->col(3) = Eigen::MatrixXd::Constant(x->rows(), 1, (*theta_x)(0,3) * (*s)(0,5));

//        std::cout << *x << std::endl;

		//sales
		(*s).col(11) = ((*w).col(i*2 + 1).array() >= x->col(3).array()).cast<double>().array() *x->col(2).array();
		(*s).col(12) = (*w).col(i*2 + 1);

//        std::cout << *s << std::endl;

		//money
		s->col(0) = s->col(0).array() + (s->col(11).array() * s->col(12).array());

//        std::cout << *s << std::endl;

		//inventory
		s->col(2) -= s->col(11);

//        std::cout << *s << std::endl;

		double atc = 0.0;

		//updates current profit
		for (int j=0; j < s->rows(); j++){

			if ((*s)(j,8)>0.0){
				atc = (*s)(j,7) / (*s)(j,8);

                if (param->ACCOUNTING_TYPE=="modern"){
                    //update profit to cost of production
                    (*s)(j,14) -= atc * (*s)(j,11);
                };
                if (param->ACCOUNTING_TYPE=="classic"){
                    //update profit to cost of production
                    (*s)(j,14) -= (*s)(j,9) * (*s)(j,10);
                };

				//update cost of production
				(*s)(j,7) -= atc * (*s)(j,11);
				(*s)(j,8) -= (*s)(j,11);
			};

			//update profit to sales
			(*s)(j,14) += (*s)(j,11) * (*s)(j,12);

			atc = 0.0;
		};

//        std::cout << *s << std::endl;

		//labor payment
		s->col(0) = s->col(0).array() -  (s->col(9).array() * s->col(10).array());

//        std::cout << *s << std::endl;

		//dividends
		s->col(0) = s->col(0).array() - (s->col(14).array() > s_zero.array()).cast<double>().array() * s->col(14).array() * s->col(16).array();

//        std::cout << *s << std::endl;

		//zero out labor contract
		s->col(9) = s_zero;
		s->col(10) = s_zero;

        //dead or alive
		s->col(15) = (s->col(0).array() >= s_zero.array()).cast<double>().array() * s->col(15).array();

//        std::cout << *s << std::endl;

		//stores realized goal
		c->col(i) = (s->col(15).array() > s_zero.array()).cast<double>().array() * s->col(14).array();

//        std::cout << *c << std::endl;

		//zero out current profit
		s->col(14) = s_zero;

	};

};

void
F::_bf(Eigen::MatrixXd* s, Eigen::MatrixXd* ret){
    (*ret)(0,0) = (*s)(0,0);
};

void
F::_bf(Eigen::Block<Eigen::MatrixXd>& s, Eigen::MatrixXd* ret){
    (*ret)(0,0) = s(0,0);
};

double
F::_V(Eigen::MatrixXd* s, Eigen::MatrixXd* phi_f, Eigen::MatrixXd& theta_V){
    _bf(s, phi_f);

    return (theta_V*(*phi_f).transpose()).sum();

};

double
F::_V(Eigen::Block<Eigen::MatrixXd>& s, Eigen::MatrixXd* phi_f, Eigen::MatrixXd& theta_V){
    _bf(s, phi_f);

    return (theta_V*(*phi_f).transpose()).sum();

};

Eigen::MatrixXd
F::_API_LM(Eigen::MatrixXd* s0, int N, int M){
//Approximate policy iteration using linear models.
//
//p.407 ADP
//

    //fix basis functions
    long n_theta_t = param->ADP_BF_N;

    //inner theta
    //theta for now and future
    Eigen::MatrixXd theta_V_n = Eigen::MatrixXd::Constant(1, n_theta_t, 1);

    //policy theta
    Eigen::MatrixXd theta_V_pi = theta_V;

    Eigen::MatrixXd s_n_m;
    Eigen::MatrixXd v_n_m;
    Eigen::MatrixXd x_n_m(1,4);
    Eigen::MatrixXd phi(1, param->ADP_BF_N);
    Eigen::MatrixXd phi1(1, param->ADP_BF_N);

    for (int n=0; n<N; n++){
        theta_V_n = theta_V_pi;

        s_n_m = MatrixXd::Zero(M+1, s0->cols());
        s_n_m.row(0) += s0->row(0);
        //v_n_m_T1 = 0
        v_n_m = Eigen::MatrixXd::Constant(M, n_theta_t, 0.0);

        for (int m=0; m<M; m++){

            Eigen::Block<Eigen::MatrixXd> s = s_n_m.block(m,0,1,(*s0).cols());

            //choose action, takes reference to s
            _opt_CS_s_V(s, x_n_m, theta_V_pi, param->opt_CS_N);

            s_n_m.block(m+1,0,1,(*s0).cols()) = s;

            Eigen::Block<Eigen::MatrixXd> s1 = s_n_m.block(m+1,0,1,(*s0).cols());
            s = s_n_m.block(m,0,1,(*s0).cols());

            //make step given action, to update state and return value function
            double _c = _c_theta_x(x_n_m, s1, theta_V_pi);

            double c_n_m = _c - _V(s1,&phi,theta_V_pi);

            _bf(s,&phi);
            _bf(s1,&phi1);
            Eigen::MatrixXd v_n_m = phi - param->BETA*phi1;

//            std::cout << s_n_m.row(m) << std::endl;
//            std::cout << s << std::endl;
//
//            std::cout << s_n_m.row(m+1) << std::endl;
//            std::cout << theta_V_pi << std::endl;
//            std::cout << theta_V_n << std::endl;

            //update theta_V_n
            theta_V_n = _RLS(theta_V_n,
                            v_n_m,
                            s,
                            m,
                            c_n_m);
            //assume that only 1 element in v and restrict it
            if (theta_V_n(0,0) > 1000){
                theta_V_n(0,0) = 1000;
            };
            if (theta_V_n(0,0) < 0.0){
                theta_V_n(0,0) = 0.01;
            };

        };

        theta_V_pi = theta_V_n;
    };

    //std::cout << theta_V_pi << std::endl;
    return theta_V_pi;
};

Eigen::MatrixXd
F::_RLS(Eigen::MatrixXd& theta, Eigen::MatrixXd& v, Eigen::Block<Eigen::MatrixXd>& s, int i, double c){

    //i - iteration if i = 0 - initialize B
    //for each theta_t:
    Eigen::MatrixXd B_n_1;

    if (i == 0){
        //identity matrix
        double e_B_0 = 0.0005;

        //B(n-1)
        B_n_1 = e_B_0 * Eigen::MatrixXd::Identity(param->ADP_BF_N, param->ADP_BF_N);

    }else{
        B_n_1 = B_n_1_RLS;
    };

    //container for basis functions
    Eigen::MatrixXd phi(1, param->ADP_BF_N);
    _bf(s,&phi);

    double e_n = c - (v.transpose()*theta)(0,0);

    double gamma_n = 1 + ((v.transpose()*B_n_1)*phi)(0,0);

    Eigen::MatrixXd B_n = B_n_1 - 1/gamma_n *((B_n_1*(phi*v.transpose()))*B_n_1);

    Eigen::MatrixXd theta_n = theta + 1/gamma_n * ((e_n*B_n_1)*phi);

//    std::cout << 1/gamma_n * ((e_n*B_n_1)*phi) << std::endl;

//    std::cout << theta_n << std::endl;

    B_n_1_RLS = B_n;

    return theta_n;

};

double
F::_c_theta_x(Eigen::MatrixXd& theta_x, Eigen::Block<Eigen::MatrixXd>& s0, Eigen::MatrixXd& theta_V){

    int N = 1;
    double _c = 0.0;

//    std::cout << s0 << std::endl;

	//gets matrix of thetas
	//prepare initial matrix
	MatrixXd m_s0 = MatrixXd::Zero(N, s0.cols());

	m_s0.rowwise() += s0.row(0);

	//allocate space to the matrix
	MatrixXd n_w;
	MatrixXd n_x = MatrixXd::Zero(N, 4);
	MatrixXd n_s = MatrixXd::Zero(N, s0.cols());
    MatrixXd v_bf_ret = MatrixXd::Zero(1, param->ADP_BF_N);
	MatrixXd n_c = MatrixXd::Zero(N, 1);;
    MatrixXd n_v = MatrixXd::Zero(N, 1);;

    //create random number generators for the implementation

	boost::normal_distribution<> nd_w1(s0(0,3), pow(s0(0,17),0.5));
	boost::normal_distribution<> nd_w2(s0(0,5), pow(s0(0,18),0.5));

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w1(w.rng, nd_w1);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w2(w.rng, nd_w2);

    if (w.param->SIMULATION_MODE != "test"){

        //draw random variables
        n_w = MatrixXd::Zero(N, 2 * 1);
        for (int i = 0; i < N; i++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%2 == 0){
                    n_w(i,j) = rng_w1();
                } else {
                    n_w(i,j) = rng_w2();
                };
                n_w(i,j) = std::max(0.0, (double)n_w(i,j));
            };
        };
    };

    //redraw random in comparative with Python mode
    if (w.param->SIMULATION_MODE == "test"){
        n_w = MatrixXd::Zero(N, 2 * param->T_MAX);
        for (int k = 0; k < N; k++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%2 == 0){
                    n_w(k,j) = Tools::get_normal(s0(0,3), pow(s0(0,17),0.5), w.myrng);
                } else {
                    n_w(k,j) = Tools::get_normal(s0(0,5), pow(s0(0,18),0.5), w.myrng);
                };
                n_w(k,j) = std::max(0.0, (double)n_w(k,j));
            };
        };
    };

    //call estimator of results of steps, given theta_x
    n_s = m_s0;
    _step_opt_CS(theta_x, &n_w, &n_x, &n_s, &n_c, &n_v, theta_V, &v_bf_ret, 1);
    _c = (n_c + param->BETA*n_v).mean();

    s0.row(0) = n_s.row(0);

//    std::cout << s0 << std::endl;

    return _c;

};

void
F::_opt_CS_s_V(Eigen::Block<Eigen::MatrixXd>& s0,  Eigen::MatrixXd& theta_x, Eigen::MatrixXd& theta_V, int N){

    //	N = 1;

	//gets matrix of thetas
	//prepare initial matrix
	MatrixXd m_s0 = MatrixXd::Zero(N, s0.cols());

	m_s0.rowwise() += s0.row(0);

	//allocate space to the matrix
	MatrixXd n_w;
	MatrixXd n_x;
	MatrixXd n_s = MatrixXd::Zero(N, s0.cols());
	MatrixXd n_c;
    MatrixXd v_bf_ret = MatrixXd::Zero(1, param->ADP_BF_N);
    MatrixXd n_v;

	//create matrix for the grid - small size here
	MatrixXd grid(4,3);

    create_decision_grid(grid,0);

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(grid, &theta_x_all);

    } else{

        Tools::mgrid(grid, &theta_x_all);
    };

	//create random number generators for the implementation

	boost::normal_distribution<> nd_w1(s0(0,3), pow(s0(0,17),0.5));
	boost::normal_distribution<> nd_w2(s0(0,5), pow(s0(0,18),0.5));

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w1(w.rng, nd_w1);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w2(w.rng, nd_w2);

    if (w.param->SIMULATION_MODE != "test"){

        //draw random variables
        n_w = MatrixXd::Zero(N, 2 * 1);
        for (int i = 0; i < N; i++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%2 == 0){
                    n_w(i,j) = rng_w1();
                } else {
                    n_w(i,j) = rng_w2();
                };
                n_w(i,j) = std::max(0.0, (double)n_w(i,j));
            };
        };
    };

	//matrix of all
	MatrixXd c_all(theta_x_all.cols(),1);

	for (int i = 0 ; i < theta_x_all.cols(); i++){
        //redraw random in comparative with Python mode
        if (w.param->SIMULATION_MODE == "test"){
            n_w = MatrixXd::Zero(N, 2 * 1);
            for (int k = 0; k < N; k++){
                for(int j=0; j< n_w.cols(); j++){
                    if (j%2 == 0){
                        n_w(k,j) = Tools::get_normal(s0(0,3), pow(s0(0,17),0.5), w.myrng);
                    } else {
                        n_w(k,j) = Tools::get_normal(s0(0,5), pow(s0(0,18),0.5), w.myrng);
                    };
                    n_w(k,j) = std::max(0.0, (double)n_w(k,j));
                };
            };
        };

		//call estimator of results of steps, given theta_x
		theta_x.row(0) = theta_x_all.col(i).transpose();
		n_x = MatrixXd::Zero(N, 4);
		n_s = m_s0;
		n_c = MatrixXd::Zero(N, 1);
        n_v = MatrixXd::Zero(N, 1);
		_step_opt_CS(theta_x, &n_w, &n_x, &n_s, &n_c, &n_v, theta_V, &v_bf_ret, 1);

		//after n_c
		//average over n runs
        c_all(i,0) = (n_c + param->BETA*n_v).mean();

	};

//    std::cout << (n_c + param->BETA*n_v).mean() << std::endl;

	//find all max elements
	double max = c_all(0,0);
	std::list<int> i_max;
	for (int i=0; i<c_all.rows(); i++){
		if (c_all(i,0) == max){
			i_max.push_back(i);
		}else{
			if (c_all(i,0) > max){
				max = c_all(i,0);
				i_max.clear();
				i_max.push_back(i);
			};
		}
	};

	//if more than 1 max - randomly pick
	long theta_max_i;
	if (i_max.size()>1){
		boost::random::uniform_int_distribution<> i_max_dist(0, i_max.size()-1);
		theta_max_i = i_max_dist(w.rng);
	}else{
		theta_max_i = i_max.front();
	};

	theta_x.row(0) = theta_x_all.col(theta_max_i).transpose();

//    std::cout << theta_x << std::endl;
};

void
F::create_decision_grid(Eigen::MatrixXd& grid, int begin_index){
    int i = begin_index;

    if (param->GRID == "small"){
		grid(i,0) = 0.0;
		grid(i,1) = w.NH;
		grid(i,2) = ((grid(i,1) - grid(i,0))/4);
		grid(i+1,0) = 0.8;
		grid(i+1,1) = 1.25;
		grid(i+1,2) = 0.2;
		grid(i+2,0) = 0.0;
		grid(i+2,1) = 1.0;
		grid(i+2,2) = 0.5;
		grid(i+3,0) = 0.8;
		grid(i+3,1) = 1.25;
		grid(i+3,2) = 0.2;
	};

    if (param->GRID == "big"){
		grid(i,0) = 0.0;
		grid(i,1) = w.NH;
		grid(i,2) = (grid(i,1) - grid(i,0))/4;
		grid(i+1,0) = 0.1;
		grid(i+1,1) = 2.1;
		grid(i+1,2) = 0.45;
		grid(i+2,0) = 0.0;
		grid(i+2,1) = 1.0;
		grid(i+2,2) = 0.5;
		grid(i+3,0) = 0.1;
		grid(i+3,1) = 2.1;
		grid(i+3,2) = 0.45;

	};
    if (param->GRID == "test"){

        //        grid(0,0) = w.NH;
        //        grid(0,1) = w.NH + 0.5;
        //        grid(0,2) = 1.0;
        //        grid(1,0) = 1.0;
        //        grid(1,1) = 1.1;
        //        grid(1,2) = 1.0;
        //        grid(2,0) = 1.0;
        //        grid(2,1) = 1.1;
        //        grid(2,2) = 1.0;
        //        grid(3,0) = 1.0;
        //        grid(3,1) = 1.1;
        //        grid(3,2) = 1.0;

        grid(i,0) = 2.5;
        grid(i,1) = 2.5 + 1.5;
        grid(i,2) = 1.0;

        grid(i+1,0) = 1.0;
        grid(i+1,1) = 1.1;
        grid(i+1,2) = 1.0;

        grid(i+2,0) = 0.5;
        grid(i+2,1) = 1.1;
        grid(i+2,2) = 1.0;

        grid(i+3,0) = 1.2;
        grid(i+3,1) = 1.3;
        grid(i+3,2) = 1.0;

    };

};

void
F::_step_opt_CS(MatrixXd& theta_x,
                MatrixXd *w,
                MatrixXd *x,
                MatrixXd *s,
                MatrixXd *c,
                MatrixXd* v,
                MatrixXd& theta_v,
                MatrixXd* ret,
                int T){

	//sets decisions for labor market
	//zero vector
	auto s_zero = MatrixXd::Zero(s->rows(),1);
    Eigen::Block<Eigen::MatrixXd> s_block = s->block(0,0,1,s->cols());;

	for (int i=0; i< T; i++){

		x->col(0) = Eigen::MatrixXd::Constant(x->rows(), 1, theta_x(0,0));
		x->col(1) = theta_x(0,1) * s->col(3).array();

        //        std::cout << *x << std::endl;

		//labor market results
		//if wage is less than realized wage - zero, otherwise hire all desired number

		(*s).col(9) = ((*w).col(i*2).array() <= x->col(1).array()).cast<double>().array() *x->col(0).array();
		(*s).col(10) = (*w).col(i*2);

        //cast to whole numbers
        for (int j=0; j < s->rows(); j++){
            (*s)(j,9) = floor((*s)(j,9));
        };

        //        std::cout << *s << std::endl;

		//current production
		s->col(13) = s->col(9).array().pow(param->F_F_theta(0,1))*param->F_F_theta(0,0);
		s->col(2) += s->col(13);

        //        std::cout << *s << std::endl;

		//update for cost structure
		s->col(7) = s->col(7).array() + ((*s).col(9).array() * (*s).col(10).array());
		s->col(8) += s->col(13);

        //        std::cout << *s << std::endl;

		//sets decision for goods market
		x->col(2) = theta_x(0,2) * s->col(2).array();
		x->col(3) = Eigen::MatrixXd::Constant(x->rows(), 1, theta_x(0,3) * (*s)(0,5));

        //        std::cout << *x << std::endl;

		//sales
		(*s).col(11) = ((*w).col(i*2 + 1).array() >= x->col(3).array()).cast<double>().array() *x->col(2).array();
		(*s).col(12) = (*w).col(i*2 + 1);

        //        std::cout << *s << std::endl;

		//money
		s->col(0) = s->col(0).array() + (s->col(11).array() * s->col(12).array());

        //        std::cout << *s << std::endl;

		//inventory
		s->col(2) -= s->col(11);

        //        std::cout << *s << std::endl;

		double atc = 0.0;

		//updates current profit
		for (int j=0; j < s->rows(); j++){

			if ((*s)(j,8)>0.0){
				atc = (*s)(j,7) / (*s)(j,8);

                if (param->ACCOUNTING_TYPE=="modern"){
                    //update profit to cost of production
                    (*s)(j,14) -= atc * (*s)(j,11);
                };
                if (param->ACCOUNTING_TYPE=="classic"){
                    //update profit to cost of production
                    (*s)(j,14) -= (*s)(j,9) * (*s)(j,10);
                };

				//update cost of production
				(*s)(j,7) -= atc * (*s)(j,11);
				(*s)(j,8) -= (*s)(j,11);
			};

			//update profit to sales
			(*s)(j,14) += (*s)(j,11) * (*s)(j,12);

			atc = 0.0;
		};

        //        std::cout << *s << std::endl;

		//labor payment
		s->col(0) = s->col(0).array() -  (s->col(9).array() * s->col(10).array());

        //        std::cout << *s << std::endl;

		//dividends
		s->col(0) = s->col(0).array() - (s->col(14).array() > s_zero.array()).cast<double>().array() * s->col(14).array() * s->col(16).array();

//        std::cout << *s << std::endl;

        //zero out labor contract
		s->col(9) = s_zero;
		s->col(10) = s_zero;

        //dead or alive
		s->col(15) = (s->col(0).array() >= s_zero.array()).cast<double>().array() * s->col(15).array();

        //        std::cout << *s << std::endl;

		//stores realized goal
		c->col(i) = (s->col(15).array() > s_zero.array()).cast<double>().array() * s->col(14).array();

        //add value function estimation
        for (int j=0; j < s->rows(); j++){
            s_block  = s->block(j,0,1,s->cols());

            (*v)(j,i) = _V(s_block,ret, theta_V);
        };

//        std::cout << *c << std::endl;

//        std::cout << *v << std::endl;

		//zero out current profit
		s->col(14) = s_zero;

	};

};

void
F::opt_QL(){
    //find best action for the state

    //find current bin for the state
    _s0();
    std::vector<double> s_bin = _place_state_QL(s0);
    s_t_bin = s_bin;
    std::vector<int> Q_s;

    //create list of all indexes that correspond to current state
    for (int i = 0; i < s_bin.size(); i++){
        for (int j = 0; j< Q.cols(); j++){
            if (_is_bin(j, i, s_bin)){
                Q_s.push_back(j);
            };

        };
    };

    //find index with max Q
    std::vector<int> Q_max;
    double max = Q(6,Q_s.at(0));

    for (auto j:Q_s){
        if (Q(6,j) > max){
            max = Q(6,j);
            Q_max.clear();
            Q_max.push_back(j);

        }else{
            if (Q(6,j) == max){
                Q_max.push_back(j);
            };
        };

    };

    boost::random::uniform_01<> _exp_dist;
    double exp_rate = _exp_dist(w.rng);

    boost::random::uniform_int_distribution<> Q_dist(0, Q.cols()-1);
    long theta_max_i;

    if (exp_rate < param->QL_experimental_rate){
        theta_max_i = Q_dist(w.rng);
    }else{
        //pick randomly best action
        if (Q_max.size()>1){
            boost::random::uniform_int_distribution<> Q_max_dist(0, Q_max.size()-1);
            theta_max_i = Q_max.at(Q_max_dist(w.rng));

        }else{
            theta_max_i = Q_max.front();
        };
    };

    theta_x_t_bin.push_back(theta_max_i);

    //transform from index to decision
    theta_x.row(0) = Q.block(s_bin.size(),theta_max_i,4,1).transpose();

};

bool
F::_is_bin(int j, int i, std::vector<double>& s_bin){
    bool is_bin = false;

    if (i < s_bin.size()){
        is_bin = (Q(i,j) == Q_bin.at(i).at(s_bin.at(i))) && _is_bin(j, i+1, s_bin);
//        std::cout << (Q(i,j) == s_bin.at(i)) << std::endl;
//        std::cout << _is_bin(j, i+1, s_bin) << std::endl;
    }else{
        is_bin = true;
    };

    return is_bin;
};

void
F::wm_update_Q(){
    //updates Q matrix
    //reward
    double R_t1 = account->profit.at(account->profit.size()-2);

    //new state
    _s0();
    std::vector<double> s_bin = _place_state_QL(s0);
    std::vector<int> Q_s;

    //create list of all indexes that correspond to current state
    for (int i = 0; i < s_bin.size(); i++){
        for (int j = 0; j< Q.cols(); j++){
            if (_is_bin(j, i+1, s_bin)){
                Q_s.push_back(j);
            };

        };
    };

    //find index with max Q
    std::vector<int> Q_max;
    double max = Q(6,Q_s.at(0));

    for (auto j:Q_s){
        if (Q(6,j) > max){
            max = Q(6,j);
            Q_max.clear();
            Q_max.push_back(j);

        }else{
            if (Q(6,j) == max){
                Q_max.push_back(j);
            };
        };

    };

    //pick best action
    long theta_max_i;
    long max_i;
    if (Q_max.size()>1){
        boost::random::uniform_int_distribution<> Q_max_dist(0, Q_max.size()-1);
        max_i = Q_max_dist(w.rng);
        theta_max_i = Q_max.at(max_i);
    }else{
        theta_max_i = Q_max.front();
    };

    //update Q value
    Q(6,theta_x_t_bin.back()) = Q(6,theta_x_t_bin.back())
                                + Q(7,theta_x_t_bin.back())
                                * (R_t1 + param->BETA*Q(6,theta_max_i) - Q(6,theta_x_t_bin.back()));
//    std::cout << Q << std::endl;

};

void
F::_set_QL(){
    //setup QL matrix
    //create matrix for the grid - small size here
	Q_grid = Eigen::MatrixXd::Zero(8,3);

    //money and stock of goods on hand
    Q_grid(0,0) = 0.0;
    Q_grid(0,1) = 100.0;
    Q_grid(0,2) = ((Q_grid(0,1) - Q_grid(0,0))/2);
    Q_grid(1,0) = 0.0;
    Q_grid(1,1) = w.NH;
    Q_grid(1,2) = ((Q_grid(1,1) - Q_grid(1,0))/2);

    create_decision_grid(Q_grid, 2);

    Q_grid(6,0) = 0.0;
    Q_grid(6,1) = 0.1;
    Q_grid(6,2) = 1.0;

    Q_grid(7,0) = param->QL_L; //speed of learning
    Q_grid(7,1) = Q_grid(7,0) + 0.1;
    Q_grid(7,2) = Q_grid(7,0) + 1.0;

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(Q_grid, &Q);

    } else{

        Tools::mgrid(Q_grid, &Q);
    };

    create_bin_values(Q_grid, Q_bin);

    //update Q to push to hiring all of them
    for (int j = 0; j < Q.cols(); j++){
        if (Q(2,j) == Q_bin.at(2).back()){
            Q(6,j) = 0.05;
        };
    };

//    std::cout << Q << std::endl;

//    Tools::print_vector(Q_bin);

};

void
F::create_bin_values(Eigen::MatrixXd& _grid, std::vector<std::vector<double>>& _bin){
    long N_rows = _grid.rows();
	int n_steps_i;
	double n_steps_i_int;

	double val;
    std::vector<double> values_temp;

    //create matrix of bin values
    //create list of values
	for (int i=0; i < N_rows; i++){
		//n_steps
		n_steps_i_int = (_grid(i,1) - _grid(i,0))/_grid(i,2);

		n_steps_i = static_cast<int>(floor(n_steps_i_int)) + 1;

		for (int j=0; j<n_steps_i; j++){
			val = _grid(i,0) + _grid(i,2)*j;
			values_temp.push_back(val);
		};

		_bin.push_back(values_temp);
		values_temp.clear();
	};

};

std::vector<double>
F::_place_state_QL(Eigen::MatrixXd& s){
    std::vector<double> s_bin;

    //pick first and compare
    //push values for bins ina  vector

    bool FLAG_PLACED = false;

    int j = Q_bin.at(0).size()-1;
    while ((j>=0) && !FLAG_PLACED){

           if (s(0,0) >= Q_bin.at(0).at(j)){
               s_bin.push_back(j);
               FLAG_PLACED = true;
           };
        j -= 1;
    };

    j = Q_bin.at(1).size()-1;
    FLAG_PLACED = false;
    while ((j>=0) && !FLAG_PLACED){

        if (s(0,2) >= Q_bin.at(1).at(j)){
            s_bin.push_back(j);
            FLAG_PLACED = true;
        };
        j -= 1;
    };

    return s_bin;
};

void
F::_set_mRE(){
    //
    mRE_grid = Eigen::MatrixXd::Zero(5,3);
    create_decision_grid(mRE_grid, 0);
    mRE_grid(4,0) = 1.0;
    mRE_grid(4,1) = 1.1;
    mRE_grid(4,2) = 1.0;

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(mRE_grid, &mRE);

    } else{

        Tools::mgrid(mRE_grid, &mRE);
    };

    create_bin_values(mRE_grid, mRE_bin);

    //update mRE to push to hiring all of them
    for (int j = 0; j < mRE.cols(); j++){
        if (mRE(0,j) == mRE_bin.at(0).back()){
            mRE(4,j) = 1.05;
        };
    };

//    //storage for actual probabilities
//    mRE_grid(5,0) = 0.0;
//    mRE_grid(5,1) = 0.1;
//    mRE_grid(5,2) = 1.0;

};

void
F::opt_mRE(){
    //form matrix of probabilities
    double sum_prob = 0.0;

    for (int j=0; j<mRE.cols();j++){
        sum_prob += exp(mRE(4,j)/param->mRE_T);
    };

    std::vector<double> probs;

    for (int j=0; j<mRE.cols();j++){
        probs.push_back(exp(mRE(4,j)/param->mRE_T)/sum_prob);
    };

    //roll weighted dice
    boost::random::discrete_distribution<> dist(probs.begin(), probs.end());

    int theta_i = dist(w.rng);

    theta_x_t_bin.push_back(theta_i);

    //transform from index to decision
    theta_x.row(0) = mRE.block(0,theta_i,4,1).transpose();

};

void
F::wm_update_mRE(){
    //adjustment matrix
    Eigen::MatrixXd _E_re  = Eigen::MatrixXd::Zero(1, mRE.cols());

    //reward
    double R_t1 = account->profit.at(account->profit.size()-2);

    //adjustment
    _E_re = mRE.row(4) * (param->mRE_EPSILON/(mRE.cols()-1));

    //include reward
    _E_re(0,theta_x_t_bin.back()) = R_t1 * (1 - param->mRE_EPSILON);

    //update whole matrix
    mRE.row(4) = mRE.row(4) * (1 - param->mRE_PHI) + _E_re;

//    std::cout << mRE << std::endl;
};

void
F::_PS_receive_payment(MessagePSSendPayment* mes){};

bool
F::_PS_accept_PO(MessagePSSendPayment* mes){
	return true;
};

ContractBDt0*
F::_PS_contract(){
	return ass_asCBDt0.front();
};


/*
    Formula Student Driverless Project (FSD-Project).
    Copyright (c) 2019:
     - chentairan <killasipilin@gmail.com>

    FSD-Project 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.

    FSD-Project 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 FSD-Project.  If not, see <https://www.gnu.org/licenses/>.
*/

#ifndef  GROUND_SEGMENT_HPP
#define GROUND_SEGMENT_HPP

//#include "fsd_common_msgs/ConeDetections.h"
#include "geometry_msgs/Point32.h"
#include "opencv2/opencv.hpp"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "std_msgs/String.h"
#include "std_msgs/Header.h"
#include <chrono>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/centroid.h>
#include <velodyne_pointcloud/point_types.h>

#include <Eigen/Dense>
using Eigen::MatrixXf;
using Eigen::JacobiSVD;
using Eigen::VectorXf;

namespace scan_line_run
{
  /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */
  struct PointXYZIRL
  {
    PCL_ADD_POINT4D;                    // quad-word XYZ
    float    intensity;                 ///< laser intensity reading
    uint16_t ring;                      ///< laser ring number
    uint16_t label;                     ///< point label
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW     // ensure proper alignment
  } EIGEN_ALIGN16;

}; // namespace scan_line_run

#define SLRPointXYZIRL scan_line_run::PointXYZIRL
#define VPoint velodyne_pointcloud::PointXYZIR
#define RUN pcl::PointCloud<SLRPointXYZIRL>
// Register custom point struct according to PCL
POINT_CLOUD_REGISTER_POINT_STRUCT(scan_line_run::PointXYZIRL,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, intensity, intensity)
                                  (uint16_t, ring, ring)
                                  (uint16_t, label, label))

namespace ns_ground_segment {

class GroundSegment {

 public:
  // Constructor
  GroundSegment(ros::NodeHandle &nh);

  // Getters
  sensor_msgs::PointCloud2 getGroundPoint();
  sensor_msgs::PointCloud2 getGroundlessPoint();
  sensor_msgs::PointCloud2 getAllPoints();

  bool is_ok();

  // Setters
  void setRawLidar(sensor_msgs::PointCloud2 msg);

  void runAlgorithm();

 private:

    float d_;
    MatrixXf normal_;
    float th_dist_d_;

    ros::NodeHandle &nh_;

    void loadParameters();
    int sensor_model_;
     double sensor_height_;
    int num_seg_;
    int num_iter_;
    int num_lpr_;
    double th_seeds_;
    double th_dist_;

    bool getRawLidar, is_ok_flag_;
    bool is_cone_reconstruct_;

    sensor_msgs::PointCloud cluster_;
    sensor_msgs::PointCloud2 raw_pc2_;

    pcl::PointCloud<VPoint> laserCloudIn;
    pcl::PointCloud<VPoint> laserCloudIn_org;
    SLRPointXYZIRL point;

    void velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg);
    void estimate_plane_(void);
    void extract_initial_seeds_(const pcl::PointCloud<VPoint>& p_sorted);
    int name_index = 0;

    // Model parameter for ground plane fitting
    // The ground plane model is: ax+by+cz+d=0
    // Here normal:=[a,b,c], d=d
    // th_dist_d_ = threshold_dist - d

    //publish
    sensor_msgs::PointCloud2 ground_msg;
    sensor_msgs::PointCloud2 groundless_msg;
    sensor_msgs::PointCloud2 all_points_msg;
};
} // namespace ns_ground_segment

#endif // GROUND_SEGMENT_HPP


//==================================================================================================
/*!
  @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_FUNCTION_FFS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FFS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-bitwise
    This function object returns  the index
    of the first bit set (beginning with the least
    significant bit) in the parameter.

    @par Header <boost/simd/function/ffs.hpp>

    @see  firstbitset,  firstbitunset

    @par Example:

      @snippet ffs.cpp ffs

    @par Possible output:

      @snippet ffs.txt ffs

  **/
  as_integer_t<Value> ffs(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/ffs.hpp>
#include <boost/simd/function/simd/ffs.hpp>

#endif


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

//
// *** System
//
#include <iostream>
#include <iomanip>

//
// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

//
// *** ViennaCL
//
//#define VIENNACL_DEBUG_ALL
#define VIENNACL_WITH_UBLAS 1
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/vector_proxy.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/norm_1.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/norm_inf.hpp"

#include "viennacl/scheduler/execute.hpp"
#include "viennacl/scheduler/io.hpp"

#include "Random.hpp"

using namespace boost::numeric;

//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType const & s1, ScalarType const & s2)
{
   viennacl::backend::finish();
   if (s1 != s2)
      return (s1 - s2) / std::max(std::fabs(s1), std::fabs(s2));
   return 0;
}
//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType const & s1, viennacl::scalar<ScalarType> const & s2)
{
   viennacl::backend::finish();
   if (s1 != s2)
      return (s1 - s2) / std::max(std::fabs(s1), std::fabs(s2));
   return 0;
}
//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType const & s1, viennacl::entry_proxy<ScalarType> const & s2)
{
   viennacl::backend::finish();
   if (s1 != s2)
      return (s1 - s2) / std::max(std::fabs(s1), std::fabs(s2));
   return 0;
}
//
// -------------------------------------------------------------
//
template<typename ScalarType, typename ViennaCLVectorType>
ScalarType diff(ublas::vector<ScalarType> const & v1, ViennaCLVectorType const & vcl_vec)
{
   ublas::vector<ScalarType> v2_cpu(vcl_vec.size());
   viennacl::backend::finish();
   viennacl::copy(vcl_vec, v2_cpu);

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if ( std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) > 0 )
         v2_cpu[i] = std::fabs(v2_cpu[i] - v1[i]) / std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) );
      else
         v2_cpu[i] = 0.0;
   }

   return ublas::norm_inf(v2_cpu);
}

template<typename T1, typename T2>
int check(T1 const & t1, T2 const & t2, double epsilon)
{
  int retval = EXIT_SUCCESS;

  double temp = std::fabs(diff(t1, t2));
  if (temp > epsilon)
  {
    std::cout << "# Error! Relative difference: " << temp << std::endl;
    retval = EXIT_FAILURE;
  }
  else
    std::cout << "PASSED!" << std::endl;
  return retval;
}

//
// -------------------------------------------------------------
//
template< typename NumericT, typename Epsilon, typename UblasVectorType, typename ViennaCLVectorType1, typename ViennaCLVectorType2 >
int test(Epsilon const& epsilon,
         UblasVectorType     & ublas_v1, UblasVectorType     & ublas_v2,
         ViennaCLVectorType1 &   vcl_v1, ViennaCLVectorType2 &   vcl_v2)
{
  int retval = EXIT_SUCCESS;

  NumericT                    cpu_result = 42.0;
  viennacl::scalar<NumericT>  gpu_result = 43.0;
  NumericT                    alpha      = NumericT(3.1415);
  NumericT                    beta       = NumericT(2.7172);

  //
  // Initializer:
  //
  std::cout << "Checking for zero_vector initializer..." << std::endl;
  ublas_v1 = ublas::zero_vector<NumericT>(ublas_v1.size());
  vcl_v1 = viennacl::zero_vector<NumericT>(vcl_v1.size());
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << "Checking for scalar_vector initializer..." << std::endl;
  ublas_v1 = ublas::scalar_vector<NumericT>(ublas_v1.size(), cpu_result);
  vcl_v1 = viennacl::scalar_vector<NumericT>(vcl_v1.size(), cpu_result);
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  ublas_v1 = ublas::scalar_vector<NumericT>(ublas_v1.size(), gpu_result);
  vcl_v1 = viennacl::scalar_vector<NumericT>(vcl_v1.size(), gpu_result);
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << "Checking for unit_vector initializer..." << std::endl;
  ublas_v1 = ublas::unit_vector<NumericT>(ublas_v1.size(), 5);
  vcl_v1 = viennacl::unit_vector<NumericT>(vcl_v1.size(), 5);
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  for (std::size_t i=0; i<ublas_v1.size(); ++i)
  {
    ublas_v1[i] = NumericT(1.0) + random<NumericT>();
    ublas_v2[i] = NumericT(1.0) + random<NumericT>();
  }

  viennacl::copy(ublas_v1.begin(), ublas_v1.end(), vcl_v1.begin());  //resync
  viennacl::copy(ublas_v2.begin(), ublas_v2.end(), vcl_v2.begin());

  std::cout << "Checking for successful copy..." << std::endl;
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_v2, vcl_v2, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  // --------------------------------------------------------------------------

  std::cout << "Testing simple assignments..." << std::endl;

  {
  ublas_v1 = ublas_v2;
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), vcl_v2); // same as vcl_v1 = vcl_v2;
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  ublas_v1 += ublas_v2;
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_inplace_add(), vcl_v2); // same as vcl_v1 += vcl_v2;
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  ublas_v1 -= ublas_v2;
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_inplace_sub(), vcl_v2); // same as vcl_v1 -= vcl_v2;
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "Testing composite assignments..." << std::endl;
  {
  ublas_v1 = ublas_v1 + ublas_v2;
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), vcl_v1 + vcl_v2); // same as vcl_v1 = vcl_v1 + vcl_v2;
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }
  {
  ublas_v1 += alpha * ublas_v1 - beta * ublas_v2 + ublas_v1 / beta - ublas_v2 / alpha;
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_inplace_add(), alpha * vcl_v1 - beta * vcl_v2 + vcl_v1 / beta - vcl_v2 / alpha); // same as vcl_v1 += alpha * vcl_v1 - beta * vcl_v2 + beta * vcl_v1 - alpha * vcl_v2;
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  ublas_v1 = ublas_v1 - ublas_v2;
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), vcl_v1 - vcl_v2); // same as vcl_v1 = vcl_v1 - vcl_v2;
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "--- Testing reductions ---" << std::endl;
  std::cout << "inner_prod..." << std::endl;
  {
  cpu_result = inner_prod(ublas_v1, ublas_v2);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::inner_prod(vcl_v1, vcl_v2)); // same as gpu_result = inner_prod(vcl_v1, vcl_v2);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  cpu_result = inner_prod(ublas_v1 + ublas_v2, ublas_v2);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::inner_prod(vcl_v1 + vcl_v2, vcl_v2)); // same as gpu_result = inner_prod(vcl_v1 + vcl_v2, vcl_v2);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  cpu_result = inner_prod(ublas_v1, ublas_v2 - ublas_v1);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::inner_prod(vcl_v1, vcl_v2 - vcl_v1)); // same as gpu_result = inner_prod(vcl_v1, vcl_v2 - vcl_v1);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  cpu_result = inner_prod(ublas_v1 - ublas_v2, ublas_v2 + ublas_v1);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::inner_prod(vcl_v1 - vcl_v2, vcl_v2 + vcl_v1)); // same as gpu_result = inner_prod(vcl_v1 - vcl_v2, vcl_v2 + vcl_v1);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "norm_1..." << std::endl;
  {
  cpu_result = norm_1(ublas_v1);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::norm_1(vcl_v1)); // same as gpu_result = norm_1(vcl_v1);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  cpu_result = norm_1(ublas_v1 + ublas_v2);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::norm_1(vcl_v1 + vcl_v2)); // same as gpu_result = norm_1(vcl_v1 + vcl_v2);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "norm_2..." << std::endl;
  {
  cpu_result = norm_2(ublas_v1);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::norm_2(vcl_v1)); // same as gpu_result = norm_2(vcl_v1);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  cpu_result = norm_2(ublas_v1 + ublas_v2);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::norm_2(vcl_v1 + vcl_v2)); // same as gpu_result = norm_2(vcl_v1 + vcl_v2);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "norm_inf..." << std::endl;
  {
  cpu_result = norm_inf(ublas_v1);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::norm_inf(vcl_v1)); // same as gpu_result = norm_inf(vcl_v1);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  {
  cpu_result = norm_inf(ublas_v1 - ublas_v2);
  viennacl::scheduler::statement   my_statement(gpu_result, viennacl::op_assign(), viennacl::linalg::norm_inf(vcl_v1 - vcl_v2)); // same as gpu_result = norm_inf(vcl_v1 - vcl_v2);
  viennacl::scheduler::execute(my_statement);

  if (check(cpu_result, gpu_result, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "--- Testing elementwise operations (binary) ---" << std::endl;
  std::cout << "x = element_prod(x, y)... ";
  {
  ublas_v1 = element_prod(ublas_v1, ublas_v2);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_prod(vcl_v1, vcl_v2));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_prod(x + y, y)... ";
  {
  ublas_v1 = element_prod(ublas_v1 + ublas_v2, ublas_v2);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_prod(vcl_v1 + vcl_v2, vcl_v2));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_prod(x, x + y)... ";
  {
  ublas_v1 = element_prod(ublas_v1, ublas_v1 + ublas_v2);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_prod(vcl_v1, vcl_v2 + vcl_v1));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_prod(x - y, y + x)... ";
  {
  ublas_v1 = element_prod(ublas_v1 - ublas_v2, ublas_v2 + ublas_v1);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_prod(vcl_v1 - vcl_v2, vcl_v2 + vcl_v1));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_div(x, y)... ";
  {
  ublas_v1 = element_div(ublas_v1, ublas_v2);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_div(vcl_v1, vcl_v2));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_div(x + y, y)... ";
  {
  ublas_v1 = element_div(ublas_v1 + ublas_v2, ublas_v2);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_div(vcl_v1 + vcl_v2, vcl_v2));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_div(x, x + y)... ";
  {
  ublas_v1 = element_div(ublas_v1, ublas_v1 + ublas_v2);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_div(vcl_v1, vcl_v2 + vcl_v1));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_div(x - y, y + x)... ";
  {
  ublas_v1 = element_div(ublas_v1 - ublas_v2, ublas_v2 + ublas_v1);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_div(vcl_v1 - vcl_v2, vcl_v2 + vcl_v1));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_pow(x, y)... ";
  {
  for (std::size_t i=0; i<ublas_v1.size(); ++i)
  {
    ublas_v1[i] = NumericT(2.0) + random<NumericT>();
    ublas_v2[i] = NumericT(1.0) + random<NumericT>();
  }
  viennacl::copy(ublas_v1, vcl_v1);
  viennacl::copy(ublas_v2, vcl_v2);

  for (std::size_t i=0; i<ublas_v1.size(); ++i)
    ublas_v1[i] = std::pow(ublas_v1[i], ublas_v2[i]);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_pow(vcl_v1, vcl_v2));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_pow(x + y, y)... ";
  {
  for (std::size_t i=0; i<ublas_v1.size(); ++i)
  {
    ublas_v1[i] = NumericT(2.0) + random<NumericT>();
    ublas_v2[i] = NumericT(1.0) + random<NumericT>();
  }
  viennacl::copy(ublas_v1, vcl_v1);
  viennacl::copy(ublas_v2, vcl_v2);

  for (std::size_t i=0; i<ublas_v1.size(); ++i)
    ublas_v1[i] = std::pow(ublas_v1[i]  + ublas_v2[i], ublas_v2[i]);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_pow(vcl_v1 + vcl_v2, vcl_v2));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_pow(x, x + y)... ";
  {
  for (std::size_t i=0; i<ublas_v1.size(); ++i)
  {
    ublas_v1[i] = NumericT(2.0) + random<NumericT>();
    ublas_v2[i] = NumericT(1.0) + random<NumericT>();
  }
  viennacl::copy(ublas_v1, vcl_v1);
  viennacl::copy(ublas_v2, vcl_v2);

  for (std::size_t i=0; i<ublas_v1.size(); ++i)
    ublas_v1[i] = std::pow(ublas_v1[i], ublas_v1[i] + ublas_v2[i]);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_pow(vcl_v1, vcl_v2 + vcl_v1));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = element_pow(x - y, y + x)... ";
  {
  for (std::size_t i=0; i<ublas_v1.size(); ++i)
  {
    ublas_v1[i] = NumericT(2.0) + random<NumericT>();
    ublas_v2[i] = NumericT(1.0) + random<NumericT>();
  }
  viennacl::copy(ublas_v1, vcl_v1);
  viennacl::copy(ublas_v2, vcl_v2);

  for (std::size_t i=0; i<ublas_v1.size(); ++i)
    ublas_v1[i] = std::pow(ublas_v1[i] - ublas_v2[i], ublas_v2[i] + ublas_v1[i]);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_pow(vcl_v1 - vcl_v2, vcl_v2 + vcl_v1));
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "--- Testing elementwise operations (unary) ---" << std::endl;
#define GENERATE_UNARY_OP_TEST(OPNAME) \
  ublas_v1 = ublas::scalar_vector<NumericT>(ublas_v1.size(), NumericT(0.21)); \
  ublas_v2 = NumericT(3.1415) * ublas_v1; \
  viennacl::copy(ublas_v1.begin(), ublas_v1.end(), vcl_v1.begin()); \
  viennacl::copy(ublas_v2.begin(), ublas_v2.end(), vcl_v2.begin()); \
  { \
  for (std::size_t i=0; i<ublas_v1.size(); ++i) \
    ublas_v1[i] = std::OPNAME(ublas_v2[i]); \
  viennacl::scheduler::statement my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_##OPNAME(vcl_v2)); \
  viennacl::scheduler::execute(my_statement); \
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS) \
    return EXIT_FAILURE; \
  } \
  { \
  for (std::size_t i=0; i<ublas_v1.size(); ++i) \
  ublas_v1[i] = std::OPNAME(ublas_v2[i] / NumericT(2)); \
  viennacl::scheduler::statement my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::element_##OPNAME(vcl_v2 / NumericT(2))); \
  viennacl::scheduler::execute(my_statement); \
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS) \
    return EXIT_FAILURE; \
  }

  GENERATE_UNARY_OP_TEST(cos);
  GENERATE_UNARY_OP_TEST(cosh);
  GENERATE_UNARY_OP_TEST(exp);
  GENERATE_UNARY_OP_TEST(floor);
  GENERATE_UNARY_OP_TEST(fabs);
  GENERATE_UNARY_OP_TEST(log);
  GENERATE_UNARY_OP_TEST(log10);
  GENERATE_UNARY_OP_TEST(sin);
  GENERATE_UNARY_OP_TEST(sinh);
  GENERATE_UNARY_OP_TEST(fabs);
  //GENERATE_UNARY_OP_TEST(abs); //OpenCL allows abs on integers only
  GENERATE_UNARY_OP_TEST(sqrt);
  GENERATE_UNARY_OP_TEST(tan);
  GENERATE_UNARY_OP_TEST(tanh);

#undef GENERATE_UNARY_OP_TEST

  std::cout << "--- Testing complicated composite operations ---" << std::endl;
  std::cout << "x = inner_prod(x, y) * y..." << std::endl;
  {
  ublas_v1 = inner_prod(ublas_v1, ublas_v2) * ublas_v2;
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), viennacl::linalg::inner_prod(vcl_v1, vcl_v2) * vcl_v2);
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  std::cout << "x = y / norm_1(x)..." << std::endl;
  {
  ublas_v1 = ublas_v2 / norm_1(ublas_v1);
  viennacl::scheduler::statement   my_statement(vcl_v1, viennacl::op_assign(), vcl_v2 / viennacl::linalg::norm_1(vcl_v1) );
  viennacl::scheduler::execute(my_statement);

  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  }

  // --------------------------------------------------------------------------
  return retval;
}

template< typename NumericT, typename Epsilon >
int test(Epsilon const& epsilon)
{
  int retval = EXIT_SUCCESS;
  std::size_t size = 24656;

  std::cout << "Running tests for vector of size " << size << std::endl;

  //
  // Set up UBLAS objects
  //
  ublas::vector<NumericT> ublas_full_vec(size);
  ublas::vector<NumericT> ublas_full_vec2(ublas_full_vec.size());

  for (std::size_t i=0; i<ublas_full_vec.size(); ++i)
  {
    ublas_full_vec[i]  = NumericT(1.0) + random<NumericT>();
    ublas_full_vec2[i] = NumericT(1.0) + random<NumericT>();
  }

  ublas::range r1(    ublas_full_vec.size() / 4, 2 * ublas_full_vec.size() / 4);
  ublas::range r2(2 * ublas_full_vec2.size() / 4, 3 * ublas_full_vec2.size() / 4);
  ublas::vector_range< ublas::vector<NumericT> > ublas_range_vec(ublas_full_vec, r1);
  ublas::vector_range< ublas::vector<NumericT> > ublas_range_vec2(ublas_full_vec2, r2);

  ublas::slice s1(    ublas_full_vec.size() / 4, 3, ublas_full_vec.size() / 4);
  ublas::slice s2(2 * ublas_full_vec2.size() / 4, 2, ublas_full_vec2.size() / 4);
  ublas::vector_slice< ublas::vector<NumericT> > ublas_slice_vec(ublas_full_vec, s1);
  ublas::vector_slice< ublas::vector<NumericT> > ublas_slice_vec2(ublas_full_vec2, s2);

  //
  // Set up ViennaCL objects
  //
  viennacl::vector<NumericT> vcl_full_vec(ublas_full_vec.size());
  viennacl::vector<NumericT> vcl_full_vec2(ublas_full_vec2.size());

  viennacl::fast_copy(ublas_full_vec.begin(), ublas_full_vec.end(), vcl_full_vec.begin());
  viennacl::copy(ublas_full_vec2.begin(), ublas_full_vec2.end(), vcl_full_vec2.begin());

  viennacl::range vcl_r1(    vcl_full_vec.size() / 4, 2 * vcl_full_vec.size() / 4);
  viennacl::range vcl_r2(2 * vcl_full_vec2.size() / 4, 3 * vcl_full_vec2.size() / 4);
  viennacl::vector_range< viennacl::vector<NumericT> > vcl_range_vec(vcl_full_vec, vcl_r1);
  viennacl::vector_range< viennacl::vector<NumericT> > vcl_range_vec2(vcl_full_vec2, vcl_r2);

  {
    viennacl::vector<NumericT> vcl_short_vec(vcl_range_vec);
    viennacl::vector<NumericT> vcl_short_vec2 = vcl_range_vec2;

    ublas::vector<NumericT> ublas_short_vec(ublas_range_vec);
    ublas::vector<NumericT> ublas_short_vec2(ublas_range_vec2);

    std::cout << "Testing creation of vectors from range..." << std::endl;
    if (check(ublas_short_vec, vcl_short_vec, epsilon) != EXIT_SUCCESS)
      return EXIT_FAILURE;
    if (check(ublas_short_vec2, vcl_short_vec2, epsilon) != EXIT_SUCCESS)
      return EXIT_FAILURE;
  }

  viennacl::slice vcl_s1(    vcl_full_vec.size() / 4, 3, vcl_full_vec.size() / 4);
  viennacl::slice vcl_s2(2 * vcl_full_vec2.size() / 4, 2, vcl_full_vec2.size() / 4);
  viennacl::vector_slice< viennacl::vector<NumericT> > vcl_slice_vec(vcl_full_vec, vcl_s1);
  viennacl::vector_slice< viennacl::vector<NumericT> > vcl_slice_vec2(vcl_full_vec2, vcl_s2);

  viennacl::vector<NumericT> vcl_short_vec(vcl_slice_vec);
  viennacl::vector<NumericT> vcl_short_vec2 = vcl_slice_vec2;

  ublas::vector<NumericT> ublas_short_vec(ublas_slice_vec);
  ublas::vector<NumericT> ublas_short_vec2(ublas_slice_vec2);

  std::cout << "Testing creation of vectors from slice..." << std::endl;
  if (check(ublas_short_vec, vcl_short_vec, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_short_vec2, vcl_short_vec2, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  //
  // Now start running tests for vectors, ranges and slices:
  //

  std::cout << " ** vcl_v1 = vector, vcl_v2 = vector **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_short_vec, vcl_short_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** vcl_v1 = vector, vcl_v2 = range **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_short_vec, vcl_range_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** vcl_v1 = vector, vcl_v2 = slice **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_short_vec, vcl_slice_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  ///////

  std::cout << " ** vcl_v1 = range, vcl_v2 = vector **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_range_vec, vcl_short_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** vcl_v1 = range, vcl_v2 = range **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_range_vec, vcl_range_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** vcl_v1 = range, vcl_v2 = slice **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_range_vec, vcl_slice_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  ///////

  std::cout << " ** vcl_v1 = slice, vcl_v2 = vector **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_slice_vec, vcl_short_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** vcl_v1 = slice, vcl_v2 = range **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_slice_vec, vcl_range_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** vcl_v1 = slice, vcl_v2 = slice **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec, ublas_short_vec2,
                          vcl_slice_vec, vcl_slice_vec2);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  return EXIT_SUCCESS;
}

//
// -------------------------------------------------------------
//
int main()
{
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "## Test :: Vector" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;

   int retval = EXIT_SUCCESS;

   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;
   {
      typedef float NumericT;
      NumericT epsilon = static_cast<NumericT>(1.0E-4);
      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: float" << std::endl;
      retval = test<NumericT>(epsilon);
      if ( retval == EXIT_SUCCESS )
         std::cout << "# Test passed" << std::endl;
      else
         return retval;
   }
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;
#ifdef VIENNACL_WITH_OPENCL
   if ( viennacl::ocl::current_device().double_support() )
#endif
   {
      {
         typedef double NumericT;
         NumericT epsilon = 1.0E-12;
         std::cout << "# Testing setup:" << std::endl;
         std::cout << "  eps:     " << epsilon << std::endl;
         std::cout << "  numeric: double" << std::endl;
         retval = test<NumericT>(epsilon);
         if ( retval == EXIT_SUCCESS )
           std::cout << "# Test passed" << std::endl;
         else
           return retval;
      }
      std::cout << std::endl;
      std::cout << "----------------------------------------------" << std::endl;
      std::cout << std::endl;
   }

  std::cout << std::endl;
  std::cout << "------- Test completed --------" << std::endl;
  std::cout << std::endl;

   return retval;
}


/*
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

Copyright (c) 2018 Panda Team
*/

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE space_tree_test
#include <boost/test/unit_test.hpp>

#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>

#include <iostream>
#include <vector>
#include "modules/space.hpp"

template <typename T>
struct distance {
    int operator()(const T& lhs, const T& rhs) const { return std::abs(lhs - rhs); }
};

BOOST_AUTO_TEST_CASE(test_insert)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    for (auto t : data) {
        tree.insert(t);
        BOOST_TEST(tree.check_covering());
    }
}

BOOST_AUTO_TEST_CASE(test_insert_batch)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    BOOST_TEST(tree.check_covering());
}
BOOST_AUTO_TEST_CASE(test_nn)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    tree.print();
    BOOST_TEST(tree.nn(200)->data == 200);
}

BOOST_AUTO_TEST_CASE(test_knn)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    auto k1 = tree.knn(3, 15);
    BOOST_TEST(k1.size() == 7);
    BOOST_TEST(k1[0].first->data == 3);
    BOOST_TEST(k1[1].first->data == 1);
    BOOST_TEST(k1[2].first->data == 5);
    BOOST_TEST(k1[3].first->data == -10);
    BOOST_TEST(k1[4].first->data == 50);
    BOOST_TEST(k1[5].first->data == 200);
    BOOST_TEST(k1[6].first->data == -200);
}

BOOST_AUTO_TEST_CASE(test_erase)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    for (auto d : data) {
        tree.erase(d);
        BOOST_TEST(tree.check_covering());
    }
}

BOOST_AUTO_TEST_CASE(test_erase_root)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    for (int i = 0; i < 7; i++) {
        auto root = tree.get_root();
        tree.erase(root->data);
        BOOST_TEST(tree.check_covering());
    }
}

BOOST_AUTO_TEST_CASE(test_insert_if)
{
    metric::Tree<int, distance<int>> tree;
    tree.insert(1);
    BOOST_TEST(!tree.insert_if(2, 10));
    BOOST_TEST(tree.insert_if(15, 10));
    BOOST_TEST(!tree.insert_if(14, 10));
    BOOST_TEST(tree.insert_if(26, 10));
}

BOOST_AUTO_TEST_CASE(test_insert2)
{
    std::vector<int> data = { 7, 8, 9, 10, 11, 12, 13 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    tree.print();
}

BOOST_AUTO_TEST_CASE(test_to_json)
{
    metric::Tree<int, distance<int>> tree;
    tree.insert(1);
    auto s = tree.to_json();
    std::string json1 = "{\n\"nodes\": [\n{ \"id\":0, \"values\":1}\n],\n\"edges\": [\n]}\n";
    BOOST_TEST(s == json1);
    tree.insert(2);
    std::string json2 = "{\n\"nodes\": [\n{ \"id\":0, \"values\":1},\n{ \"id\":1, \"values\":2}\n],\n\"edges\": [\n{ "
                        "\"source\":0, \"target\":1, \"distance\":1}\n]}\n";
    BOOST_TEST(tree.to_json() == json2);
}

BOOST_AUTO_TEST_CASE(test_serialize_boost_text)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    std::ostringstream os;
    boost::archive::text_oarchive oar(os);
    tree.serialize(oar);
    metric::Tree<int, distance<int>> tree1;
    std::istringstream is(os.str());
    boost::archive::text_iarchive iar(is);
    tree1.deserialize(iar, is);
    BOOST_TEST(tree1.check_covering());
    BOOST_TEST(tree1 == tree);
}

BOOST_AUTO_TEST_CASE(test_serialize_boost_binary)
{
    std::vector<int> data = { 3, 5, -10, 50, 1, -200, 200 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    std::ostringstream os;
    boost::archive::binary_oarchive oar(os);
    tree.serialize(oar);
    metric::Tree<int, distance<int>> tree1;
    std::istringstream is(os.str());
    boost::archive::binary_iarchive iar(is);
    tree1.deserialize(iar, is);
    BOOST_TEST(tree1.check_covering());
    BOOST_TEST(tree1 == tree);
}

struct Record {
    float v;
    std::vector<float> vv;
    int a;
    float operator-(const Record& r) const { return v - r.v; }
    bool operator!=(const Record rhs) { return v != rhs.v || vv != rhs.vv || a != rhs.a; }
    template <typename Archive>
    void serialize(Archive& ar, const unsigned int)
    {
        ar& BOOST_SERIALIZATION_NVP(v) & BOOST_SERIALIZATION_NVP(vv) & BOOST_SERIALIZATION_NVP(a);
    }
};

BOOST_AUTO_TEST_CASE(test_serialize_boost_record_binary)
{
    std::vector<Record> data = { { 3.0f, { 1, 2, 3 }, 1 }, { 5.0f, { 1, 6, 3 }, 2 }, { -10.0f, { 1, 6, 3 }, 3 },
        { 50.0f, { 1, 6, 3 }, 4 }, { 1.0f, { 1, 6, 3 }, 5 }, { -200.0f, { 1, 6, 3 }, 6 }, { 200.0f, { 1, 6, 3 }, 7 } };

    metric::Tree<Record, distance<Record>> tree;
    tree.insert(data);
    std::ostringstream os;
    boost::archive::binary_oarchive oar(os);

    tree.serialize(oar);
    metric::Tree<Record, distance<Record>> tree1;
    std::istringstream is(os.str());
    boost::archive::binary_iarchive iar(is);
    tree1.deserialize(iar, is);
    BOOST_TEST(tree1.check_covering());
    BOOST_TEST(tree1 == tree);
}

BOOST_AUTO_TEST_CASE(test_serialize_boost_record_text)
{
    std::vector<Record> data = { { 3.0f, { 1, 2, 3 }, 1 }, { 5.0f, { 1, 6, 3 }, 2 }, { -10.0f, { 1, 6, 3 }, 3 },
        { 50.0f, { 1, 6, 3 }, 4 }, { 1.0f, { 1, 6, 3 }, 5 }, { -200.0f, { 1, 6, 3 }, 6 }, { 200.0f, { 1, 6, 3 }, 7 } };

    metric::Tree<Record, distance<Record>> tree;
    tree.insert(data);
    std::ostringstream os;
    boost::archive::text_oarchive oar(os);

    tree.serialize(oar);
    metric::Tree<Record, distance<Record>> tree1;
    std::istringstream is(os.str());
    boost::archive::text_iarchive iar(is);
    tree1.deserialize(iar, is);
    BOOST_TEST(tree1.check_covering());
    BOOST_TEST(tree1 == tree);
}

BOOST_AUTO_TEST_CASE(test_serialize_boost_record_xml)
{
    std::vector<Record> data = { { 3.0f, { 1, 2, 3 }, 1 }, { 5.0f, { 1, 6, 3 }, 2 }, { -10.0f, { 1, 6, 3 }, 3 },
        { 50.0f, { 1, 6, 3 }, 4 }, { 1.0f, { 1, 6, 3 }, 5 }, { -200.0f, { 1, 6, 3 }, 6 }, { 200.0f, { 1, 6, 3 }, 7 } };

    metric::Tree<Record, distance<Record>> tree;
    tree.insert(data);
    std::ostringstream os;
    boost::archive::xml_oarchive oar(os);

    tree.serialize(oar);
    std::cout << os.str() << std::endl;
    metric::Tree<Record, distance<Record>> tree1;
    std::istringstream is(os.str());
    boost::archive::xml_iarchive iar(is);
    tree1.deserialize(iar, is);
    BOOST_TEST(tree1.check_covering());
    BOOST_TEST(tree1 == tree);
}
namespace std {
std::ostream& operator<<(std::ostream& ostr, const std::vector<std::size_t>& v)
{
    ostr << "[ ";
    for (auto i : v) {
        ostr << i << ", ";
    }
    ostr << "]";
    return ostr;
}
}
BOOST_AUTO_TEST_CASE(cluster1)
{
    std::vector<int> data = { 7, 8, 9, 10, 11, 12, 13 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    tree.print();
    std::vector<double> distribution = { 0.1, 0.2, 0.3, 0.5 };
    std::vector<std::size_t> IDS = { 1, 2, 3 };
    std::vector<int> points = { 8, 9, 10 };

    auto result = tree.clustering(distribution, IDS, data);
    auto result2 = tree.clustering(distribution, points);
    std::vector<std::vector<std::size_t>> test_result = { {}, { 1 }, { 0 }, { 2 } };
    BOOST_TEST(result == test_result, boost::test_tools::per_element());
    BOOST_TEST(result2 == test_result, boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(cluster2)
{
    std::vector<int> data = { 7, 8, 9, 10, 11, 12, 13 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    tree.print();
    std::vector<double> distribution = { 0.1, 0.2, 0.3, 0.5 };
    std::vector<std::size_t> IDS = { 3 };
    std::vector<int> points = { 10 };
    auto result = tree.clustering(distribution, IDS, data);
    auto result2 = tree.clustering(distribution, points);
    std::vector<std::vector<std::size_t>> test_result = { {}, { 3 }, { 4 }, { 2 } };
    BOOST_TEST(result == test_result, boost::test_tools::per_element());
    BOOST_TEST(result2 == test_result, boost::test_tools::per_element());
    std::size_t i = 0;
    for (auto& v : result) {
        std::cout << distribution[i] << " = {";
        for (auto p : v) {
            std::cout << "[" << data[p] << ":" << p << "]"
                      << ", ";
        }
        std::cout << "}" << std::endl;
        i++;
    }
}

BOOST_AUTO_TEST_CASE(cluster3)
{
    std::vector<int> data = { 7, 8, 9, 10, 11, 12, 13 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    tree.print();
    std::vector<double> distribution = { 0.1, 0.2, 0.5, 0.9 };
    std::vector<std::size_t> IDS = { 3 };
    std::vector<int> points = { 10 };
    auto result = tree.clustering(distribution, IDS, data);
    auto result2 = tree.clustering(distribution, points);
    std::vector<std::vector<std::size_t>> test_result = { {}, { 3 }, { 4, 2 }, { 1, 0, 5 } };
    BOOST_TEST(result == test_result, boost::test_tools::per_element());
    BOOST_TEST(result2 == test_result, boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(cluster_exception_unsorted)
{
    std::vector<int> data = { 7, 8, 9, 10, 11, 12, 13 };
    metric::Tree<int, distance<int>> tree;
    tree.insert(data);
    std::vector<double> distribution1 = { 0.9, 0.1, 0.2, 0.5, 0.9 };
    std::vector<double> distribution2 = { 0.1, 0.2, 0.5, 0.9 };
    std::vector<double> distribution3 = { 0.1, 0.2, 0.5, 0.9, 1, 5 };
    std::vector<std::size_t> IDS = { 3 };
    std::vector<int> points = { 10 };
    BOOST_CHECK_THROW(tree.clustering(distribution1, IDS, data), metric::unsorted_distribution_exception);
    BOOST_CHECK_THROW(tree.clustering(distribution1, points), metric::unsorted_distribution_exception);
    BOOST_CHECK_THROW(tree.clustering(distribution3, IDS, data), metric::bad_distribution_exception);
    BOOST_CHECK_THROW(tree.clustering(distribution3, points), metric::bad_distribution_exception);
    BOOST_REQUIRE_NO_THROW(tree.clustering(distribution2, IDS, data));
    BOOST_REQUIRE_NO_THROW(tree.clustering(distribution2, points));
}


//
// Created by guillaume on 04/09/2021.
//
#include <vector>
#include <set>
#include <string>
#include <map>
#include <cmath>
#include <algorithm>

#include <boost/format.hpp>
#include <stdexcept>

/*
 * CIGAR parsing tools
 */
namespace CIGAR {

    /*
     * Computes my homemade identity score
     * I think here, we suppose that both the reference and the query are expected to be 100% right if the alignment
     * is well done. Meaning, we want the fraction of nucleotides that are matched among all considered nucleotides
     * --> 2*numbermatches / (seqlen + reflen)
     * That differs significantly from the BLAST definition
     * But the BLAST definition, for my use case, would be stupid
     * https://medium.com/computational-biology/how-to-interpret-blast-results-ee304216fd5
     */
    float homemade_identity(unsigned int reflen,
                            unsigned int seqlen,
                            unsigned int number_matches){
        float answer = 2*number_matches / (seqlen+reflen);
        return answer;

    }

    /*
     * Returns the nth field in a tab separated line
    */
    std::string get_field(std::string &inputstring, int field) {

        std::string fieldstring;
        int field_number = 0;
        int i = 0;

        while (field_number != field) {
            if (inputstring[i] == '\t') {
                field_number += 1;
            }
            i += 1;
        }

        //... start reading
        while (i < inputstring.length() and inputstring[i] != '\t') {
            fieldstring.push_back(inputstring[i]);
            i += 1;
        }

        return fieldstring;
    }

    /*
     * Same function as above, but with as many fields as wanted
     */
    void get_fields(std::string &inputstring, std::set<int> fields, std::map<int,std::string> &output) {

        std::string fieldstring;
        int field_number = 0;
        int i = 0;
        int size_asked = fields.size();

        while(output.size() != size_asked){

            while ( find(fields.begin(),fields.end(),field_number) != fields.end()){ // While field_number can be found in the fields
                if (inputstring[i] == '\t') {
                    field_number += 1;
                }
                i += 1;
            }

            //... start reading
            while (i < inputstring.length() and inputstring[i] != '\t') {
                fieldstring.push_back(inputstring[i]);
                i += 1;
            }
            output[field_number] = fieldstring;
            field_number += 1;

            fieldstring.clear();
            i++;

        }

    }

    /*
     * From [1,4,5,6] (for instance), returns 1456
     */
    long listdigits_to_number(std::vector<int> listdigits){
        long final_digit = 0;
        long append;
        int lsize = listdigits.size();

        for (int i = 0; i < lsize; i++){
            append = std::pow(10,(lsize-i-1)) * listdigits[i];
            final_digit += append;
        }

        return final_digit;
    }

    /*
     * Modifies its second argument to make a std::map<string,long> correponding to the parsed CIGAR
     */
    void parse_CIGAR(std::string &CIGARstring,std::map<std::string,long> &outputmap)
    {

//        std::map<std::string,long> outputmap;
        outputmap["*"] = 0;
        outputmap["I"] = 0;
        outputmap["M"] = 0;
        outputmap["N"] = 0;
        outputmap["P"] = 0;
        outputmap["="] = 0;
        outputmap["D"] = 0;
        outputmap["X"] = 0;
        outputmap["H"] = 0;
        outputmap["S"] = 0;

        char tmp_char;
        std::vector<int> tmplist_digits;

        for(int i = 0; i < CIGARstring.length();i++){
            tmp_char = CIGARstring[i];

            if(isdigit(tmp_char)){
                int this_integer = (int) tmp_char - 48; // ASCII
                tmplist_digits.push_back(this_integer);
            }
            else{
                if (tmp_char == '*'){
                    outputmap["*"] = 1;
                    return;
                }

                long nb_nt = listdigits_to_number(tmplist_digits);
                tmplist_digits.clear();

                auto align_type = std::string();
                align_type.push_back(tmp_char);

                outputmap[align_type] = outputmap[align_type]+nb_nt;
            }
        }

    }

}