/*
 * 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();
};


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_ElasticElectronMomentsEvaluator.cpp
//! \author Luke Kersting
//! \brief  Elastic electron cross section moments evaluator definition
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "DataGen_ElasticElectronMomentsEvaluator.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_LegendrePolynomial.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_ContractException.hpp"
#include "MonteCarlo_TwoDDistributionHelpers.hpp"
#include "MonteCarlo_ElasticElectronScatteringDistributionNativeFactory.hpp"
#include "MonteCarlo_ElectroatomicReactionNativeFactory.hpp"
#include "Utility_StandardHashBasedGridSearcher.hpp"

namespace DataGen{

// Initialize static member data

// cutoff angle for analytical peak
double ElasticElectronMomentsEvaluator::s_rutherford_cutoff_angle = 1.0e-6;

// cutoff angle cosine for analytical peak
double ElasticElectronMomentsEvaluator::s_rutherford_cutoff_angle_cosine = 0.999999;

// Constructor
ElasticElectronMomentsEvaluator::ElasticElectronMomentsEvaluator(
    const Data::EvaluatedElectronDataContainer& native_eedl_data,
    const double& cutoff_angle )
  : d_native_eedl_data ( native_eedl_data ),
    d_cutoff_angle( cutoff_angle )
{
  // Make sure the data is valid
  testPrecondition( cutoff_angle >= s_rutherford_cutoff_angle );
  testPrecondition( cutoff_angle <= 2.0 );

  // Extract the common energy grid used for this atom
  Teuchos::ArrayRCP<double> energy_grid;
  energy_grid.assign( native_eedl_data.getElectronEnergyGrid().begin(),
                      native_eedl_data.getElectronEnergyGrid().end() );

  // Create the hard elastic distributions ( both Analog and Screened Rutherford )
  MonteCarlo::ElasticElectronScatteringDistributionNativeFactory::createHardElasticDistributions(
    d_analog_distribution,
    d_rutherford_distribution,
    native_eedl_data,
    cutoff_angle );

  // Construct the hash-based grid searcher for this atom
  Teuchos::RCP<Utility::HashBasedGridSearcher> grid_searcher(
     new Utility::StandardHashBasedGridSearcher<Teuchos::ArrayRCP<const double>, false>(
						     energy_grid,
						     100u ) );

  MonteCarlo::ElectroatomicReactionNativeFactory::createScreenedRutherfordElasticReaction(
					   native_eedl_data,
					   energy_grid,
                       grid_searcher,
					   d_rutherford_reaction );

  MonteCarlo::ElectroatomicReactionNativeFactory::createAnalogElasticReaction(
					   native_eedl_data,
					   energy_grid,
                       grid_searcher,
					   d_analog_reaction );
}

// Evaluate the Legnendre Polynomial expansion of the screened rutherford pdf
double ElasticElectronMomentsEvaluator::evaluateLegendreExpandedRutherford(
                                    const double scattering_angle,
                                    const double incoming_energy,
                                    const int polynomial_order ) const
{
  // Make sure the energy and angle are valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( scattering_angle >= 0.0 );
  testPrecondition( scattering_angle <= s_rutherford_cutoff_angle );

  // Evaluate the elastic pdf value at a given energy and scattering angle cosine
  double pdf_value =
            d_rutherford_distribution->evaluatePDF( incoming_energy,
                                                    scattering_angle );

  // Evaluate the Legendre Polynomial at the given angle and order
  double legendre_value =
    Utility::getLegendrePolynomial( 1.0-scattering_angle, polynomial_order );

  return pdf_value*legendre_value;
}

// Evaluate the Legnendre Polynomial expansion of the elastic scttering PDF
double ElasticElectronMomentsEvaluator::evaluateLegendreExpandedPDF(
                                    const double scattering_angle,
                                    const double incoming_energy,
                                    const int polynomial_order ) const
{
  // Make sure the energy and angle are valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( scattering_angle >= s_rutherford_cutoff_angle );
  testPrecondition( scattering_angle <= 2.0 );

  // Evaluate the elastic pdf value at a given energy and scattering angle cosine
  double pdf_value =
            d_analog_distribution->evaluatePDF( incoming_energy,
                                                scattering_angle );

  // Evaluate the Legendre Polynomial at the given angle and order
  double legendre_value =
    Utility::getLegendrePolynomial( 1.0 - scattering_angle, polynomial_order );

  return pdf_value*legendre_value;
}

// Evaluate the Legnendre Polynomial expansion of the elastic scttering PDF
double ElasticElectronMomentsEvaluator::evaluateLegendreExpandedPDFAtEnergyBin(
                                    const double scattering_angle,
                                    const unsigned incoming_energy_bin,
                                    const int polynomial_order ) const
{
  // Make sure the energy and angle are valid
  testPrecondition( incoming_energy_bin >= 0 );
  testPrecondition( scattering_angle >= s_rutherford_cutoff_angle );
  testPrecondition( scattering_angle <= 2.0 );

  // Evaluate the elastic pdf value at a given energy and scattering angle cosine
  double pdf_value =
            d_analog_distribution->evaluatePDF( incoming_energy_bin,
                                                scattering_angle );

  // Evaluate the Legendre Polynomial at the given angle and order
  double legendre_value =
    Utility::getLegendrePolynomial( 1.0 - scattering_angle, polynomial_order );

  return pdf_value*legendre_value;
}

// Evaluate the first n moments of the elastic scattering distribution at a given energy
void ElasticElectronMomentsEvaluator::evaluateElasticMoment(
            Teuchos::Array<Utility::long_float>& legendre_moments,
            const double energy,
            const int n,
            const double precision ) const
{
  // Make sure the energy and order is valid
  testPrecondition( energy > 0.0 );
  testPrecondition( n >= 0 );

  // Get common angular grid
  Teuchos::Array<double> angular_grid;

  angular_grid =
    MonteCarlo::ElasticElectronScatteringDistributionNativeFactory::getAngularGrid(
        d_native_eedl_data,
        energy,
        d_cutoff_angle );

  // resize array to the number of legendre moments wanted
  legendre_moments.resize(n+1);

  Utility::GaussKronrodIntegrator integrator( precision );

  // Calucuate the tabular and Rutherford component of the Legendre moment
  Utility::long_float tabular_moment,rutherford_moment;

  for ( int i = 0; i <= n; i++ )
  {
    double abs_error, moment_k;

    // Get the analog cross section
    double analog_cross_section =
        d_analog_reaction->getCrossSection( energy );

    // Calucuate the component of the moment from screened Rutherford peak
    evaluateScreenedRutherfordMoment( rutherford_moment, energy, i );

    // Create boost rapper function for the hard elastic differential cross section
    boost::function<double (double x)> distribution_wrapper =
      boost::bind<double>( &ElasticElectronMomentsEvaluator::evaluateLegendreExpandedPDF,
                         boost::cref( *this ),
                         _1,
                         energy,
                         i );

    Teuchos::Array<double>::iterator grid_point, grid_point_minus_one;
    grid_point_minus_one = angular_grid.begin();
    grid_point = ++angular_grid.begin();

    tabular_moment = Utility::long_float(0);
    for ( grid_point; grid_point != angular_grid.end(); grid_point++ )
    {
      moment_k = 0.0;
      abs_error = 0.0;
      integrator.integrateAdaptively<61>(
					distribution_wrapper,
					*grid_point_minus_one,
					*grid_point,
					moment_k,
					abs_error );

      grid_point_minus_one = grid_point;

      tabular_moment += moment_k;
    }
    legendre_moments[i] =
        rutherford_moment + tabular_moment*analog_cross_section;
  }
}

// Evaluate the first n moments of the elastic scattering distribution at a given energy
void ElasticElectronMomentsEvaluator::evaluateElasticMoment(
            Teuchos::Array<Utility::long_float>& legendre_moments,
            const unsigned energy_bin,
            const int n,
            const double precision ) const
{
  // Make sure the energy and order is valid
  testPrecondition( energy_bin >= 0 );
  testPrecondition( n >= 0 );

  // Get the energy at the given angular energy bin
  double energy = d_analog_distribution->getEnergy( energy_bin );

  // Get angular grid
  Teuchos::Array<double> angular_grid;

  angular_grid =
    MonteCarlo::ElasticElectronScatteringDistributionNativeFactory::getAngularGrid(
        d_native_eedl_data,
        energy,
        d_cutoff_angle );

  // resize array to the number of legendre moments wanted
  legendre_moments.resize(n+1);

  Utility::GaussKronrodIntegrator integrator( precision );

  // Calucuate the tabular and Rutherford component of the Legendre moment
  Utility::long_float tabular_moment,rutherford_moment;

  for ( int i = 0; i <= n; i++ )
  {
    double abs_error, moment_k;

    // Get the analog cross section
    double analog_cross_section =
        d_analog_reaction->getCrossSection( energy );

    // Calucuate the component of the moment from screened Rutherford peak
    evaluateScreenedRutherfordMoment( rutherford_moment, energy, i );

    // Create boost rapper function for the hard elastic differential cross section
    boost::function<double (double x)> distribution_wrapper =
      boost::bind<double>( &ElasticElectronMomentsEvaluator::evaluateLegendreExpandedPDFAtEnergyBin,
                         boost::cref( *this ),
                         _1,
                         energy_bin,
                         i );

    Teuchos::Array<double>::iterator grid_point, grid_point_minus_one;
    grid_point_minus_one = angular_grid.begin();
    grid_point = ++angular_grid.begin();

    tabular_moment = Utility::long_float(0);
    for ( grid_point; grid_point != angular_grid.end(); grid_point++ )
    {
      moment_k = 0.0;
      abs_error = 0.0;
      integrator.integrateAdaptively<61>(
					distribution_wrapper,
					*grid_point_minus_one,
					*grid_point,
					moment_k,
					abs_error );

      grid_point_minus_one = grid_point;

      tabular_moment += moment_k;
    }
    legendre_moments[i] =
        rutherford_moment + tabular_moment*analog_cross_section;
  }
}

// Evaluate the nth cross section moment of the screened Rutherford peak distribution
void ElasticElectronMomentsEvaluator::evaluateScreenedRutherfordMoment(
            Utility::long_float& rutherford_moment,
            const double energy,
            const int n ) const
{
  // Make sure the energy and angle are valid
  testPrecondition( energy > 0.0 );

  double angle;

  angle = s_rutherford_cutoff_angle;

  // Calcuate Moliere's modified screening constant (eta)
  double eta =
    d_rutherford_distribution->evaluateMoliereScreeningConstant( energy );

  /*!  \details If eta is small ( << 1 ) a recursion relationship can be used to
   *! calculate the moments of the screened Rutherford peak. For larger eta the
   *! moments will be calculated by numerical integration.
   */
  if ( eta <= 1.0e-2 )
  {
    rutherford_moment = d_rutherford_reaction->getCrossSection( energy );

    if ( n > 0 )
    {
      Teuchos::Array<Utility::long_float> coef_one( n+1 ), coef_two( n+1 );

      coef_one[0] = Utility::long_float(0);
      coef_one[1] = ( log( ( eta + angle )/( eta ) ) ) -
                     angle/( angle + eta );

      coef_two[0] = angle;
      coef_two[1] = ( Utility::long_float(2) - angle )*angle/
                      Utility::long_float(2);

      for ( int i = 1; i < n; i++ )
      {
        coef_one[i+1] =
          ( Utility::long_float(2) + Utility::long_float(1)/i )*
          ( Utility::long_float(1) + eta )*coef_one[i] -
          ( Utility::long_float(1) + Utility::long_float(1)/i )*coef_one[i-1] -
          ( ( Utility::long_float(2) + Utility::long_float(1)/i )/
          ( angle + eta ) )*( angle - coef_two[i] );

        coef_two[i+1] =
          ( Utility::long_float(2)*i + Utility::long_float(1) )/
          ( i + Utility::long_float(2) )*( Utility::long_float(1) - angle )*
          coef_two[i] -
          ( i - Utility::long_float(1) )/( i + Utility::long_float(2) )*
          coef_two[i-1];
      }
      Utility::long_float frac_disc =
          angle*( Utility::long_float(1) + eta/Utility::long_float(2) )/
        ( angle + eta );

      rutherford_moment *= (1.0 - eta*coef_one[n]/frac_disc );
    }
  }
  else // Numerically integrate the moment
  {
    Utility::GaussKronrodIntegrator integrator( 1e-13 );

    double abs_error, moment_n;

    double rutherford_cross_section =
        d_rutherford_reaction->getCrossSection( energy );

    double moment_zero =
        d_rutherford_distribution->evaluateIntegratedPDF( energy );

    if ( n == 0 )
    {
      moment_n = moment_zero;
    }
    else
    {
      // Create boost rapper function for the screened Rutherford peak
      boost::function<double (double x)> wrapper =
        boost::bind<double>( &ElasticElectronMomentsEvaluator::evaluateLegendreExpandedRutherford,
                         boost::cref( *this ),
                         _1,
                         energy,
                         n );

      integrator.integrateAdaptively<61>(
					wrapper,
                    0.0,
					s_rutherford_cutoff_angle,
					moment_n,
					abs_error );
    }
    rutherford_moment = moment_n/moment_zero*rutherford_cross_section;
  }
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_ElasticElectronMomentsEvaluator.cpp
//---------------------------------------------------------------------------//


/*
 * Author(s):  Salvadore Gerace <sgerace@dotdecimal.com>
 *             Thomas Madden <tmadden@mgh.harvard.edu>
 * Date:       03/27/2013
 *
 * Copyright:
 * This work was developed as a joint effort between .decimal, Inc. and
 * Partners HealthCare under research agreement A213686; as such, it is
 * jointly copyrighted by the participating organizations.
 * (c) 2013 .decimal, Inc. All rights reserved.
 * (c) 2013 Partners HealthCare. All rights reserved.
 */
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <cradle/geometry/meshing.hpp>

#include <cradle/geometry/common.hpp>
#include <cradle/geometry/distance.hpp>
#include <cradle/geometry/grid_points.hpp>
#include <cradle/geometry/intersection.hpp>
#include <cradle/geometry/polygonal.hpp>

#include <cradle/imaging/geometry.hpp>
#include <cradle/imaging/image.hpp>

#include <boost/compressed_pair.hpp>

#include <fstream>

//#include <cradle/io/stereolithography_io.hpp>

namespace cradle {

// amount to scale our geometry by to make it sized correctly for clipper
static const double geometry_scale_factor
    = 1.0 / cradle::clipper_integer_precision;

triangle_mesh
make_cube(vertex3 const& origin, vertex3 const& extent)
{
    triangle_mesh mesh;

    auto vertices = allocate(&mesh.vertices, 8);
    vertices[0] = make_vector(origin[0], origin[1], origin[2]);
    vertices[1] = make_vector(extent[0], origin[1], origin[2]);
    vertices[2] = make_vector(origin[0], extent[1], origin[2]);
    vertices[3] = make_vector(extent[0], extent[1], origin[2]);
    vertices[4] = make_vector(origin[0], origin[1], extent[2]);
    vertices[5] = make_vector(extent[0], origin[1], extent[2]);
    vertices[6] = make_vector(origin[0], extent[1], extent[2]);
    vertices[7] = make_vector(extent[0], extent[1], extent[2]);

    auto faces = allocate(&mesh.faces, 12);
    faces[0] = make_vector(0, 3, 1);
    faces[1] = make_vector(0, 2, 3);
    faces[2] = make_vector(0, 1, 5);
    faces[3] = make_vector(0, 5, 4);
    faces[4] = make_vector(0, 4, 2);
    faces[5] = make_vector(2, 4, 6);
    faces[6] = make_vector(4, 5, 6);
    faces[7] = make_vector(5, 7, 6);
    faces[8] = make_vector(2, 6, 3);
    faces[9] = make_vector(3, 6, 7);
    faces[10] = make_vector(1, 3, 5);
    faces[11] = make_vector(3, 7, 5);

    return mesh;
}

triangle_mesh
make_cylinder(
    vector3d const& base,
    double radius,
    double height,
    int resolution,
    unsigned axis_direction)
{
    triangle_mesh mesh;

    unsigned direction = 2;
    if (axis_direction < 3)
    {
        direction = axis_direction;
    }
    vector3i ordinates;
    if (direction == 0)
    {
        ordinates = make_vector(1, 2, 0);
    }
    if (direction == 1)
    {
        ordinates = make_vector(2, 0, 1);
    }
    if (direction == 2)
    {
        ordinates = make_vector(0, 1, 2);
    }

    int k = 8;
    if (resolution > k)
    {
        k = resolution;
    }
    vector3d axis = make_vector(0.0, 0.0, 0.0);
    axis[direction] = height;
    double delta = (2.0 * pi) / k;
    auto vertices = allocate(&mesh.vertices, 2 * k + 2);
    for (int i = 0; i < k; ++i)
    {
        vector3d v = make_vector(0., 0., 0.);
        v[ordinates[0]] = radius * std::cos(i * delta);
        v[ordinates[1]] = radius * std::sin(i * delta);
        vertices[i] = base + v;
        v[ordinates[2]] = height;
        vertices[i + k + 1] = base + v;
    }
    vertices[k] = base;
    vertices[2 * k + 1] = base + axis;

    auto faces = allocate(&mesh.faces, 4 * k);
    for (int i = 0; i < k; ++i)
    {
        faces[i] = make_vector(i, k, i + 1);
        faces[i + k] = make_vector(i + k + 2, 2 * k + 1, i + k + 1);
    }
    faces[k - 1] = make_vector(k - 1, k, 0);
    faces[2 * k - 1] = make_vector(k + 1, 2 * k + 1, 2 * k);
    for (int i = 0; i < k; ++i)
    {
        faces[2 * i + 2 * k] = make_vector(i, i + 1, i + k + 1);
        faces[2 * i + 2 * k + 1] = make_vector(i + 1, i + k + 2, i + k + 1);
    }
    faces[4 * k - 2] = make_vector(k - 1, 0, 2 * k);
    faces[4 * k - 1] = make_vector(0, k + 1, 2 * k);

    return mesh;
};

triangle_mesh
make_sphere(
    vector3d const& center, double radius, int theta_count, int phi_count)
{
    triangle_mesh mesh;

    int kt = 8;
    if (theta_count > kt)
    {
        kt = theta_count;
    }

    int kp = 8;
    if (phi_count > kp)
    {
        kp = phi_count;
    }

    double deltat = (2.0 * pi) / kt;
    double deltap = pi / (double(kp) - 1.0);
    auto vertices = allocate(&mesh.vertices, kt * (kp - 2) + 2);
    auto faces = allocate(&mesh.faces, (kp - 2) * 2 * kt);

    int k = 1;
    int f = 0;
    vertices[0] = center - make_vector(0., 0., radius);
    int kL = 0;
    for (int j = 1; j < kp - 1; ++j)
    {
        double z = center[2] - radius * std::cos(j * deltap);
        vector3d v = make_vector(0., 0., z);
        double rL = radius * std::sin(j * deltap);
        for (int i = 0; i < kt; ++i)
        {
            // Make next batch of vertices
            v[0] = center[0] + rL * std::cos(i * deltat);
            v[1] = center[1] + rL * std::sin(i * deltat);
            vertices[k + i] = v;
        }

        if (j == 1)
        {
            // First level
            for (int i = 0; i < kt - 1; ++i)
            {
                faces[f++] = make_vector(k + i, k + i + 1, 0);
            }
            faces[f++] = make_vector(k + kt - 1, k, 0);
        }
        else
        {
            // All other levels
            kL = k - kt;
            for (int i = 0; i < kt - 1; ++i)
            {
                faces[f++] = make_vector(kL + i + 1, kL + i, k + i);
                faces[f++] = make_vector(k + i, k + i + 1, kL + i + 1);
            }
            faces[f++] = make_vector(kL, kL + kt - 1, k + kt - 1);
            faces[f++] = make_vector(k + kt - 1, k, kL);
        }
        k += kt;
    }
    // Last Level
    vertices[k] = center + make_vector(0., 0., radius);
    kL = k - kt;
    for (int i = 0; i < kt - 1; ++i)
    {
        faces[f++] = make_vector(kL + i + 1, kL + i, k);
    }
    faces[f++] = make_vector(kL + kt - 1, kL, k);
    // std::cout << "Faces: " << f << " Edges: " << k+1 << std::endl;
    return mesh;
}

triangle_mesh
make_pyramid(
    vector3d const& base,
    double width,
    double length,
    double height,
    unsigned axis_direction)
{
    triangle_mesh mesh;

    unsigned direction = 2;
    if (axis_direction < 3)
    {
        direction = axis_direction;
    }
    vector3i ordinates;
    if (direction == 0)
    {
        ordinates = make_vector(1, 2, 0);
    }
    if (direction == 1)
    {
        ordinates = make_vector(2, 0, 1);
    }
    if (direction == 2)
    {
        ordinates = make_vector(0, 1, 2);
    }

    vector3d axis = make_vector(0.0, 0.0, 0.0);
    axis[direction] = height;

    auto vertices = allocate(&mesh.vertices, 5);

    vector3d v = base;
    v[ordinates[0]] -= 0.5 * width;
    v[ordinates[1]] -= 0.5 * length;
    vertices[0] = v;
    v[ordinates[0]] += width;
    vertices[1] = v;
    v[ordinates[1]] += length;
    vertices[2] = v;
    v[ordinates[0]] -= width;
    vertices[3] = v;
    v = base;
    v[ordinates[2]] += height;
    vertices[4] = v;

    auto faces = allocate(&mesh.faces, 6);
    faces[0] = make_vector(0, 1, 2);
    faces[1] = make_vector(0, 2, 3);
    faces[2] = make_vector(0, 4, 1);
    faces[3] = make_vector(1, 4, 2);
    faces[4] = make_vector(2, 4, 3);
    faces[5] = make_vector(3, 4, 0);

    return mesh;
}

triangle_mesh
make_parallelepiped(
    vector3d const& corner,
    vector3d const& a,
    vector3d const& b,
    vector3d const& c)
{
    triangle_mesh mesh;

    auto vertices = allocate(&mesh.vertices, 8);
    vertices[0] = corner;
    vertices[1] = corner + a;
    vertices[2] = corner + b;
    vertices[3] = corner + a + b;
    vertices[4] = corner + c;
    vertices[5] = corner + a + c;
    vertices[6] = corner + b + c;
    vertices[7] = corner + a + b + c;

    auto faces = allocate(&mesh.faces, 12);
    faces[0] = make_vector(0, 3, 1);
    faces[1] = make_vector(0, 2, 3);
    faces[2] = make_vector(0, 1, 5);
    faces[3] = make_vector(0, 5, 4);
    faces[4] = make_vector(0, 4, 2);
    faces[5] = make_vector(2, 4, 6);
    faces[6] = make_vector(4, 5, 6);
    faces[7] = make_vector(5, 7, 6);
    faces[8] = make_vector(2, 6, 3);
    faces[9] = make_vector(3, 6, 7);
    faces[10] = make_vector(1, 3, 5);
    faces[11] = make_vector(3, 7, 5);

    return mesh;
};

typedef boost::compressed_pair<unsigned char, unsigned char> cpair;

typedef line_segment<2, double> line_segment2;

double
interpolate_value(double ss, double tol, double ptb, double a, double b)
{
    double dsq = (b * b + ss);
    if (dsq < a * a)
    {
        return ((a < 0) ? 1.0 : -1.0) * std::sqrt(dsq);
        // return -std::sqrt(dsq);
    }
    else
    {
        return std::fabs(a) > tol ? -a : -a + ptb;
    }
}

vertex3
interpolate_position(
    vector3d const& origin,
    vector3d const& extent,
    int c,
    int r,
    double a,
    double b)
{
    double u = origin[c] - a * (extent[c] - origin[c]) / (b - a);
    switch (c)
    {
        case 0: {
            switch (r)
            {
                case 0: {
                    return make_vector(u, origin[1], origin[2]);
                }
                case 1: {
                    return make_vector(u, extent[1], origin[2]);
                }
                case 2: {
                    return make_vector(u, origin[1], extent[2]);
                }
                case 3: {
                    return make_vector(u, extent[1], extent[2]);
                }
                default: {
                    return vertex3();
                }
            }
            break;
        }
        case 1: {
            switch (r)
            {
                case 0: {
                    return make_vector(origin[0], u, origin[2]);
                }
                case 1: {
                    return make_vector(extent[0], u, origin[2]);
                }
                case 2: {
                    return make_vector(origin[0], u, extent[2]);
                }
                case 3: {
                    return make_vector(extent[0], u, extent[2]);
                }
                default: {
                    return vertex3();
                }
            }
            break;
        }
        case 2: {
            switch (r)
            {
                case 0: {
                    return make_vector(origin[0], origin[1], u);
                }
                case 1: {
                    return make_vector(extent[0], origin[1], u);
                }
                case 2: {
                    return make_vector(origin[0], extent[1], u);
                }
                case 3: {
                    return make_vector(extent[0], extent[1], u);
                }
                default: {
                    return vertex3();
                }
            }
            break;
        }
        default: {
            return vertex3();
        }
    }
}

typedef std::vector<vertex3> growable_vertex3_array;

typedef std::vector<face3> growable_face3_array;

api(struct)
struct growable_triangle_mesh
{
    growable_vertex3_array vertices;
    growable_face3_array faces;
};

template<class T>
void
vector_to_array(array<T>* array, std::vector<T> const& vector)
{
    size_t size = vector.size();
    if (size != 0)
    {
        auto p = allocate(array, size);
        memcpy(p, &vector[0], sizeof(T) * size);
    }
    else
    {
        clear(array);
    }
}

static triangle_mesh
collapse_mesh(growable_triangle_mesh const& growable)
{
    triangle_mesh mesh;
    vector_to_array(&mesh.vertices, growable.vertices);
    vector_to_array(&mesh.faces, growable.faces);
    return mesh;
}

typedef std::vector<vertex3> growable_vertex3_array;

typedef std::vector<face3> growable_face3_array;

api(struct)
struct growable_triangle_mesh_with_normals
{
    growable_vertex3_array vertex_positions;
    growable_vertex3_array vertex_normals;
    growable_face3_array face_position_indices;
    growable_face3_array face_normal_indices;
};

static triangle_mesh_with_normals
collapse_mesh(growable_triangle_mesh_with_normals const& growable)
{
    triangle_mesh_with_normals mesh;
    vector_to_array(&mesh.vertex_positions, growable.vertex_positions);
    vector_to_array(&mesh.vertex_normals, growable.vertex_normals);
    vector_to_array(
        &mesh.face_position_indices, growable.face_position_indices);
    vector_to_array(&mesh.face_normal_indices, growable.face_normal_indices);
    return mesh;
}

image<3, float, shared>
set_data_for_structure(
    image<3, float, shared> const& img,
    structure_geometry const& structure,
    float threshold,
    bool setDataInside)
{
    image<3, float, unique> tmp;
    create_image(tmp, img.size);
    set_spatial_mapping(
        tmp,
        img.origin,
        make_vector(img.axes[0][0], img.axes[1][1], img.axes[2][2]));
    set_value_mapping(
        tmp, img.value_mapping.intercept, img.value_mapping.slope, img.units);

    auto image_const_view = as_const_view(img);
    unsigned kk = 0;
    for (unsigned int k = 0; k < img.size[2]; ++k)
    {
        double z = img.origin[2] + img.axes[2][2] * k;
        for (unsigned int j = 0; j < img.size[1]; ++j)
        {
            double y = img.origin[1] + img.axes[1][1] * j;
            for (unsigned int i = 0; i < img.size[0]; ++i)
            {
                double x = img.origin[0] + img.axes[0][0] * i;
                if (is_inside(structure, make_vector(x, y, z))
                    == setDataInside)
                {
                    tmp.pixels.ptr[kk]
                        = (image_const_view.pixels[kk] > threshold) ? 0.0f
                                                                    : 1.0f;
                }
                else
                {
                    tmp.pixels.ptr[kk] = 0.0f;
                }
                ++kk;
            }
        }
    }
    return share(tmp);
}

image<3, double, shared>
set_data_for_structure(
    image<3, double, shared> const& img,
    structure_geometry const& structure,
    double threshold,
    bool setDataInside)
{
    image<3, double, unique> tmp;
    create_image(tmp, img.size);
    set_spatial_mapping(
        tmp,
        img.origin,
        make_vector(img.axes[0][0], img.axes[1][1], img.axes[2][2]));
    set_value_mapping(
        tmp, img.value_mapping.intercept, img.value_mapping.slope, img.units);

    auto image_const_view = as_const_view(img);
    unsigned kk = 0;
    for (unsigned int k = 0; k < img.size[2]; ++k)
    {
        double z = img.origin[2] + img.axes[2][2] * k;
        for (unsigned int j = 0; j < img.size[1]; ++j)
        {
            double y = img.origin[1] + img.axes[1][1] * j;
            for (unsigned int i = 0; i < img.size[0]; ++i)
            {
                double x = img.origin[0] + img.axes[0][0] * i;
                if (is_inside(structure, make_vector(x, y, z))
                    == setDataInside)
                {
                    tmp.pixels.ptr[kk]
                        = (image_const_view.pixels[kk] > threshold) ? 0.0
                                                                    : 1.0;
                }
                else
                {
                    tmp.pixels.ptr[kk] = 0.0;
                }
                ++kk;
            }
        }
    }
    return share(tmp);
}

// loading .OBJ files
triangle_mesh_with_normals
load_mesh_from_obj(cradle::file_path const& path)
{
    triangle_mesh_with_normals mesh;
    std::ifstream f;
    f.open(path.string(), std::ios::in);
    if (!f)
    {
        throw new cradle::file_error(path, "unable to open OBJ file");
        return mesh;
    }
    mesh = load_mesh_from_obj(f);
    f.close();
    return mesh;
}

// loading .OBJ files
triangle_mesh_with_normals
load_mesh_from_obj(std::istream& obj)
{
    growable_triangle_mesh_with_normals mesh;

    std::string line;
    while (std::getline(obj, line))
    {
        boost::trim_right(line);
        if (line.c_str()[0] == '#')
        {
            // skip comments
            continue;
        }

        std::vector<string> tokens;
        boost::split(
            tokens, line, boost::is_space(), boost::token_compress_on);

        if (tokens.size())
        {
            if ("v" == tokens[0])
            {
                if (tokens.size() != 4)
                {
                    throw cradle::exception("Error in OBJ format");
                }
                mesh.vertex_positions.push_back(make_vector(
                    atof(tokens[1].c_str()),
                    atof(tokens[2].c_str()),
                    atof(tokens[3].c_str())));
            }
            else if ("vn" == tokens[0])
            {
                if (tokens.size() != 4)
                {
                    throw cradle::exception("Error in OBJ format");
                }
                mesh.vertex_normals.push_back(make_vector(
                    atof(tokens[1].c_str()),
                    atof(tokens[2].c_str()),
                    atof(tokens[3].c_str())));
            }
            else if ("f" == tokens[0])
            {
                if (tokens.size() != 4)
                {
                    throw new cradle::exception("Error in OBJ format");
                }

                face3 face_p;
                face3 face_n = make_vector<int>(-1, -1, -1);
                for (int i = 1; i < 4; ++i)
                {
                    if (tokens[i].find('/'))
                    {
                        std::vector<string> vertex_triplet;
                        boost::split(
                            vertex_triplet, tokens[i], boost::is_any_of("/"));
                        if (vertex_triplet.size() != 3)
                        {
                            throw new cradle::exception("Error in OBJ format");
                        }
                        face_p[i - 1] = atoi(vertex_triplet[0].c_str()) - 1;
                        face_n[i - 1] = atoi(vertex_triplet[2].c_str()) - 1;
                    }
                    else
                    {
                        face_p[i - 1] = atoi(tokens[i].c_str()) - 1;
                    }
                }
                mesh.face_position_indices.push_back(face_p);
                mesh.face_normal_indices.push_back(face_n);
            }
            // ignore other kinds of lines
        }
    }

    return collapse_mesh(mesh);
}

// convert triangle_mesh_with_normals to a plain old triangle_mesh
triangle_mesh
remove_normals(triangle_mesh_with_normals const& orig)
{
    triangle_mesh mesh;
    mesh.vertices = orig.vertex_positions;
    mesh.faces = orig.face_position_indices;
    return mesh;
}

triangle3d
get_triangle(triangle_mesh const& mesh, face3_array::size_type index)
{
    face3 face = mesh.faces[index];
    return triangle3d(
        mesh.vertices[face[0]],
        mesh.vertices[face[1]],
        mesh.vertices[face[2]]);
}

vector3d
get_normal(triangle_mesh const& mesh, face3_array::size_type index)
{
    face3 face = mesh.faces[index];
    vertex3 v0 = mesh.vertices[face[0]];
    return unit(
        cross(mesh.vertices[face[1]] - v0, mesh.vertices[face[2]] - v0));
}
// calculate the bounds of a triangle mesh
box3d
bounding_box(triangle_mesh const& mesh)
{
    if (mesh.vertices.size() < 1)
    {
        return cradle::box<3, double>();
    }

    vertex3_array::const_iterator iter = mesh.vertices.begin();
    vector3d mins = (*iter);
    vector3d maxs = (*iter);
    ++iter;
    vertex3_array::const_iterator vertices_end = mesh.vertices.end();
    for (; iter != vertices_end; ++iter)
    {
        for (int i = 0; i < 3; ++i)
        {
            if ((*iter)[i] < mins[i])
                mins[i] = (*iter)[i];
            if ((*iter)[i] > maxs[i])
                maxs[i] = (*iter)[i];
        }
    }
    return cradle::box<3, double>(mins, maxs - mins);
}

// calculate the bounds for a face of a triangle mesh
box3d
bounding_box(triangle_mesh const& mesh, face3_array::size_type index)
{
    return bounding_box(get_triangle(mesh, index));
}

struct sum
{
    triangle_mesh const& mesh;
    line_segment<3, double> const& segment;

    sum(triangle_mesh const& m, line_segment<3, double> const& s)
        : mesh(m), segment(s)
    {
    }

    bool
    operator()(unsigned const& index, int* value)
    {
        segment_triangle_intersection_type type
            = is_intersecting(segment, get_triangle(mesh, index));
        if (type == segment_triangle_intersection_type::NONE)
        {
            *value = 0;
            return true;
        }
        else if (type == segment_triangle_intersection_type::FACE)
        {
            *value = 1;
            return true;
        }
        else
        {
            return false;
        }
    }
};

typedef std::pair<int, int> edge;

edge
make_edge(int a, int b)
{
    if (a > b)
    {
        std::swap(a, b);
    }
    return edge(a, b);
}

struct edge_state
{
    unsigned char state;

    edge_state() : state(0)
    {
    }

    void
    update(bool visible)
    {
        if (visible)
        {
            ++state;
        }
        else
        {
            --state;
        }
    }
};

bool
is_coincident(clipper_point const& p1, clipper_point const& p2)
{
    return p1.X == p2.X && p1.Y == p2.Y;
}

typedef std::map<edge, edge_state> edge_map;

void
erase_connectivity(
    unsigned short* connectivity_counts, int** connectivity, int i1, int i2)
{
    // Erase connectivity for first index
    int count1 = connectivity_counts[i1];
    int* c1 = connectivity[i1];
    for (int i = 0; i < count1; ++i)
    {
        if (*c1 == i2)
        {
            *c1 = -1;
            break;
        }
        ++c1;
    }

    // Erase connectivity for second index
    int count2 = connectivity_counts[i2];
    int* c2 = connectivity[i2];
    for (int i = 0; i < count2; ++i)
    {
        if (*c2 == i1)
        {
            *c2 = -1;
            break;
        }
        ++c2;
    }
}

vector2d
point_to_plane(plane<double> const& pl, vector3d const& pt)
{
    // Compute reference vector (shortcut)
    vector3d reference = cross(pl.normal, make_vector(0.0, 0.0, 1.0));
    if (length2(reference) < 1.0e-20)
    {
        reference = cross(pl.normal, make_vector(1.0, 0.0, 0.0));
    }
    reference = reference / length(reference);
    vector3d plU = reference - (dot(reference, pl.normal) * pl.normal);
    vector3d plV = cross(pl.normal, plU);
    vector3d v = pt - pl.point;
    return make_vector<double>(dot(v, plU), dot(v, plV));
}

bool
triangle_segment_intersection(
    vector3d const& s1,
    vector3d const& s2,
    triangle<3, double> const& t,
    double& u)
{
    vector3d normal = cross(t[1] - t[0], t[2] - t[0]);

    plane<double> pl(t[1], normal);

    double dist1 = dot(s1 - pl.point, pl.normal);
    double dist2 = dot(s2 - pl.point, pl.normal);
    bool intersects = (dist1 * dist2 < 0);
    if (!intersects)
        return false;

    vector2d vp1 = point_to_plane(pl, t[0]);
    vector2d vp2 = point_to_plane(pl, t[1]);
    vector2d vp3 = point_to_plane(pl, t[2]);

    u = std::fabs(dist1) / (std::fabs(dist1) + std::fabs(dist2) + 1.0e-20);
    vector2d vp = point_to_plane(pl, point_along(s1, s2, u));

    vector2d d1 = vp2 - vp1;
    vector2d d2 = vp3 - vp1;

    double denom = cross(d1, d2);
    if (denom == 0.0)
    {
        return false;
    }

    double numer1 = cross(vp, d2) - cross(vp1, d2);
    double a = numer1 / denom;
    if (a < -1.0e-12)
        return false;

    double numer2 = cross(vp1, d1) - cross(vp, d1);
    double b = numer2 / denom;
    if (b < -1.0e-12)
        return false; // I believe there may be a problem in this computation
                      // somewhere - Sal (02.09.2009)

    double sum = a + b;
    if (sum > (1.0 + 1.0e-12))
        return false;

    return true;
}

bool
get_first_last_intersection(
    vector3d const& s1,
    vector3d const& s2,
    std::vector<triangle_mesh> const& targets,
    vector3d& pt1,
    vector3d& pt2,
    double& uu1,
    double& uu2)
{
    double u1 = 1.0e100;
    double u2 = -1.0e100;
    for (size_t i = 0; i < targets.size(); ++i)
    {
        for (size_t j = 0; j < targets[i].faces.size(); ++j)
        {
            double temp_u = 0.0;
            bool isIntersected = triangle_segment_intersection(
                s1, s2, get_triangle(targets[i], j), temp_u);
            if (isIntersected)
            {
                if (temp_u < u1)
                {
                    u1 = temp_u;
                }
                if (temp_u > u2)
                {
                    u2 = temp_u;
                }
            }
            // if (isIntersected) std::cout << "Intersect: " << temp_u << " "
            // << u << std::endl;
        }
    }
    if (u2 > 0.0)
    {
        pt1 = point_along(s1, s2, u1);
        pt2 = point_along(s1, s2, u2);
        uu1 = u1;
        uu2 = u2;
        return true;
    }
    return false;
}

bool
get_first_last_intersection(
    vector3d const& s1,
    vector3d const& s2,
    triangle_mesh const& mesh,
    vector3d& pt1,
    vector3d& pt2,
    double& uu1,
    double& uu2)
{
    double u1 = 1.0e100;
    double u2 = -1.0e100;
    for (size_t j = 0; j < mesh.faces.size(); ++j)
    {
        double temp_u = 0.0;
        bool isIntersected = triangle_segment_intersection(
            s1, s2, get_triangle(mesh, j), temp_u);
        if (isIntersected)
        {
            if (temp_u < u1)
            {
                u1 = temp_u;
            }
            if (temp_u > u2)
            {
                u2 = temp_u;
            }
        }
    }
    if (u2 > 0.0)
    {
        pt1 = point_along(s1, s2, u1);
        pt2 = point_along(s1, s2, u2);
        uu1 = u1;
        uu2 = u2;
        return true;
    }
    return false;
}

bool
get_deepest_intersection(
    vector3d const& s1,
    vector3d const& s2,
    std::vector<triangle_mesh> const& targets,
    vector3d& pt,
    double& uu)
{
    double u = 0.0;
    for (size_t i = 0; i < targets.size(); ++i)
    {
        for (size_t j = 0; j < targets[i].faces.size(); ++j)
        {
            double temp_u = 0.0;
            bool isIntersected = triangle_segment_intersection(
                s1, s2, get_triangle(targets[i], j), temp_u);
            if (isIntersected && (temp_u > u))
            {
                u = temp_u;
            }
            // if (isIntersected) std::cout << "Intersect: " << temp_u << " "
            // << u << std::endl;
        }
    }
    if (u > 0.0)
    {
        pt = point_along(s1, s2, u);
        uu = u;
        return true;
    }
    return false;
}

double
compute_solid_angle(triangle_mesh const& mesh, vector3d const& p)
{
    double ang = 0.0;
    face3_array::const_iterator end = mesh.faces.end();
    for (face3_array::const_iterator iter = mesh.faces.begin(); iter != end;
         ++iter)
    {
        // Get triangle
        vector3d a = mesh.vertices[(*iter)[0]] - p;
        vector3d b = mesh.vertices[(*iter)[1]] - p;
        vector3d c = mesh.vertices[(*iter)[2]] - p;

        double alength = length(a);
        double blength = length(b);
        double clength = length(c);

        double numer = dot(a, cross(b, c));
        double denom = (alength * blength * clength) + clength * dot(a, b)
                       + blength * dot(a, c) + alength * dot(b, c);

        ang += std::atan2(numer, denom);
    }
    return 2.0 * ang;
}

// transforms the vertices in a triangle mesh
// (leaves the original alone and creates a transformed COPY)
triangle_mesh
transform_triangle_mesh(
    const triangle_mesh& original, const matrix<4, 4, double>& matrix)
{
    triangle_mesh output_mesh;
    output_mesh.faces = original.faces;
    auto n_vertices = original.vertices.size();
    auto output_vertices = allocate(&output_mesh.vertices, n_vertices);
    for (size_t i = 0; i != n_vertices; ++i)
        output_vertices[i] = transform_point(matrix, original.vertices[i]);
    return output_mesh;
}

triangle_mesh_with_normals
transform_triangle_mesh(
    triangle_mesh_with_normals const& original,
    matrix<4, 4, double> const& matrix)
{
    triangle_mesh_with_normals output_mesh;

    {
        output_mesh.face_position_indices = original.face_position_indices;
        auto n_vertices = original.vertex_positions.size();
        auto output_vertices
            = allocate(&output_mesh.vertex_positions, n_vertices);
        for (size_t i = 0; i != n_vertices; ++i)
        {
            output_vertices[i]
                = transform_point(matrix, original.vertex_positions[i]);
        }
    }

    {
        output_mesh.face_normal_indices = original.face_normal_indices;
        auto n_normals = original.vertex_normals.size();
        auto output_normals = allocate(&output_mesh.vertex_normals, n_normals);
        for (size_t i = 0; i != n_normals; ++i)
        {
            output_normals[i]
                = transform_vector(matrix, original.vertex_normals[i]);
        }
    }

    return output_mesh;
}

} // namespace cradle


//  (C) Copyright Jeremy William Murphy 2016.

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

#ifndef BOOST_MATH_COMMON_FACTOR_RT_HPP
#define BOOST_MATH_COMMON_FACTOR_RT_HPP

#include <boost/assert.hpp>
#include <boost/core/enable_if.hpp>
#include <boost/mpl/and.hpp>
#include <boost/type_traits.hpp>

#include <boost/config.hpp>  // for BOOST_NESTED_TEMPLATE, etc.
#include <boost/limits.hpp>  // for std::numeric_limits
#include <climits>           // for CHAR_MIN
#include <boost/detail/workaround.hpp>
#include <iterator>
#include <algorithm>
#include <limits>

#if (defined(BOOST_MSVC) || (defined(__clang__) && defined(__c2__)) || (defined(BOOST_INTEL) && defined(_MSC_VER))) && (defined(_M_IX86) || defined(_M_X64))
#include <intrin.h>
#endif

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127 4244)  // Conditional expression is constant
#endif

namespace boost {
   namespace math {

      template <class T, bool a = is_unsigned<T>::value || (std::numeric_limits<T>::is_specialized && !std::numeric_limits<T>::is_signed)>
      struct gcd_traits_abs_defaults
      {
         inline static const T& abs(const T& val) { return val; }
      };
      template <class T>
      struct gcd_traits_abs_defaults<T, false>
      {
         inline static T abs(const T& val)
         {
            using std::abs;
            return abs(val);
         }
      };

      template <class T>
      struct gcd_traits_defaults : public gcd_traits_abs_defaults<T>
      {
         BOOST_FORCEINLINE static unsigned make_odd(T& val)
         {
            unsigned r = 0;
            while(!(val & 1u))
            {
               val >>= 1;
               ++r;
            }
            return r;
         }
         inline static bool less(const T& a, const T& b)
         {
            return a < b;
         }

         enum method_type
         {
            method_euclid = 0,
            method_binary = 1,
            method_mixed = 2,
         };

         static const method_type method =
            boost::has_right_shift_assign<T>::value && boost::has_left_shift_assign<T>::value && boost::has_less<T>::value && boost::has_modulus<T>::value
            ? method_mixed :
            boost::has_right_shift_assign<T>::value && boost::has_left_shift_assign<T>::value && boost::has_less<T>::value
            ? method_binary : method_euclid;
      };
      //
      // Default gcd_traits just inherits from defaults:
      //
      template <class T>
      struct gcd_traits : public gcd_traits_defaults<T> {};
      //
      // Special handling for polynomials:
      //
      namespace tools {
         template <class T>
         class polynomial;
      }

      template <class T>
      struct gcd_traits<boost::math::tools::polynomial<T> > : public gcd_traits_defaults<T>
      {
         static const boost::math::tools::polynomial<T>& abs(const boost::math::tools::polynomial<T>& val) { return val; }
      };
      //
      // Some platforms have fast bitscan operations, that allow us to implement
      // make_odd much more efficiently:
      //
#if (defined(BOOST_MSVC) || (defined(__clang__) && defined(__c2__)) || (defined(BOOST_INTEL) && defined(_MSC_VER))) && (defined(_M_IX86) || defined(_M_X64))
#pragma intrinsic(_BitScanForward,)
      template <>
      struct gcd_traits<unsigned long> : public gcd_traits_defaults<unsigned long>
      {
         BOOST_FORCEINLINE static unsigned find_lsb(unsigned long val)
         {
            unsigned long result;
            _BitScanForward(&result, val);
            return result;
         }
         BOOST_FORCEINLINE static unsigned make_odd(unsigned long& val)
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };

#ifdef _M_X64
#pragma intrinsic(_BitScanForward64)
      template <>
      struct gcd_traits<unsigned __int64> : public gcd_traits_defaults<unsigned __int64>
      {
         BOOST_FORCEINLINE static unsigned find_lsb(unsigned __int64 mask)
         {
            unsigned long result;
            _BitScanForward64(&result, mask);
            return result;
         }
         BOOST_FORCEINLINE static unsigned make_odd(unsigned __int64& val)
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
#endif
      //
      // Other integer type are trivial adaptations of the above,
      // this works for signed types too, as by the time these functions
      // are called, all values are > 0.
      //
      template <> struct gcd_traits<long> : public gcd_traits_defaults<long>
      { BOOST_FORCEINLINE static unsigned make_odd(long& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<unsigned int> : public gcd_traits_defaults<unsigned int>
      { BOOST_FORCEINLINE static unsigned make_odd(unsigned int& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<int> : public gcd_traits_defaults<int>
      { BOOST_FORCEINLINE static unsigned make_odd(int& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<unsigned short> : public gcd_traits_defaults<unsigned short>
      { BOOST_FORCEINLINE static unsigned make_odd(unsigned short& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<short> : public gcd_traits_defaults<short>
      { BOOST_FORCEINLINE static unsigned make_odd(short& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<unsigned char> : public gcd_traits_defaults<unsigned char>
      { BOOST_FORCEINLINE static unsigned make_odd(unsigned char& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<signed char> : public gcd_traits_defaults<signed char>
      { BOOST_FORCEINLINE static signed make_odd(signed char& val){ signed result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<char> : public gcd_traits_defaults<char>
      { BOOST_FORCEINLINE static unsigned make_odd(char& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<wchar_t> : public gcd_traits_defaults<wchar_t>
      { BOOST_FORCEINLINE static unsigned make_odd(wchar_t& val){ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
#ifdef _M_X64
      template <> struct gcd_traits<__int64> : public gcd_traits_defaults<__int64>
      { BOOST_FORCEINLINE static unsigned make_odd(__int64& val){ unsigned result = gcd_traits<unsigned __int64>::find_lsb(val); val >>= result; return result; } };
#endif

#elif defined(BOOST_GCC) || defined(__clang__) || (defined(BOOST_INTEL) && defined(__GNUC__))

      template <>
      struct gcd_traits<unsigned> : public gcd_traits_defaults<unsigned>
      {
         BOOST_FORCEINLINE static unsigned find_lsb(unsigned mask)
         {
            return __builtin_ctz(mask);
         }
         BOOST_FORCEINLINE static unsigned make_odd(unsigned& val)
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
      template <>
      struct gcd_traits<unsigned long> : public gcd_traits_defaults<unsigned long>
      {
         BOOST_FORCEINLINE static unsigned find_lsb(unsigned long mask)
         {
            return __builtin_ctzl(mask);
         }
         BOOST_FORCEINLINE static unsigned make_odd(unsigned long& val)
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
      template <>
      struct gcd_traits<boost::ulong_long_type> : public gcd_traits_defaults<boost::ulong_long_type>
      {
         BOOST_FORCEINLINE static unsigned find_lsb(boost::ulong_long_type mask)
         {
            return __builtin_ctzll(mask);
         }
         BOOST_FORCEINLINE static unsigned make_odd(boost::ulong_long_type& val)
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
      //
      // Other integer type are trivial adaptations of the above,
      // this works for signed types too, as by the time these functions
      // are called, all values are > 0.
      //
      template <> struct gcd_traits<boost::long_long_type> : public gcd_traits_defaults<boost::long_long_type>
      {
         BOOST_FORCEINLINE static unsigned make_odd(boost::long_long_type& val) { unsigned result = gcd_traits<boost::ulong_long_type>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<long> : public gcd_traits_defaults<long>
      {
         BOOST_FORCEINLINE static unsigned make_odd(long& val) { unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<int> : public gcd_traits_defaults<int>
      {
         BOOST_FORCEINLINE static unsigned make_odd(int& val) { unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<unsigned short> : public gcd_traits_defaults<unsigned short>
      {
         BOOST_FORCEINLINE static unsigned make_odd(unsigned short& val) { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<short> : public gcd_traits_defaults<short>
      {
         BOOST_FORCEINLINE static unsigned make_odd(short& val) { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<unsigned char> : public gcd_traits_defaults<unsigned char>
      {
         BOOST_FORCEINLINE static unsigned make_odd(unsigned char& val) { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<signed char> : public gcd_traits_defaults<signed char>
      {
         BOOST_FORCEINLINE static signed make_odd(signed char& val) { signed result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<char> : public gcd_traits_defaults<char>
      {
         BOOST_FORCEINLINE static unsigned make_odd(char& val) { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<wchar_t> : public gcd_traits_defaults<wchar_t>
      {
         BOOST_FORCEINLINE static unsigned make_odd(wchar_t& val) { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
#endif

namespace detail
{

   //
   // The Mixed Binary Euclid Algorithm
   // Sidi Mohamed Sedjelmaci
   // Electronic Notes in Discrete Mathematics 35 (2009) 169-176
   //
   template <class T>
   T mixed_binary_gcd(T u, T v)
   {
      using std::swap;
      if(gcd_traits<T>::less(u, v))
         swap(u, v);

      unsigned shifts = 0;

      if(!u)
         return v;
      if(!v)
         return u;

      shifts = (std::min)(gcd_traits<T>::make_odd(u), gcd_traits<T>::make_odd(v));

      while(gcd_traits<T>::less(1, v))
      {
         u %= v;
         v -= u;
         if(!u)
            return v << shifts;
         if(!v)
            return u << shifts;
         gcd_traits<T>::make_odd(u);
         gcd_traits<T>::make_odd(v);
         if(gcd_traits<T>::less(u, v))
            swap(u, v);
      }
      return (v == 1 ? v : u) << shifts;
   }

    /** Stein gcd (aka 'binary gcd')
     *
     * From Mathematics to Generic Programming, Alexander Stepanov, Daniel Rose
     */
    template <typename SteinDomain>
    SteinDomain Stein_gcd(SteinDomain m, SteinDomain n)
    {
        using std::swap;
        BOOST_ASSERT(m >= 0);
        BOOST_ASSERT(n >= 0);
        if (m == SteinDomain(0))
            return n;
        if (n == SteinDomain(0))
            return m;
        // m > 0 && n > 0
        int d_m = gcd_traits<SteinDomain>::make_odd(m);
        int d_n = gcd_traits<SteinDomain>::make_odd(n);
        // odd(m) && odd(n)
        while (m != n)
        {
            if (n > m)
                swap(n, m);
            m -= n;
            gcd_traits<SteinDomain>::make_odd(m);
        }
        // m == n
        m <<= (std::min)(d_m, d_n);
        return m;
    }

    /** Euclidean algorithm
     *
     * From Mathematics to Generic Programming, Alexander Stepanov, Daniel Rose
     *
     */
    template <typename EuclideanDomain>
    inline EuclideanDomain Euclid_gcd(EuclideanDomain a, EuclideanDomain b)
    {
        using std::swap;
        while (b != EuclideanDomain(0))
        {
            a %= b;
            swap(a, b);
        }
        return a;
    }

    template <typename T>
    inline BOOST_DEDUCED_TYPENAME enable_if_c<gcd_traits<T>::method == gcd_traits<T>::method_mixed, T>::type
       optimal_gcd_select(T const &a, T const &b)
    {
       return detail::mixed_binary_gcd(a, b);
    }

    template <typename T>
    inline BOOST_DEDUCED_TYPENAME enable_if_c<gcd_traits<T>::method == gcd_traits<T>::method_binary, T>::type
       optimal_gcd_select(T const &a, T const &b)
    {
       return detail::Stein_gcd(a, b);
    }

    template <typename T>
    inline BOOST_DEDUCED_TYPENAME enable_if_c<gcd_traits<T>::method == gcd_traits<T>::method_euclid, T>::type
       optimal_gcd_select(T const &a, T const &b)
    {
       return detail::Euclid_gcd(a, b);
    }

    template <class T>
    inline T lcm_imp(const T& a, const T& b)
    {
       T temp = boost::math::detail::optimal_gcd_select(a, b);
#if BOOST_WORKAROUND(BOOST_GCC_VERSION, < 40500)
       return (temp != T(0)) ? T(a / temp * b) : T(0);
#else
       return temp ? T(a / temp * b) : T(0);
#endif
    }

} // namespace detail

template <typename Integer>
inline Integer gcd(Integer const &a, Integer const &b)
{
    return detail::optimal_gcd_select(static_cast<Integer>(gcd_traits<Integer>::abs(a)), static_cast<Integer>(gcd_traits<Integer>::abs(b)));
}

template <typename Integer>
inline Integer lcm(Integer const &a, Integer const &b)
{
   return detail::lcm_imp(static_cast<Integer>(gcd_traits<Integer>::abs(a)), static_cast<Integer>(gcd_traits<Integer>::abs(b)));
}

/**
 * Knuth, The Art of Computer Programming: Volume 2, Third edition, 1998
 * Chapter 4.5.2, Algorithm C: Greatest common divisor of n integers.
 *
 * Knuth counts down from n to zero but we naturally go from first to last.
 * We also return the termination position because it might be useful to know.
 *
 * Partly by quirk, partly by design, this algorithm is defined for n = 1,
 * because the gcd of {x} is x. It is not defined for n = 0.
 *
 * @tparam  I   Input iterator.
 * @return  The gcd of the range and the iterator position at termination.
 */
template <typename I>
std::pair<typename std::iterator_traits<I>::value_type, I>
gcd_range(I first, I last)
{
    BOOST_ASSERT(first != last);
    typedef typename std::iterator_traits<I>::value_type T;

    T d = *first++;
    while (d != T(1) && first != last)
    {
        d = gcd(d, *first);
        first++;
    }
    return std::make_pair(d, first);
}

}  // namespace math
}  // namespace boost

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif  // BOOST_MATH_COMMON_FACTOR_RT_HPP