#if HAVE_CONFIG_H
# include <config.h>
#endif

#include "RegPT.h"
#include "SPT.h"
#include "PowerSpectrum.h"
#include "LinearPS.h"
#include "Quadrature.h"
#include "SpecialFunctions.h"
#include "array.h"
#include "Spline.h"
#include <gsl/gsl_sf_bessel.h>

#include <cassert>
#include <cmath>
#include <cstdio>
#include <cerrno>
#include <cstdlib>
#include <stdlib.h>
#include <sys/stat.h>
#include <math.h>
#include <vector>
#include <boost/bind.hpp>
using boost::cref;

// Load Linear PS
RegPT::RegPT(const Cosmology& C, const PowerSpectrum& P_l, real epsrel_)
: C(C), P_l(P_l)
{
    epsrel = epsrel_;
}

//Error in exponential index integral
const double error1 = 1e-3;
//Atsushi limit on k-angular integration
inline double ATS(real k, real r){
 real KMAX = QMAXp/k;
 real KMIN = QMINp/k;
 if(r>=0.5) {
 return 1./(2.*r);
 }
 else{
 return  Min(XMAX, (1.+r*r-KMIN*KMIN)/2./r);
 }
}

inline int num_selec_mag(double kmin, double kmax, double y){
    	int mag_int;
    //	 mag_int = (n2*1.-1)*(y-QMINp/0.5)*0.005/QMAXp + 1./n2; // linear
    //	 mag_int = sqrt((y-QMINp/kmax)*kmin/QMAXp)*(n2-1); //quadratic
    mag_int = (int)round((n2-1)*log((y*kmax)/QMINp)/log(QMAXp*kmax/(kmin*QMINp))); //exponential
    		return mag_int;
}

// spline linear growth for sigma_d^2 integral
Spline F1_reg;
// Regularized PT exponential
static real regpt_exp(const PowerSpectrum& P_l, double q){
  return  P_l(q)/(6.*pow2(M_PI));
}
// Initialization of sigma_d damping term
Spline damp_term;
void RegPT::sigmad_init() const{
vector<double> kval_table, sigd_table;
double k,sigd;
int n3 = 200;
  for(int i = 0; i<n3; i++ ){
    k = QMINp*exp(i*log(2.*QMAXp/(QMINp))/(n3-1.));
    sigd = pow2(k)*Integrate<ExpSub>(bind(regpt_exp, cref(P_l), _1), QMINp, k/2., error1);
    kval_table.push_back(k);
    sigd_table.push_back(sigd);
      }
    damp_term = LinearSpline(kval_table,sigd_table);
  }

  // used to treat f as free parameter in GR template
double rempfr;
void RegPT::rempreg(real f) const{
rempfr = f/fl_spt;
}

// normalization of linear growth
double sig_8r;
// function to set normalization of linear growth to fiducial for data comparisons
void RegPT::Greg(real dfid) const {
  sig_8r = dfid/D_spt;
       }

/* P_{ab}(k) 1-loop in regpt: analytical LCDM and nDGP */
// 1:  P_dd, 2: P_dt , 3: P_tt ;
real RegPT::PLOOPr(int a, real k) const {
  SPT spt(C,P_l,1e-3);
  double D0,D1,D2,D3;
  if(pow2(D_spt/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
  switch (a) {
    case 1:
      D0= pow2(sig_8r)*pow2(D_spt/dnorm_spt);
      D1 = pow4(sig_8r)*spt.P13D(k,1);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      D3 = pow4(sig_8r);
      break;
    case 2:
      D0= -rempfr*fdgp_spt*pow2(sig_8r)*pow2(D_spt/dnorm_spt);
      D1= rempfr*pow4(sig_8r)*spt.P13D(k,2);
      D2 =  pow6(sig_8r)*spt.P13D(k,1)*spt.P13D(k,3)*pow2(dnorm_spt/(D_spt*P_l(k)*2.))/(-rempfr*fdgp_spt);
      D3 =  pow4(sig_8r)*rempfr;
      break;
    case 3:
      D0=  pow2(sig_8r)*pow2(rempfr*fdgp_spt*D_spt/dnorm_spt);
      D1 = rempfr*rempfr* pow4(sig_8r)*spt.P13D(k,3);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      D3 =  pow4(sig_8r)*pow2(rempfr);
      break;
  }
  return exp(-pow2(D_spt/dnorm_spt)*damp_term(k))*(D0*P_l(k)+D1+ D3*spt.P22D(k,a)+pow2(D_spt/dnorm_spt)*damp_term(k)/2.*(2.*D0*P_l(k)+ D1) + pow2(pow2(D_spt/dnorm_spt)*damp_term(k))/4.*D0*P_l(k) + P_l(k)*D2);
}
}

/* P_{ab}(k) 1-loop regpt :  numerical for arbitrary model of gravity - kernel dependent */

// kmin and kmax are only used if the gravity model is scale independant and kernels only need be initialized once
//kmin and kmax then refer to the limits of the desired output range of scales

real RegPT::PLOOPnr(real kmin, real kmax, int a, real k) const {
  SPT spt(C,P_l,1e-3);
  double D0;
  double D1;
  double D2;
  if(pow2(F1_nk/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
  switch (a) {
    case 1:
      D0= pow2(F1_nk/dnorm_spt);
      D1 = spt.P13n(kmin,kmax,1,k);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      break;
    case 2:
      D0= F1_nk*G1_nk/pow2(dnorm_spt);
      D1 =  spt.P13n(kmin,kmax,2,k);
      D2 = spt.P13n(kmin,kmax,1,k)*spt.P13n(kmin,kmax,3,k)*pow2(1./(P_l(k)*2.))/D0;
      break;
    case 3:
      D0=  pow2(G1_nk/dnorm_spt);
      D1 =  spt.P13n(kmin,kmax,3,k);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      break;
  }
  return exp(-pow2(F1_nk/dnorm_spt)*damp_term(k))*(D0*P_l(k) + D1 + spt.P22n(kmin,kmax,a,k) + pow2(F1_nk/dnorm_spt)*damp_term(k)/2.*(2.*D0*P_l(k)+ D1) + pow2(pow2(F1_nk/dnorm_spt)*damp_term(k))/4.*D0*P_l(k) + P_l(k)*D2);
  }
}

/* TNS  Multipoles  */
//bl is linear galaxy bias
// a = 1 : Monopole
// a = 2 : Quadrupole
// a = 3 : Hexdecapole
real RegPT::PTNSMnDGPr(real k, real bl, real sigma_v, int a) const {
  if(pow2(D_spt/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
	return  (pow2(bl)*factL(k,sigma_v,fl_spt,1.,0,a,6)*PLOOPr(1, k) - 2*factL(k,sigma_v,fl_spt,1.,1,a,6)*bl*PLOOPr(2, k) + factL(k,sigma_v,fl_spt,1.,2,a,6)* PLOOPr(3, k)+  pow4(sig_8r)*ABr(k, bl, sigma_v,a));
}
}

/*  Arbitrary model TNS  Multipoles with DFoG term  */
//bl is linear galaxy bias
// a = 1 : Monopole
// a = 2 : Quadrupole
// a = 3 : Hexdecapole
real RegPT::PTNSMmgr(real k, double kmin, double kmax, int a,real bl, real sigma_v1 ) const {
  if(pow2(F1_nk/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
	return  (pow2(bl)*factL(k,sigma_v1,fl_spt,1.,0,a,6)*PLOOPnr(kmin,kmax,1, k)  - 2*factL(k,sigma_v1,fl_spt,1.,1,a,6)*bl*PLOOPnr(kmin,kmax,2, k)  + factL(k,sigma_v1,fl_spt,1.,2,a,6)* PLOOPnr(kmin,kmax,3, k)+  ABnr(kmin,kmax, bl, k, sigma_v1, a));
}
}

//REDSHIFT SPACE MODEL-TNS TERMS with damping

/* Non vanishing A, B and C terms */

// Notes for analytic ABC terms : DEFINITION OF THETA= - DEL V / aHf (and is accounted for in definition of Cross Bispectrum)
// See http://arxiv.org/pdf/1006.0699v1.pdf for derivation

// NOTE ON PARAMETERS: x =k1.k/(k^2*r)

static real ABC(int a, const PowerSpectrum& P_L, real k, real r, real x) {
    real d = 1 + r*r - 2*r*x;
    double dampexpa = exp(-pow2(D_spt/dnorm_spt)*damp_term(k*r)/2.-pow2(D_spt/dnorm_spt)*damp_term(k*sqrt(d))/2.-pow2(D_spt/dnorm_spt)*damp_term(k)/2.);
    double dampexpb = exp(-pow2(D_spt/dnorm_spt)*damp_term(k*sqrt(d))-pow2(D_spt/dnorm_spt)*damp_term(k*r));
    if(d < 1e-5)
        return 0;
    else
		switch(a) {
		// A terms
				//A11
			case 1:
				return dampexpa*(-r*r*r/7.)*(x+6*x*x*x+r*r*x*(-3+10*x*x)+r*(-3+x*x-12*x*x*x*x))*P_L(k) * P_L(k*sqrt(d))/pow2(d);
      	//A12
			case 2:
				return  dampexpa*(r*r*r*r/14.)*(x*x-1)*(-1+7*r*x-6*x*x) * P_L(k) * P_L(k*sqrt(d))/pow2(d);
      	//A22
			case 3:
				return  dampexpa*(r*r*r/14.)*(r*r*x*(13-41*x*x)-4*(x+6*x*x*x)+r*(5+9*x*x+42*x*x*x*x))*P_L(k)*P_L(k*sqrt(d))/pow2(d);
      	//A33
			case 4:
				return dampexpa*(r*r*r/14.)*(1-7*r*x+6*x*x)*(-2*x+r*(-1+3*x*x))*P_L(k)*P_L(k*sqrt(d))/pow2(d);
      	//A11t
			case 5:
				return  dampexpa*(1./7.)*(x+r-2*r*x*x)*(3*r+7*x-10*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
      	//A12t
			case 6:
				return dampexpa*(r/14.)*(x*x-1)*(3*r+7*x-10*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
      	//A22t
			case 7:
				return dampexpa*(1./14.)*(28*x*x+r*x*(25-81*x*x)+r*r*(1-27*x*x+54*x*x*x*x))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
      	//A23t
			case 8:
				return dampexpa*(r/14.)*(-x*x+1)*(r-7*x+6*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//A33t
			case 9:
				return dampexpa*(1./14.)*(-2*x-r+3*r*x*x)*(r-7*x+6*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//a11
			case 10:
				return dampexpa*(-7*x*x+pow3(r)*x*(-3+10*x*x)+3*r*(x+6*pow3(x))+r*r*(6-19*x*x-8*pow4(x)))*P_L(k)*P_L(k*r)/(7.*d);
		//a12
			case 11:
				return dampexpa*r*(-1+x*x)*(6*r-7*(1+r*r)*x + 8*r*x*x)*P_L(k)*P_L(k*r)/(14.*d);
			//a22
			case 12:
				return dampexpa*(-28*x*x+r*r*r*x*(-13+41*x*x)+r*x*(11+73*x*x)-2*r*r*(-9+31*x*x+20*x*x*x*x))*P_L(k)*P_L(k*r)/(14.*d);
		//a33
			case 13:
				return dampexpa*(7*x + r*(-6+7*r*x-8*x*x))*(-2*x+r*(-1+3*x*x))*P_L(k)*P_L(k*r)/(14.*d);
		//B terms
				//B111
			case 14:
				return dampexpb*(r*r/2.)*(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/d;
	   //B112
			case 15:
				return dampexpb*(3*r*r/8.)*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/d;
		//B121
			case 16:
				return dampexpb*(3.*r*r*r*r/8.)*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
			//B122
			case 17:
				return dampexpb*(5*r*r*r*r/16.)*pow3(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
			//B211
			case 18:
				return dampexpb*(r/2.)*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/d;
		//B212
			case 19:
				return dampexpb*(-3*r/4.)*(x*x-1)*(-r-2*x+5*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/d;
			//B221
			case 20:
				return dampexpb*(3*r*r/4.)*(x*x-1)*(-2+r*r+6*r*x-5*r*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
			//B222
			case 21:
				return dampexpb*(-3*r*r/16.)*pow2(x*x-1)*(6-30*r*x-5*r*r+35*r*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//B312
			case 22:
				return dampexpb*(r/8.)*(4*x*(3-5*x*x)+r*(3-30*x*x+35*x*x*x*x))*P_L(k*r)*P_L(k*sqrt(d))/d;
		//B321
			case 23:
				return dampexpb*(r/8.)*(-8*x+r*(-12+36*x*x+12*r*x*(3-5*x*x)+r*r*(3-30*x*x+35*x*x*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//B322
			case 24:
				return dampexpb*(3*r/16.)*(x*x-1)*(-8*x+r*(-12+60*x*x+20*r*x*(3-7*x*x)+5*r*r*(1-14*x*x+21*x*x*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//B422 		//B422 // HAVE INCLUDED THE 'TYPO' IN 3RD TERM : 6rr -> 6rrx
			case 25:
				return dampexpb*(r/16.)*(8*x*(-3+5*x*x)-6*r*(3-30*x*x+35*x*x*x*x)+6*r*r*x*(15-70*x*x+63*x*x*x*x)+r*r*r*(5-21*x*x*(5-15*x*x+11*x*x*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
  	// C terms
			//C11
			case 26:
				return dampexpa*2.*((r*r*r)*(x*x-1)*(Dd_spt*F_spt*r*(r*x-1)-D_spt*Fd_spt*x*d)*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
			//C12
			case 27:
				return -dampexpa*D_spt*Fd_spt*((r*r*r*r)*pow2(x*x-1)*P_L(k)*P_L(k*sqrt(d)))/(Dd_spt*pow2(d));
			//C22
			case 28:
				return dampexpa*((r*r*r)*(x*x-1)*(2*Dd_spt*F_spt*r*(r*x-1)-D_spt*Fd_spt*(r+4*x+2*r*r*x-7*r*x*x))*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
			//C23
			case 29:
				return -dampexpa*D_spt*Fd_spt*((r*r*r*r)*pow2(x*x-1)*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
      //C33
			case 30:
				return dampexpa*D_spt*Fd_spt*((r*r*r)*(x*x-1)*(-2*x+r*(-1+3*x*x))*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
				//C11t
			case 31:
				return dampexpa*2*F_spt*(r*(x*x-1)*(-x+r*(-1+2*x*x))*P_L(k*r)*P_L(k*sqrt(d)))/ pow2(d);
				//C12t
			case 32:
				return -dampexpa*F_spt*(r*r*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d)))/ pow2(d);
				//C22t
			case 33:
				return dampexpa*r*(x*x-1)*(2*D_spt*Fd_spt*(-x+r*(-1+2*x*x)) + Dd_spt*F_spt*(-2*x+r*(-1+3*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/ (Dd_spt*pow2(d));
				//C23t
			case 34:
				return -dampexpa*D_spt*Fd_spt*(r*r*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
				//C33t
			case 35:
				return dampexpa*D_spt*Fd_spt*(r*(x*x-1)*(-2*x+r*(-1+3*x*x))*P_L(k*r)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
				//c11
			case 36:
				return  -dampexpa*2*r*(1-x*x)*(D_spt*Fd_spt*r*(r*x-1)-Dd_spt*F_spt*x*d)*P_L(k*r)*P_L(k)/(Dd_spt*d);
				//c12
			case 37:
				return -dampexpa*D_spt*Fd_spt*r*r*pow2(-1+x*x)*P_L(k*r)*P_L(k)/(Dd_spt*d);
				//c22
			case 38:
				return dampexpa*r*(-1+x*x)*(-2*Dd_spt*F_spt*x*d+D_spt*Fd_spt*(-2*x+2*r*r*x+3*r*(-1+x*x)))*P_L(k*r)*P_L(k)/(Dd_spt*d);
				//c33
			case 39:
				return dampexpa*D_spt*Fd_spt*r*(-1+x*x)*(-2*x+r*(-1+3*x*x))*P_L(k*r)*P_L(k)/(Dd_spt*d);
			default:
			warning("SPT: invalid indices, a = %d\n", a);
				return 0;
      }
}

/* function to select multipole or u-dependent Redshift PS */
// it gives either u^(2n) or factL(k, u(i.e.sigma_v), n, a, b, e, f) - see factL

// e = 0:  RSD PS
// e = 1 : Monopole
// e = 2 : Quadrupole
// e = 3 : Hexdecapole

// e = 3 : Hexdecapole

static real u0(real k, real u,real F0, int n, int a){
	if (a >= 1) {
		return factL(k,u,F0,1.,n,a,6); }
		else
			return pow(u,2*n);
}

static real midintabc(real bl, int a, const PowerSpectrum& P_L, real k, real u, real r) {
  real KMAX = QMAXp/k;
  real KMIN = QMINp/k;
	real YMIN = Max(XMIN, (1.+r*r-KMAX*KMAX)/2./r);
  real YMAX = ATS(k,r);
	real F0 =rempfr*fdgp_spt;
  real D1 = D_spt;
  real X1 = 1;
  real u01 = u0(k,u,F0,1,a);
  real u02 = u0(k,u,F0,2,a);
  real u03 = u0(k,u,F0,3,a);
  real u04 = u0(k,u,F0,4,a);

	//A term
		return     pow4(D1/dnorm_spt)*2.*(F0*u01*pow2(bl)*(Integrate(bind(ABC,1, cref(P_L), k, r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,5, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,10, cref(P_L), k, r, _1),  YMIN, YMAX ,  1e-3*P_L(k)))

							+ F0*F0*u01*bl*(Integrate(bind(ABC,2, cref(P_L), k,  r, _1),  YMIN, YMAX , 1e-3*P_L(k))
										   + Integrate(bind(ABC,6, cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,11, cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k)))

							+ F0*F0*u02*bl*(Integrate(bind(ABC,3, cref(P_L), k,  r, _1),   YMIN, YMAX , 1e-3*P_L(k))
										   + Integrate(bind(ABC,7, cref(P_L), k,  r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,12, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k)))

					     + F0*F0*F0*u02*(Integrate(bind(ABC,2, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,8, cref(P_L), k,  r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,11, cref(P_L), k, r, _1), YMIN, YMAX ,  1e-3*P_L(k)))

						   + F0*F0*F0*u03*(Integrate(bind(ABC,4, cref(P_L), k, r, _1),   YMIN, YMAX , 1e-3*P_L(k))
										   + Integrate(bind(ABC,9, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,13, cref(P_L), k,  r, _1),YMIN, YMAX ,  1e-3*P_L(k))))

	//B term
		  +      pow4(D1/dnorm_spt)*2*(F0*F0*pow2(bl)*u01*Integrate(bind(ABC,14,  cref(P_L), k, r, _1), YMIN, YMAX , 1e-3* P_L(k))

						  -F0*F0*F0*bl*u01*(Integrate(bind(ABC,15, cref(P_L), k, r, _1), YMIN, YMAX , 1e-3*P_L(k))
											    +Integrate(bind(ABC,16, cref(P_L),  k,  r, _1),YMIN, YMAX , 1e-3*P_L(k)))

						+F0*F0*F0*F0*u01*Integrate(bind(ABC,17, cref(P_L),  k, r, _1), YMIN, YMAX , 1e-3*P_L(k))

							  +F0*F0*pow2(bl)*u02*Integrate(bind(ABC,18,  cref(P_L), k,  r, _1),YMIN, YMAX ,  1e-3*P_L(k))

						 -F0*F0*F0*bl*u02*(Integrate(bind(ABC,19, cref(P_L), k,  r, _1), YMIN, YMAX , 1e-3*P_L(k))
										      + Integrate(bind(ABC,20, cref(P_L),  k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k)))

					  +F0*F0*F0*F0*u02*Integrate(bind(ABC,21, cref(P_L),  k,  r, _1),YMIN, YMAX , 1e-3*P_L(k))

					    - F0*F0*F0*bl*u03*(Integrate(bind(ABC,22,  cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k))
											  + Integrate(bind(ABC,23,  cref(P_L), k,  r, _1), YMIN, YMAX , 1e-3*P_L(k)))

					  + F0*F0*F0*F0*u03*Integrate(bind(ABC,24,  cref(P_L), k,  r, _1),YMIN, YMAX ,  1e-3*P_L(k))

							+F0*F0*F0*F0*u04*Integrate(bind(ABC,25,  cref(P_L), k,  r, _1), YMIN, YMAX , 1e-3*P_L(k)))

	//C - nDGP analytic

		  +       X1*D1*D1*2/pow4(dnorm_spt)*(F0*pow2(bl)*u01*(Integrate(bind(ABC,26, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,31, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,36, cref(P_L), k, r, _1), YMIN, YMAX ,  1e-3*P_L(k))) +

							F0*F0*bl*u01*(Integrate(bind(ABC,27, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,32, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,37, cref(P_L), k, r, _1),  YMIN, YMAX ,  1e-3*P_L(k))) +

						   F0*F0*bl*u02*(Integrate(bind(ABC,28, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,33, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,38, cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k))) +

						F0*F0*F0*u02*(Integrate(bind(ABC,29, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,34, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,37, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))) +

					   F0*F0*F0*u03*(Integrate(bind(ABC,30, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
									   + Integrate(bind(ABC,35, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
									   + Integrate(bind(ABC,39, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))));

}

/* Non vanishing A, B and C terms */
//numerical calculations
// Notes for numerical AB terms : DEFINITION OF THETA=  DEL V / aH (and is accounted for in definition of Cross Bispectrum)
// Integrating them over angle
static real ABCn(double u0x[], int y, real bl, int a, const PowerSpectrum& P_L, real k, real u, real r) {
  double myresult = 0.;
	double temp_abc;
  double abc[10];
  // Set the limits of angular integration
  double KMAX = QMAXp/k;
  double KMIN = QMINp/k;
  double YMIN = Max(XMIN, (1.+r*r-KMAX*KMAX)/2./r);
  double YMAX = ATS(k,r);
  // Multipole factors
  real u01 = u0x[0];//u0(k,u,fl_spt,1,a);
  real u02 = u0x[1];//u0(k,u,fl_spt,2,a);
  real u03 = u0x[2];//u0(k,u,fl_spt,3,a);
  real u04 = u0x[3];//u0(k,u,fl_spt,4,a);
  // Find those corresponding limits in the GL Quad abscissae (See SpecialFunctions.h)
  int x_min = searchnearest(x128, YMIN);
  int x_max = searchnearest(x128, YMAX);
  for( int i = x_min; i <= x_max; i++ )
    				{
    		 		double x = x128[i];
    		 		double d = 1+ r*r - 2*r*x;
    		 		if(d < 1e-5){
    			 	temp_abc=0.;
    		 		}
    		  	else {
              // 1st order kernels F1/G1(k-p)
              abc[0] = F1kmp_nk[i*n2 + y];
              abc[1] = G1kmp_nk[i*n2 + y]/bl;
              // 1st order kernels F1/G1(p)
              abc[2] = F1p_nk[i*n2 + y];
              abc[3] = G1p_nk[i*n2 + y]/bl;
              //symmetrized 2nd order kernels for ps F2/G2(p,k-p)
              abc[4] = F2_nk[i*n2 + y];
              abc[5] = G2_nk[i*n2 + y]/bl;
              //symmetrized 2nd order kernels F2/G2(-p,k)
              abc[6] = F2B_nk[i*n2 + y];
              abc[7] = G2B_nk[i*n2 + y]/bl;
              //symmetrized 2nd order kernels F2/G2(-k,k-p)
              abc[8] = F2C_nk[i*n2 + y];
              abc[9] = G2C_nk[i*n2 + y]/bl;

              double dampexpa = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*r)/2.-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))/2.-pow2(F1_nk/dnorm_spt)*damp_term(k)/2.);
              double dampexpb = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))-pow2(F1_nk/dnorm_spt)*damp_term(k*r));

// A terms
//u^2
    		  temp_abc =  pow3(bl)*dampexpa/d*(-u01*(F1_nk*r*(-2*abc[8]*abc[1]*r*(r*x-1)+abc[9]*(2*abc[0]*x*d+abc[1]*r*(1-x*x)))*P_L(k)*P_L(k*sqrt(d))
            						+ abc[4]*r*(-2*abc[2]*abc[1]*r*(-1+r*x)+abc[3]*(2*abc[0]*x*d+abc[1]*r*(1-x*x)))*P_L(k*r)*P_L(k*sqrt(d))
            						+ F1_nk*r*(-2*abc[7]*abc[2]*r*(r*x-1)+abc[3]*(2*abc[6]*x*d+abc[7]*r*(1-x*x)))*P_L(k)*P_L(k*r))
//u^4
            -u02*(r*(2*abc[8]*G1_nk/bl*abc[1]*r*(r*x-1)+abc[9]*(G1_nk/bl*(-2*abc[0]*x*d+abc[1]*r*(x*x-1))+F1_nk*abc[1]*(-2*x+r*(-1+3*x*x))))*P_L(k)*P_L(k*sqrt(d))
                           +r*(abc[4]*abc[1]*abc[3]*(-2*x+r*(-1+3*x*x))+abc[5]*(2*abc[2]*abc[1]*r*(r*x-1)+abc[3]*(-2*abc[0]*x*d+abc[1]*r*(x*x-1))))*P_L(k*r)*P_L(k*sqrt(d))
                           +r*(abc[3]*abc[7]*F1_nk*(-2*x-r+3*x*x*r)+2*abc[2]*G1_nk/bl*abc[7]*r*(r*x-1)+G1_nk/bl*abc[3]*(-2*abc[6]*x*d+abc[7]*r*(x*x-1)))*P_L(k)*P_L(k*r))
//u^6
           -u03*(G1_nk/bl*abc[1]*abc[9]*r*(r+2*x-3*r*x*x)*P_L(k)*P_L(k*sqrt(d))
                        +abc[3]*abc[1]*abc[5]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))
                        +abc[3]*G1_nk/bl*abc[7]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k)))

// B terms
          +pow4(bl)*dampexpb*P_L(k*r)*P_L(k*sqrt(d))/(16.*d*d)*
//u^2
          (u01*(abc[1]*abc[3]*r*r*(x*x-1)*(2*abc[0]*d*(4*abc[2]+3*abc[3]*(x*x-1))
                         +abc[1]*r*r*(x*x-1)*(6*abc[2]+5*abc[3]*(x*x-1))))
//u^4
          +u02*(abc[1]*abc[3]*r*(-4*abc[0]*d*(3*abc[3]*(x*x-1)*(-2*x+r*(5*x*x-1))+abc[2]*(-4*x+r*(-2+6*x*x)))
                					   -3*abc[1]*r*(x*x-1)*(4*abc[2]*(2-6*r*x+r*r*(-1+5*x*x))
                						 +abc[3]*(x*x-1)*(6-30*r*x+5*r*r*(-1+7*x*x)))))
//u^6
          +u03*(abc[1]*abc[3]*r*(3*abc[3]*abc[1]*(x*x-1)*(-8*x+12*r*(5*x*x-1)-20*r*r*x*(7*x*x-3)+5*pow3(r)*(1-14*x*x+21*pow4(x)))
                              +2*abc[0]*abc[3]*(4*x*(3-5*x*x)+pow3(r)*(3-30*x*x+35*pow4(x))+r*(3-54*x*x+75*pow4(x))+r*r*(6*x+40*pow3(x)-70*pow5(x)))
                              +2*abc[2]*abc[1]*(-8*x+12*r*(3*x*x-1)+r*r*(36*x-60*pow3(x))+pow3(r)*(3-30*x*x+35*pow4(x)))))
//u^8
          +u04*(pow2(abc[1]*abc[3])*r*(8*x*(5*x*x-3)-6*r*(3-30*x*x+35*pow4(x))
                     				+6*r*r*x*(15-70*x*x+63*pow4(x))  		// HAVE INCLUDED THE 'TYPO' IN TERM : 6rr -> 6rrx
                     				+pow3(r)*(5-105*x*x+315*pow4(x)-231*pow6(x)))));
    				}
    				myresult += w128[i] * temp_abc;
    				}
          return  2*myresult;
}

/*Generalised A and B correction term */

/*Analytical A and B*/
// a = 1 : LCDM
// a = 2 : MG

// e = 0:  RSD PS
// e = 1 : Monopole
// e = 2 : Quadrupole
// e = 3 : Hexdecapole

//U = u for RSD PS
//U = sigma_v for multipoles

real RegPT::ABr(real k, real bl, real U, int a) const{
    int n3 = 300;
    real KMAX = QMAXp/k;
    real KMIN = QMINp/k;
    double y[n3];
    double integrand[n3];
    for (int i = 0; i<n3; i++){
    y[i] = KMIN * exp(i*log(KMAX/KMIN)/(n3-1.));
    integrand[i] = midintabc(bl, a,cref(P_l), k, U, y[i]);
    }
  double res = 0.;
    for( int i = 1; i < n3; ++ i ){
  res += 0.5 * (y[i] - y[i-1])*(integrand[i] + integrand[i-1]);
      }
    return  k*k*k/(4*M_PI*M_PI) * res;
    }

/*Numerical A and B */

// e = 0:  RSD PS
// e = 1 : Monopole
// e = 2 : Quadrupole
// e = 3 : Hexdecapole

//U = u for RSD PS
//U = sigma_v for multipoles

real RegPT::ABnr(double kmin, double kmax, real bl, real k, real U, int a) const{
	real KMAX = QMAXp/k;
  real KMIN = QMINp/k;
  int y1 = num_selec_mag(kmin, kmax, KMIN);
  int y2 = num_selec_mag(kmin, kmax, KMAX);
  double y[n2];
  double integrand[n2];
  double u0x[4];
  u0x[0]=u0(k,U,fl_spt,1,a);
  u0x[1]=u0(k,U,fl_spt,2,a);
  u0x[2]=u0(k,U,fl_spt,3,a);
  u0x[3]=u0(k,U,fl_spt,4,a);
  for (int i = y1; i<=y2; i++){
  y[i] = QMINp/kmax * exp(i*log(QMAXp*kmax/(QMINp*kmin))/(n2*1.-1.));
  integrand[i] = ABCn(u0x,i, bl, a, cref(P_l), k, U, y[i]);
  }
double res = 0.;
  for( int i = y1+1; i <= y2; ++ i ){
res += 0.5 * (y[i] - y[i-1])*(integrand[i] + integrand[i-1]);
}
  return  k*k*k/(4*M_PI*M_PI)/pow4(dnorm_spt) * res;
}

// Separation of numerical A B terms for interpolation

static void ABnr_selec(double abarray[], int y, const PowerSpectrum& P_L, real k, real r) {
  double myresult[14];
  for(int ii=0;ii<14;ii++){
    myresult[ii]=0.;
  }
	double temp_abs[14];
  double abc[10];
  // Set the limits of angular integration
  double KMAX = QMAXp/k;
  double KMIN = QMINp/k;
  double YMIN = Max(XMIN, (1.+r*r-KMAX*KMAX)/2./r);
  double YMAX = ATS(k,r);
  // Find those corresponding limits in the GL Quad abscissae (See SpecialFunctions.h)
  int x_min = searchnearest(x128, YMIN);
  int x_max = searchnearest(x128, YMAX);
  for( int i = x_min; i <= x_max; i++ )
    				{
    		 		double x = x128[i];
    		 		double d = 1+ r*r - 2*r*x;
    		 		if(d < 1e-5){
              for(int ii=0;ii<14;ii++){
                temp_abs[ii]=0.;
              }
    		 		}
    		  	else {
              // 1st order kernels F1/G1(k-p)
              abc[0] = F1kmp_nk[i*n2 + y];
              abc[1] = G1kmp_nk[i*n2 + y];
              // 1st order kernels F1/G1(p)
              abc[2] = F1p_nk[i*n2 + y];
              abc[3] = G1p_nk[i*n2 + y];
              //symmetrized 2nd order kernels for ps F2/G2(p,k-p)
              abc[4] = F2_nk[i*n2 + y];
              abc[5] = G2_nk[i*n2 + y];
              //symmetrized 2nd order kernels F2/G2(-p,k)
              abc[6] = F2B_nk[i*n2 + y];
              abc[7] = G2B_nk[i*n2 + y];
              //symmetrized 2nd order kernels F2/G2(-k,k-p)
              abc[8] = F2C_nk[i*n2 + y];
              abc[9] = G2C_nk[i*n2 + y];

              double dampexpa = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*r)/2.-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))/2.-pow2(F1_nk/dnorm_spt)*damp_term(k)/2.);
              double dampexpb = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))-pow2(F1_nk/dnorm_spt)*damp_term(k*r));
              double prefacb = dampexpb*P_L(k*r)*P_L(k*sqrt(d))/(16.*d*d);
//A terms
//u^2b^2
          temp_abs[0] =  -dampexpa/d*((F1_nk*r*(-2*abc[8]*abc[1]*r*(r*x-1)+abc[9]*2*abc[0]*x*d))*P_L(k)*P_L(k*sqrt(d))
                                    + abc[4]*r*(-2*abc[2]*abc[1]*r*(-1+r*x)+abc[3]*2*abc[0]*x*d)*P_L(k*r)*P_L(k*sqrt(d))
                                    +F1_nk*r*(-2*abc[7]*abc[2]*r*(r*x-1)+abc[3]*2*abc[6]*x*d)*P_L(k)*P_L(k*r));
//u^2b
          temp_abs[1] =  -dampexpa/d*((F1_nk*r*(abc[9]*abc[1]*r*(1-x*x))*P_L(k)*P_L(k*sqrt(d))
            						            + abc[4]*r*(abc[3]*abc[1]*r*(1-x*x))*P_L(k*r)*P_L(k*sqrt(d))
            						            + F1_nk*r*(abc[3]*abc[7]*r*(1-x*x))*P_L(k)*P_L(k*r)));
//u^4 b
          temp_abs[2] = -dampexpa/d*(r*(2*abc[8]*G1_nk*abc[1]*r*(r*x-1)+abc[9]*(G1_nk*(-2*abc[0]*x*d)+F1_nk*abc[1]*(-2*x+r*(-1+3*x*x))))*P_L(k)*P_L(k*sqrt(d))
                                     +r*(abc[4]*abc[1]*abc[3]*(-2*x+r*(-1+3*x*x))+abc[5]*(2*abc[2]*abc[1]*r*(r*x-1)+abc[3]*(-2*abc[0]*x*d)))*P_L(k*r)*P_L(k*sqrt(d))
                                     +r*(abc[3]*abc[7]*F1_nk*(-2*x-r+3*x*x*r)+2*abc[2]*G1_nk*abc[7]*r*(r*x-1)+G1_nk*abc[3]*(-2*abc[6]*x*d))*P_L(k)*P_L(k*r));
// u^4
          temp_abs[3] = -dampexpa/d*(r*(abc[9]*(G1_nk*(abc[1]*r*(x*x-1))))*P_L(k)*P_L(k*sqrt(d))
                                     +r*(abc[5]*(abc[3]*(abc[1]*r*(x*x-1))))*P_L(k*r)*P_L(k*sqrt(d))
                                     +r*(G1_nk*abc[3]*(abc[7]*r*(x*x-1)))*P_L(k)*P_L(k*r));

//u^6
          temp_abs[4] = -dampexpa/d*(G1_nk*abc[1]*abc[9]*r*(r+2*x-3*r*x*x)*P_L(k)*P_L(k*sqrt(d))
                                  +abc[3]*abc[1]*abc[5]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))
                                  +abc[3]*G1_nk*abc[7]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k));

//B terms
//u^2 b^2
          temp_abs[5]= prefacb*(abc[1]*abc[3]*r*r*(x*x-1)*(2*abc[0]*d*(4*abc[2])));
//u^2 b
          temp_abs[6]= prefacb*(abc[1]*abc[3]*r*r*(x*x-1)*(2*abc[0]*d*(3*abc[3]*(x*x-1))
                         +abc[1]*r*r*(x*x-1)*(6*abc[2])));
//u^2
          temp_abs[7]= prefacb*(abc[1]*abc[3]*r*r*(x*x-1)*(abc[1]*r*r*(x*x-1)*(5*abc[3]*(x*x-1))));
//u^4b^2
          temp_abs[8]= prefacb*(abc[1]*abc[3]*r*(-4*abc[0]*d*(abc[2]*(-4*x+r*(-2+6*x*x)))));
//u^4 b
          temp_abs[9]= prefacb*(abc[1]*abc[3]*r*(-4*abc[0]*d*(3*abc[3]*(x*x-1)*(-2*x+r*(5*x*x-1)))
                					   -3*abc[1]*r*(x*x-1)*(4*abc[2]*(2-6*r*x+r*r*(-1+5*x*x)))));
//u^4
          temp_abs[10]= prefacb*(abc[1]*abc[3]*r*(-3*abc[1]*r*(x*x-1)*(abc[3]*(x*x-1)*(6-30*r*x+5*r*r*(-1+7*x*x)))));

//u^6 b
          temp_abs[11] = prefacb*(abc[1]*abc[3]*r*(2*abc[0]*abc[3]*(4*x*(3-5*x*x)+pow3(r)*(3-30*x*x+35*pow4(x))+r*(3-54*x*x+75*pow4(x))+r*r*(6*x+40*pow3(x)-70*pow5(x)))
                                +2*abc[2]*abc[1]*(-8*x+12*r*(3*x*x-1)+r*r*(36*x-60*pow3(x))+pow3(r)*(3-30*x*x+35*pow4(x)))));

//u^6
          temp_abs[12]= prefacb*(abc[1]*abc[3]*r*(3*abc[3]*abc[1]*(x*x-1)*(-8*x+12*r*(5*x*x-1)-20*r*r*x*(7*x*x-3)+5*pow3(r)*(1-14*x*x+21*pow4(x)))));

//u^8
          temp_abs[13]= prefacb*(pow2(abc[1]*abc[3])*r*(8*x*(5*x*x-3)-6*r*(3-30*x*x+35*pow4(x))
                               				+6*r*r*x*(15-70*x*x+63*pow4(x))  		// HAVE INCLUDED THE 'TYPO' IN TERM : 6rr -> 6rrx
                               				+pow3(r)*(5-105*x*x+315*pow4(x)-231*pow6(x))));

    				}
            for(int ii=0;ii<14;ii++){
              myresult[ii] += w128[i]*temp_abs[ii];
            }
    				}
            for(int ii=0;ii<14;ii++){
              abarray[ii]=2*myresult[ii];
            }
}

void RegPT::ABr_selec(double myarray[], real k) const {
  	real KMAX = QMAXp/k;
    real KMIN = QMINp/k;
    int y1 = num_selec_mag(k, k, KMIN);
    int y2 = num_selec_mag(k, k, KMAX);
    double y[n2];
    double integrand[n2][14];
    double abarray[14];
    for (int i = y1; i<=y2; i++){
    y[i] = QMINp/k * exp(i*log(QMAXp/QMINp)/(n2*1.-1.));
    ABnr_selec(abarray,i, cref(P_l), k, y[i]);
    for(int ii=0;ii<14;ii++){
    integrand[i][ii] = abarray[ii];
      }
    }
  double res[14];
  for(int ii=0;ii<14;ii++){
    res[ii]=0.;
  }
    for( int i = y1+1; i <= y2; ++ i ){
      for(int ii=0;ii<14;ii++){
        res[ii] += 0.5 * (y[i] - y[i-1])*(integrand[i][ii] + integrand[i-1][ii]);
      }
  }
  for(int ii=0;ii<14;ii++){
    myarray[ii] = k*k*k/(4*M_PI*M_PI)/pow4(dnorm_spt) * res[ii];
  }
}


/// Copyright (c) 2014-2016 Andrew Hundt, Johns Hopkins University
///
/// @author Andrew Hundt <ATHundt@gmail.com>
///
/// Dual licensed under the BSD or Apache License, Version 2.0
/// (the "Licenses") licenses at users option. Contributions shall be made such
/// that users can continue to choose between the Licenses.
/// You may not use this project except in compliance with the Licenses.
///
/// BSD license:
/// ------------
///
/// Redistribution and use in source and binary forms, with or without
/// modification, are permitted provided that the following conditions are met:
///
/// 1. Redistributions of source code must retain the above copyright notice, this
///    list of conditions and the following disclaimer.
/// 2. Redistributions in binary form must reproduce the above copyright notice,
///    this list of conditions and the following disclaimer in the documentation
///    and/or other materials provided with the distribution.
///
/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
/// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
/// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
/// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
/// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
/// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
/// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
/// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
/// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
///
/// The views and conclusions contained in the software and documentation are those
/// of the authors and should not be interpreted as representing official policies,
/// either expressed or implied, of the grl Project.
///
/// Apache v2 license:
/// ------------------
///
/// You may obtain a copy of the Apache License, Version 2.0 (the "License") at
///
///     http://www.apache.org/licenses/LICENSE-2.0
///
/// Unless required by applicable law or agreed to in writing, software
/// distributed under the License is distributed on an "AS IS" BASIS,
/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
/// See the License for the specific language governing permissions and
/// limitations under the License.

#ifndef RTREE_PLANNER_HPP_
#define RTREE_PLANNER_HPP_

#include <vector>

#include <boost/random/mersenne_twister.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <boost/pending/indirect_cmp.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/algorithm/fill.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/sign.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)

namespace plan {

// Define properties for vertex
// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Point>
struct bg_vertex_property
{
    bg_vertex_property()
    {
        boost::geometry::assign_zero(location);
    }
    bg_vertex_property(Point const& loc)
        : location(loc)
    {
    }

    Point location;
};

// Define properties for edge
// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Line>
struct bg_edge_property
{
    bg_edge_property(Line const& line)
        : length(boost::geometry::length(line))
        , m_line(line)
    {
    }

    inline Line const& line() const
    {
        return m_line;
    }

    double length;
private :
    Line m_line;
};

    //typedef bg::model::point<double, 3, bg::cs::cartesian> point;
    typedef boost::array<double,6> ArmPos;
    typedef ArmPos point_type;
    typedef bg::model::box<point_type> box;
    //typedef bg::model::referring_segment<point_type> line_type; /// @todo maybe better to use refs?
    typedef bg::model::segment<point_type> line_type;
    typedef line_type edge_type;
    //typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon

    typedef boost::adjacency_list
            <
                boost::vecS, boost::vecS, boost::undirectedS
                , bg_vertex_property<point_type> // bundled
                , bg_edge_property<line_type>
            > graph_type;

    typedef boost::graph_traits<graph_type>::vertex_descriptor vertex_descriptor_type;
    typedef bg_vertex_property<point_type>                     vertex_property_type;
    typedef bg_edge_property<edge_type>                        edge_property_type;

    typedef std::pair<point_type, vertex_descriptor_type> rtree_value;
    typedef boost::geometry::index::rtree<rtree_value,bgi::rstar<16, 4> > knn_rtree_type;

} // namespace plan

BOOST_GEOMETRY_REGISTER_LINESTRING(std::vector<plan::ArmPos>)

template<typename T>
inline T normalizeRadiansPiToMinusPi(T rad)
{
  // copy the sign of the value in radians to the value of pi
  T signedPI = boost::math::copysign(boost::math::constants::pi<T>(),rad);
  // set the value of rad to the appropriate signed value between pi and -pi
  rad = std::fmod(rad+signedPI,(boost::math::constants::two_pi<T>())) - signedPI;

  return rad;
}

// functor for getting sum of previous result and square of current element
// source: http://stackoverflow.com/questions/1326118/sum-of-square-of-each-elements-in-the-vector-using-for-each
template<typename T>
struct square
{
    T operator()(const T& Left, const T& Right) const
    {
        return (Left + Right*Right);
    }
};

namespace boost { namespace geometry {

/// distance on an n-torus, so any dimensions offset by 2pi are equal and distances wrap
/// @example comparable_distance(pi-.01,-pi+.01) == .02 radians
double comparable_distance(plan::ArmPos const& p1, plan::ArmPos const& p2 ) {
    plan::ArmPos diff;
    boost::transform(p1,p2,diff.begin(),std::minus<plan::ArmPos::value_type>());
    boost::transform(diff,diff.begin(),&normalizeRadiansPiToMinusPi<plan::ArmPos::value_type>);
    return boost::accumulate(diff,0,square<plan::ArmPos::value_type>());
}

/// distance between a a point and an axis aligned "box" on the surface of an n-torus
//  so any dimensions offset by 2pi are equal and distances wrap
template<typename Box>
double comparable_distance(plan::ArmPos const& armpos, Box const& box ){
    namespace bg = boost::geometry;
    plan::ArmPos normAP = normalizeRadiansPiToMinusPi(armpos);
    plan::ArmPos mindiff;
    boost::transform(normAP,bg::get<bg::min_corner>(box),mindiff.begin(),std::minus<plan::ArmPos::value_type>());
    boost::transform(mindiff,mindiff.begin(),&normalizeRadiansPiToMinusPi<plan::ArmPos::value_type>);
    plan::ArmPos maxdiff;
    boost::transform(normAP,bg::get<bg::max_corner>(box),maxdiff.begin(),std::minus<plan::ArmPos::value_type>());
    boost::transform(maxdiff,maxdiff.begin(),&normalizeRadiansPiToMinusPi<plan::ArmPos::value_type>);

    plan::ArmPos::value_type final_distance = 0.0;
    for(int i = 0; i < armpos.size(); ++i){
        if(mindiff[i] >= 0.0 && maxdiff[i] <= 0.0) continue; // between the min and max means "in the box" for this dimension
        plan::ArmPos::value_type min_dist = std::min(std::abs(mindiff[i]),std::abs(maxdiff[i]));
        final_distance+=min_dist*min_dist;
    }

    return final_distance;
//    diff (min<D> - p<D>), (p<D> - max<D>)
}

}} // namespace boost::geometry

///
/// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Graph, typename Route>
inline void add_edge_to_route(Graph const& graph,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex1,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex2,
            Route& route)
{
    std::pair
        <
            typename boost::graph_traits<Graph>::edge_descriptor,
            bool
        > opt_edge = boost::edge(vertex1, vertex2, graph);
    if (opt_edge.second)
    {
        // Get properties of edge and of vertex
        plan::edge_property_type const& edge_prop = graph[opt_edge.first];
        plan::bg_vertex_property
            <
                typename boost::geometry::point_type<plan::edge_type>::type
            > const& vertex_prop = graph[vertex2];

        // Depending on how edge connects to vertex, copy it forward or backward
        if (boost::geometry::equals(*bg::segment_view<plan::edge_type>(edge_prop.line()).begin(), vertex_prop.location))
        {
            std::copy(bg::segment_view<plan::edge_type>(edge_prop.line()).begin(), bg::segment_view<plan::edge_type>(edge_prop.line()).end(),
                std::back_inserter(route));
        }
        else
        {
            std::reverse_copy(bg::segment_view<plan::edge_type>(edge_prop.line()).begin(), bg::segment_view<plan::edge_type>(edge_prop.line()).end(),
                std::back_inserter(route));
        }
    }
}

/// After running dijkstra's algorithm build list of shortest path steps between start and end position.
/// @note there will be duplicate positions that should be removed.
/// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Graph, typename Route>
inline void build_route(Graph const& graph,
            std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> const& predecessors,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex1,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex2,
            Route& route)
{
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_type;
    vertex_type pred = predecessors[vertex2];

    add_edge_to_route(graph, pred, vertex2, route);
    while (pred != vertex1)
    {
        add_edge_to_route(graph, predecessors[pred], pred, route);
        pred = predecessors[pred];
    }
}

#endif


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

#include <nt2/linalg/functions/balance.hpp>

#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/colvect.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/gebak.hpp>
#include <nt2/include/functions/gebal.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  //BALANCE Scalar
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( balance_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( balance_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      return a0;
    }
  };

  //============================================================================
  //BALANCE
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( balance_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::balance_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type child0;
    typedef typename child0::value_type                                  type_t;
    typedef typename meta::as_real<type_t>::type                        rtype_t;
    typedef typename meta::as_integer<rtype_t>::type                    itype_t;
    typedef nt2::memory::container<tag::table_,  type_t, nt2::_2D>   o_semantic;
    typedef nt2::memory::container<tag::table_, rtype_t, nt2::_2D>   r_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }
  private:
    //==========================================================================
    /// INTERNAL ONLY - B = BALANCE(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval2_1(a0, a1, nt2::both_);
    }

    /// INTERNAL ONLY - B = BALANCE(A, perm_/no_perm_/both_/none_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval2_1(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    //perm_
    /// INTERNAL ONLY: 1o 2i
    BOOST_FORCEINLINE
    void eval2_1 ( A0& a0, A1& a1
                   , nt2::policy<ext::perm_>
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a, boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      BOOST_ASSERT_MSG(issquare(boost::proto::child_c<0>(a1)),
                       "matrix to balance must be square");
      nt2_la_int ilo, ihi;
      nt2::container::table<rtype_t> scale(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                       , boost::proto::value(scale)
                       , ilo, ihi, 'P'));
      boost::proto::child_c<0>(a1) = a;
    }

    //no_perm_
    /// INTERNAL ONLY: 1o 2i
    BOOST_FORCEINLINE
      void eval2_1 ( A0& a0, A1& a1
                   , nt2::policy<ext::no_perm_>
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a, boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      BOOST_ASSERT_MSG(issquare(a),
                       "matrix to balance must be square");
      nt2_la_int ilo, ihi;
      nt2::container::table<rtype_t> scale(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(gebal( boost::proto::value(a)
                         , boost::proto::value(scale)
                         , ilo, ihi, 'S'));
      boost::proto::child_c<0>(a1) = a;
    }

    //both_
    /// INTERNAL ONLY: 1o 2i
    BOOST_FORCEINLINE
      void eval2_1 ( A0& a0, A1& a1
                   , nt2::policy<ext::both_>
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a, boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      BOOST_ASSERT_MSG(issquare(a), "matrix to balance must be square");
      nt2_la_int ilo, ihi;
      nt2::container::table<rtype_t> scale(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                         , boost::proto::value(scale)
                         , ilo, ihi, 'B'));
      boost::proto::child_c<0>(a1) = a;
    }

    // none_
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
      void eval2_1 ( A0& a0, A1& a1
                   , nt2::policy<ext::none_>
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a, boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      BOOST_ASSERT_MSG(issquare(a), "matrix to balance must be square");
      boost::proto::child_c<0>(a1) = a;
    }

    //==========================================================================
    /// INTERNAL ONLY - [T, B] = BALANCE(A)
    // finds a similarity transformation T such
    // that B = T\A*T has, as nearly as possible, approximately equal
    // row and column norms and balanced matrix B
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2(a0, a1, 'B');
    }

    //==========================================================================
    /// INTERNAL ONLY - [T, B] = BALANCE(A, perm_/noperm_/both_/none_)
    // finds a similarity transformation T such
    // that B = T\A*T has, as nearly as possible, approximately equal
    // row and column norms and balanced matrix B
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , char job
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a, boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (o_semantic, t, boost::proto::child_c<0>(a1));
      BOOST_ASSERT_MSG(issquare(a),
                       "matrix to balance must be square");
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, job));
      t = nt2::eye(n, n, meta::as_<type_t>());
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(t)
                             , boost::proto::value(scale)
                             , ilo, ihi,  job, 'R'));
      boost::proto::child_c<0>(a1) = t;
      boost::proto::child_c<1>(a1) = a;
    }

    // both_
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::both_>
                   ) const
    {
      eval2_2(a0, a1, 'B');
    }

    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::perm_>
                   ) const
    {
      eval2_2(a0, a1, 'P');
    }
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::no_perm_>
                   ) const
    {
      eval2_2(a0, a1, 'S');
    }
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::none_>
                   ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a, boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a1));
      nt2_la_int n = height(a);
      BOOST_ASSERT_MSG(issquare(a), "matrix to balance must be square");
      boost::proto::child_c<0>(a1) = nt2::eye(n, n, meta::as_<type_t>());
      boost::proto::child_c<1>(a1) = a;
    }

    //==========================================================================
    /// INTERNAL ONLY - [S, P, B] = BALANCE(A)
    // finds a similarity transformation T such
    // that B = T\A*T has, as nearly as possible, approximately equal
    // row and column norms and balanced matrix B
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_2(a0, a1, 'B');
    }

    //==========================================================================
    /// INTERNAL ONLY - [S, P, B] = BALANCE(A, perm_/noperm_/both_/none_)
    // finds a similarity transformation T such
    // that B = T\A*T has, as nearly as possible, approximately equal
    // row and column norms and balanced matrix B
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1, char job) const
    {
      typedef typename boost::proto::result_of::child_c<A1&,1>::value_type child1;
      typedef typename child1::value_type                                 itype1_t;
      typedef nt2::memory::container<tag::table_, itype1_t, nt2::_2D>   i_semantic;
      NT2_AS_TERMINAL_INOUT(o_semantic, a,   boost::proto::child_c<0>(a0), boost::proto::child_c<2>(a1));
      NT2_AS_TERMINAL_OUT  (i_semantic, ips, boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (o_semantic, s,   boost::proto::child_c<0>(a1));
      BOOST_ASSERT_MSG(issquare(a),
                       "matrix to balance must be square");
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, job));
      nt2::container::table<type_t> t = nt2::eye(n, n, meta::as_<type_t>());
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(t)
                             , boost::proto::value(scale)
                             , ilo, ihi,  job, 'R'));
      extract_ips(ips, s, t);
      boost::proto::child_c<0>(a1) = s;
      boost::proto::child_c<1>(a1) = ips;
      boost::proto::child_c<2>(a1) = a;
    }

    /// INTERNAL ONLY
    template < class IPS,  class SCA, class T>
    BOOST_FORCEINLINE
    void  extract_ips(IPS& ips, SCA& sca, const T& t) const
    {
      size_t n =  height(t);
      ips.resize(of_size(n, 1));
      sca.resize(of_size(n, 1));
      for(size_t i=1; i <= n; ++i)
      {
        for(size_t j=1; j <= n; ++j)
        {
          if(is_nez(t(i, j)))
          {
            ips(i) = j;
            sca(i) = real(t(i, j));
            break;
          }
        }
      }
    }

    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::both_>
                   ) const
    {
      eval3_2(a0, a1, 'B');
    }

    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::perm_>
                   ) const
    {
      eval3_2(a0, a1, 'P');
    }
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::no_perm_>
                   ) const
    {
      eval3_2(a0, a1, 'S');
    }
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                   , nt2::policy<ext::none_>
                   ) const
    {
      BOOST_ASSERT_MSG(issquare(boost::proto::child_c<0>(a0)),
                       "matrix to balance must be square");
      boost::proto::child_c<2>(a1) = boost::proto::child_c<0>(a0);
      itype_t n = height(boost::proto::child_c<0>(a0));
      boost::proto::child_c<0>(a1) = nt2::ones(n, 1, meta::as_<type_t>());
      boost::proto::child_c<1>(a1) = nt2::colvect(nt2::_(itype_t(1), n));
    }

  };
} }

#endif


// This file is part of the pyMOR project (http://www.pymor.org).
// Copyright 2013-2018 pyMOR developers and contributors. All rights reserved.
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>

#include <pybind11/functional.h>
#include <pybind11/numpy.h>
#include <pybind11/operators.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

template <typename Number>
void bind_vector(pybind11::module& module) {
  typedef dealii::Vector<Number> Vector;
  typedef typename Vector::size_type size_type;
  py::class_<Vector>(module, "Vector")
      .def(py::init<>())
      .def(py::init<const Vector&>())
      .def(py::init<const size_type>())
      .def("swap", &Vector::swap)
      .def(py::self == py::self)
      .def(py::self != py::self)
      .def(py::self * py::self)
      .def(py::self += py::self)
      .def(py::self -= py::self)
      .def(py::self *= Number())
      .def(py::self /= Number())
      .def("axpy", [](Vector& self, Number a, const Vector& x) { self.add(a, x); })
      .def("norm_sqr", &Vector::norm_sqr)
      .def("mean_value", &Vector::mean_value)
      .def("norm_sqr", &Vector::norm_sqr)
      .def("l1_norm", &Vector::l1_norm)
      .def("l2_norm", &Vector::l2_norm)
      .def("lp_norm", &Vector::lp_norm, py::arg("p"))
      .def("linfty_norm", &Vector::linfty_norm)
      .def("all_zero", &Vector::all_zero)
      .def("norm_sqr", &Vector::norm_sqr)
      .def("size", &Vector::size)
      .def("__getitem__",
           [](const Vector& s, size_type i) {
             if (i >= s.size())
               throw py::index_error();
             return s[i];
           })
      .def("__setitem__",
           [](Vector& s, size_type i, Number v) {
             if (i >= s.size())
               throw py::index_error();
             s[i] = v;
           })
      /// Slicing protocol (optional)
      .def("__getitem__",
           [](const Vector& s, py::slice slice) -> Vector* {
             std::size_t start, stop, step, slicelength;
             if (!slice.compute(s.size(), &start, &stop, &step, &slicelength))
               throw py::error_already_set();
             Vector* seq = new Vector(slicelength);
             for (int i = 0; i < slicelength; ++i) {
               (*seq)[i] = s[start];
               start += step;
             }
             return seq;
           })
      .def("__setitem__",
           [](Vector& s, py::slice slice, const Vector& value) {
             std::size_t start, stop, step, slicelength;
             if (!slice.compute(s.size(), &start, &stop, &step, &slicelength))
               throw py::error_already_set();
             if ((size_t)slicelength != value.size())
               throw std::runtime_error("Left and right hand size of slice assignment have different sizes!");
             for (int i = 0; i < slicelength; ++i) {
               s[start] = value[i];
               start += step;
             }
           })
      .def("__setitem__",
           [](Vector& s, py::slice slice, py::array_t<Number> value) {
             std::size_t start, stop, step, slicelength;
             py::buffer_info info = value.request();
             if (!slice.compute(s.size(), &start, &stop, &step, &slicelength))
               throw py::error_already_set();
             if (slicelength != info.size) {
               std::stringstream ss;
               ss << "Left and right hand size of slice assignment have different sizes!";
               ss << slicelength << " vs. " << info.ndim;
               throw std::runtime_error(ss.str());
             }
             for (int i = 0; i < slicelength; ++i) {
               s[start] = *(static_cast<Number*>(info.ptr) + i);
               start += step;
             }
           })
      /// Provide buffer access
      .def_buffer([](Vector& m) -> py::buffer_info {
        return py::buffer_info(&m[0],                                /* Pointer to buffer */
                               sizeof(Number),                       /* Size of one scalar */
                               py::format_descriptor<Number>::value, /* Python struct-style format descriptor */
                               1,                                    /* Number of dimensions */
                               {
                                   m.size(),
                               }, /* Buffer dimensions */
                               {sizeof(Number)});
      })
      .def("__len__", &Vector::size)
      .def("__repr__", [](const Vector& a) {
        std::stringstream ss;
        ss << "<dealii.Vector<Number> with size '" << a.size() << "'>";
        return ss.str();
      });
}

template <typename Number>
void bind_sparse_matrix(pybind11::module& module) {
  typedef dealii::SparseMatrix<Number> Matrix;
  typedef dealii::Vector<Number> Vector;

  auto cg_solve = [](Matrix& self, Vector& solution, const Vector& rhs) {
    dealii::SolverControl solver_control(20000, 1e-12);
    dealii::SolverCG<> solver(solver_control);
    dealii::PreconditionSSOR<> preconditioner;
    preconditioner.initialize(self, 1.2);
    solver.solve(self, solution, rhs, preconditioner);

    // We have made one addition, though: since we suppress output from the
    // linear solvers, we have to print the number of iterations by hand.
    std::cout << "   " << solver_control.last_step() << " CG iterations needed to obtain convergence." << std::endl;
  };

  py::class_<Matrix>(module, "SparseMatrix")
      .def(py::init<>())
      .def(py::init<const dealii::SparsityPattern&>())
      .def(py::self *= Number())
      .def("n", [](const Matrix& mat) { return 0 ? mat.empty() : mat.n(); })
      .def("m", [](const Matrix& mat) { return 0 ? mat.empty() : mat.m(); })
      .def("clear", &Matrix::clear)
      .def("l1_norm", &Matrix::l1_norm)
      .def("linfty_norm", &Matrix::linfty_norm)
      .def("vmult", &Matrix::template vmult<Vector, Vector>)
      .def("Tvmult", &Matrix::template Tvmult<Vector, Vector>)
      .def("get_sparsity_pattern", &Matrix::get_sparsity_pattern, py::return_value_policy::reference)
      .def("add", (void (Matrix::*)(Number, const Matrix&)) & Matrix::template add<Number>)
      .def("copy_from", (Matrix & (Matrix::*)(const Matrix&)) & Matrix::template copy_from<Number>)
      .def("cg_solve", cg_solve);
}

PYBIND11_PLUGIN(pymor_dealii_bindings) {
  py::module m("pymor_dealii_bindings", "Python bindings for deal.II");
  py::class_<dealii::SparsityPattern>(m, "SparsityPattern");
  bind_vector<double>(m);
  bind_sparse_matrix<double>(m);
  return m.ptr();
}


////////////////////////////////////////////////////////////////
// Orkid Media Engine
// Copyright 1996-2020, Michael T. Mayers.
// Distributed under the Boost Software License - Version 1.0 - August 17, 2003
// see http://www.boost.org/LICENSE_1_0.txt
////////////////////////////////////////////////////////////////

#include <ork/lev2/config.h>
#if defined(ENABLE_IGL)

#include <ork/kernel/orklut.hpp>
#include <ork/math/plane.h>
#include <ork/lev2/gfx/meshutil/submesh.h>
#include <ork/lev2/gfx/meshutil/igl.h>
#include <iostream>

#include <igl/collapse_edge.h>
#include <igl/doublearea.h>
#include <igl/edge_flaps.h>
#include <igl/is_edge_manifold.h>
#include <igl/is_irregular_vertex.h>
#include <igl/is_vertex_manifold.h>
#include <igl/PI.h>
#include <igl/per_vertex_normals.h>
#include <igl/per_face_normals.h>
#include <igl/per_corner_normals.h>
#include <igl/polygon_mesh_to_triangle_mesh.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/unique_edge_map.h>
#include <igl/avg_edge_length.h>

//#include <igl/arap.h>
//#include <igl/boundary_facets.h>
//#include <igl/circulation.h>
//#include <igl/decimate.h>
//#include <igl/exterior_edges.h>
//#include <igl/orientable_patches.h>
//#include <igl/randperm.h>
//#include <igl/slice.h>
//#include <igl/copyleft/cgal/mesh_boolean.h>
//#include <igl/MeshBooleanType.h>

#include <Eigen/Core>

namespace ork::meshutil {
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t submesh::toIglMesh(int numsides) const {
  return std::make_shared<IglMesh>(*this, numsides);
}
//////////////////////////////////////////////////////////////////////////////
IglMesh::IglMesh(const Eigen::MatrixXd& verts, const Eigen::MatrixXi& faces)
    : _verts(verts)
    , _faces(faces) {
  OrkAssert(_verts.cols() == 3); // make sure we have vec3's
}
//////////////////////////////////////////////////////////////////////////////
IglMesh::IglMesh(const submesh& inp_submesh, int numsides)
    : _verts(inp_submesh._vtxpool.GetNumVertices(), 3)
    , _faces(inp_submesh.GetNumPolys(numsides), numsides) {
  size_t numverts = inp_submesh._vtxpool.GetNumVertices();
  size_t numfaces = inp_submesh.GetNumPolys(numsides);
  _verts          = Eigen::MatrixXd(numverts, 3);
  _normals        = Eigen::MatrixXd(numverts, 3);
  _binormals      = Eigen::MatrixXd(numverts, 3);
  _tangents       = Eigen::MatrixXd(numverts, 3);
  _uvs            = Eigen::MatrixXd(numverts, 2);
  _colors         = Eigen::MatrixXd(numverts, 4);
  _faces          = Eigen::MatrixXi(numfaces, numsides);
  ///////////////////////////////////////////////
  // fill in vertices
  ///////////////////////////////////////////////
  for (int v = 0; v < numverts; v++) {
    const auto& inpvtx = inp_submesh._vtxpool.GetVertex(v);
    const auto& inpuv  = inpvtx.mUV[0];
    _verts.row(v) << inpvtx.mPos.x, inpvtx.mPos.y, inpvtx.mPos.z;
    _normals.row(v) << inpvtx.mNrm.x, inpvtx.mNrm.y, inpvtx.mNrm.z;
    _binormals.row(v) << inpuv.mMapBiNormal.x, inpuv.mMapBiNormal.y, inpuv.mMapBiNormal.z;
    _tangents.row(v) << inpuv.mMapTangent.x, inpuv.mMapTangent.y, inpuv.mMapTangent.z;
    _uvs.row(v) << inpuv.mMapTexCoord.x, inpuv.mMapTexCoord.y;
    _colors.row(v) << inpvtx.mCol[0].x, inpvtx.mCol[0].y, inpvtx.mCol[0].z, inpvtx.mCol[0].w;
  }
  ///////////////////////////////////////////////
  // fill in faces
  ///////////////////////////////////////////////
  orkvector<int> face_indices;
  inp_submesh.FindNSidedPolys(face_indices, numsides);
  for (int f = 0; f < face_indices.size(); f++) {
    const auto& face = inp_submesh.RefPoly(f);
    switch (numsides) {
      case 3:
        _faces.row(f) <<         //
            face.GetVertexID(0), // tri index 0
            face.GetVertexID(1), // tri index 1
            face.GetVertexID(2); // tri index 2
        break;
      case 4:
        _faces.row(f) <<         //
            face.GetVertexID(0), // tri index 0
            face.GetVertexID(1), // tri index 1
            face.GetVertexID(2), // tri index 2
            face.GetVertexID(3); // tri index 3
        break;
      default:
        OrkAssert(false);
        break;
    }
  }
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::numFaces() const {
  return _faces.rows();
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::numVertices() const {
  return _verts.rows();
}
//////////////////////////////////////////////////////////////////////////////
unique_edges_ptr_t IglMesh::uniqueEdges() const {
  auto rval = std::make_shared<UniqueEdges>();
  igl::unique_edge_map(_faces, rval->E, rval->uE, rval->EMAP, rval->_ue2e);
  rval->_count = rval->uE.rows();
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::numEdges() const {
  return uniqueEdges()->_count;
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::sidesPerFace() const {
  return _faces.cols();
}
//////////////////////////////////////////////////////////////////////////////
bool IglMesh::isVertexManifold() const {
  Eigen::MatrixXi B;
  return igl::is_vertex_manifold(_faces, B);
}
//////////////////////////////////////////////////////////////////////////////
bool IglMesh::isEdgeManifold() const {
  return igl::is_edge_manifold(_faces);
}
//////////////////////////////////////////////////////////////////////////////
double IglMesh::averageEdgeLength() const {
  return igl::avg_edge_length(_verts, _faces);
}
//////////////////////////////////////////////////////////////////////////////
fvec4 IglMesh::computeAreaStatistics() const {
  Eigen::VectorXd area;
  igl::doublearea(_verts, _faces, area);
  area              = area.array() / 2;
  double area_avg   = area.mean();
  double area_min   = area.minCoeff() / area_avg;
  double area_max   = area.maxCoeff() / area_avg;
  double area_sigma = sqrt(((area.array() - area_avg) / area_avg).square().mean());
  return fvec4(area_min, area_max, area_avg, area_sigma);
}
//////////////////////////////////////////////////////////////////////////////
fvec4 IglMesh::computeAngleStatistics() const {
  Eigen::MatrixXd angles;
  igl::internal_angles(_verts, _faces, angles);
  angles             = 360.0 * (angles / (2 * igl::PI)); // Convert to degrees
  double angle_avg   = angles.mean();
  double angle_min   = angles.minCoeff();
  double angle_max   = angles.maxCoeff();
  double angle_sigma = sqrt((angles.array() - angle_avg).square().mean());
  return fvec4(angle_min, angle_max, angle_avg, angle_sigma);
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::countIrregularVertices() const {
  // Count the number of irregular vertices, the border is ignored
  auto irregular                = igl::is_irregular_vertex(_verts, _faces);
  size_t vertex_count           = _verts.rows();
  size_t irregular_vertex_count = std::count(
      irregular.begin(), //
      irregular.end(),
      true);
  return irregular_vertex_count;
}
//////////////////////////////////////////////////////////////////////////////
size_t IglMesh::genus() const {

  // an orientable 2-manifold mesh M with g“handles” (i.e., genus)
  // has Euler-Poincaré characteristic χ(M) = V-E+F = 2(1-g)

  // 2(1-g)=v-e+f     : 2-2g == Euler-Poincaré characteristic
  //-2g=v-e+f-2
  // 2g=-v+e-f+2
  // 2g=2+e-f-v
  // g = (2+e-f-v)/2
  const size_t this_genus = (2 + numEdges() - numFaces() - numVertices()) / 2;
  return this_genus;
}
//////////////////////////////////////////////////////////////////////////////
submesh_ptr_t IglMesh::toSubMesh() const {
  auto subm           = std::make_shared<submesh>();
  size_t numFaces     = this->numFaces();
  size_t sidesPerFace = this->sidesPerFace();
  OrkAssert(sidesPerFace == 3 or sidesPerFace == 4);
  /////////////////////////////////////////////
  std::vector<vertex> submeshverts;
  submeshverts.resize(numFaces * sidesPerFace);
  /////////////////////////////////////////////
  OrkAssert(_verts.cols() == 3); // make sure we have vec3's
  size_t numVerts     = _verts.rows();
  size_t numNormals   = _normals.rows();
  size_t numBinormals = _binormals.rows();
  size_t numTangents  = _tangents.rows();
  size_t numUvs       = _uvs.rows();
  size_t numColors    = _colors.rows();
  auto generateVertex = [&](int faceindex, int facevtxindex) -> vertex { //
    vertex outv;
    const Eigen::MatrixXi& face = _faces.row(faceindex);
    int per_vert_index          = face(facevtxindex);
    /////////////////////////////////////////////
    // position
    /////////////////////////////////////////////
    auto inp_pos = _verts.row(per_vert_index);
    outv.mPos    = fvec3(inp_pos(0), inp_pos(1), inp_pos(2));
    /////////////////////////////////////////////
    // normal
    /////////////////////////////////////////////
    auto donormal = [&](int index) {
      OrkAssert(_normals.cols() == 3);
      auto inp  = _normals.row(index);
      outv.mNrm = fvec3(inp(0), inp(1), inp(2));
    };
    if (numNormals == numVerts) // per vertex
      donormal(per_vert_index);
    else if (numNormals == numFaces) // per face
      donormal(faceindex);
    else if (numNormals == 0) {
    } // no normals
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // binormal
    /////////////////////////////////////////////
    auto dobinormal = [&](int index) {
      OrkAssert(_binormals.cols() == 3);
      auto inp                 = _binormals.row(index);
      outv.mUV[0].mMapBiNormal = fvec3(inp(0), inp(1), inp(2));
    };
    if (numBinormals == numVerts) // per vertex
      dobinormal(per_vert_index);
    else if (numBinormals == numFaces) // per face
      dobinormal(faceindex);
    else if (numBinormals == 0) {
    } // no binormals
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // tangent
    /////////////////////////////////////////////
    auto dotangent = [&](int index) {
      OrkAssert(_tangents.cols() == 3);
      auto inp                = _tangents.row(index);
      outv.mUV[0].mMapTangent = fvec3(inp(0), inp(1), inp(2));
    };
    if (numTangents == numVerts) // per vertex
      dotangent(per_vert_index);
    else if (numTangents == numFaces) // per face
      dotangent(faceindex);
    else if (numTangents == 0) {
    } // no tangents
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // texturecoord
    /////////////////////////////////////////////
    auto dotexcoord = [&](int index) {
      OrkAssert(_uvs.cols() == 2);
      auto inp                 = _uvs.row(index);
      outv.mUV[0].mMapTexCoord = fvec2(inp(0), inp(1));
    };
    if (numUvs == numVerts) // per vertex
      dotexcoord(per_vert_index);
    else if (numUvs == numFaces) // per face
      dotexcoord(faceindex);
    else if (numUvs == 0) {
    } // no texcoords
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    // color
    /////////////////////////////////////////////
    auto docolor = [&](int index) {
      auto inp = _colors.row(index);
      switch (_colors.cols()) {
        case 1: // luminance
          outv.mCol[0] = fvec4(inp(0), inp(0), inp(0), 1);
          break;
        case 3: // rgb
          outv.mCol[0] = fvec4(inp(0), inp(1), inp(2), 1);
          break;
        case 4: // rgba
          outv.mCol[0] = fvec4(inp(0), inp(1), inp(2), inp(3));
          break;
        default:
          OrkAssert(false);
          break;
      }
    };
    if (numColors == numVerts)
      docolor(per_vert_index);
    else if (numColors == numFaces)
      docolor(faceindex);
    else if (numColors == 1)
      docolor(0);
    else if (numColors == 0)
      outv.mCol[0] = fvec4(1, 1, 1, 1);
    else
      OrkAssert(false);
    /////////////////////////////////////////////
    return outv;
  }; // auto generateVertex = [&](int faceindex, int facevtxindex) -> vertex { //
  /////////////////////////////////////////////
  for (int f = 0; f < numFaces; f++) {
    switch (sidesPerFace) {
      case 3: {
        auto o0 = subm->newMergeVertex(generateVertex(f, 0));
        auto o1 = subm->newMergeVertex(generateVertex(f, 1));
        auto o2 = subm->newMergeVertex(generateVertex(f, 2));
        subm->MergePoly(poly(o0, o1, o2));
        break;
      }
      case 4: {
        auto o0 = subm->newMergeVertex(generateVertex(f, 0));
        auto o1 = subm->newMergeVertex(generateVertex(f, 1));
        auto o2 = subm->newMergeVertex(generateVertex(f, 2));
        auto o3 = subm->newMergeVertex(generateVertex(f, 3));
        subm->MergePoly(poly(o0, o1, o2, o3));
        break;
      }
      default:
        OrkAssert(false);
        break;
    }
  }
  return subm;
}
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t IglMesh::triangulated() const {
  auto rval = std::make_shared<IglMesh>();
  igl::polygon_mesh_to_triangle_mesh(_faces, rval->_faces);
  rval->_verts = _verts;
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t IglMesh::decimated(float amount) const {
  auto rval = std::make_shared<IglMesh>();
  Eigen::MatrixXd V;
  Eigen::MatrixXi F;
  Eigen::MatrixXd OV = _verts;
  Eigen::MatrixXi OF = _faces;
  // Prepare array-based edge data structures and priority queue
  Eigen::VectorXi EMAP;
  Eigen::MatrixXi E, EF, EI;
  typedef std::set<std::pair<double, int>> PriorityQueue;
  PriorityQueue Q;
  std::vector<PriorityQueue::iterator> Qit;
  // If an edge were collapsed, we'd collapse it to these points:
  Eigen::MatrixXd C;
  int num_collapsed;
  { // prep
    F = OF;
    V = OV;
    igl::edge_flaps(F, E, EMAP, EF, EI);
    Qit.resize(E.rows());

    C.resize(E.rows(), V.cols());
    Eigen::VectorXd costs(E.rows());
    Q.clear();
    for (int e = 0; e < E.rows(); e++) {
      double cost = e;
      Eigen::RowVectorXd p(1, 3);
      igl::shortest_edge_and_midpoint(e, V, F, E, EMAP, EF, EI, cost, p);
      C.row(e) = p;
      Qit[e]   = Q.insert(std::pair<double, int>(cost, e)).first;
    }
    num_collapsed = 0;
  }
  { // decimate
    while (not Q.empty()) {
      bool something_collapsed = false;
      // collapse edge
      const int max_iter = std::ceil(amount * Q.size());
      for (int j = 0; j < max_iter; j++) {
        if (not igl::collapse_edge(
                igl::shortest_edge_and_midpoint, //
                V,
                F,
                E,
                EMAP,
                EF,
                EI,
                Q,
                Qit,
                C)) {
          break;
        }
        something_collapsed = true;
        num_collapsed++;
      }
    }
  }
  rval->_verts = V;
  rval->_faces = F;
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd IglMesh::computeFaceNormals() const {
  Eigen::MatrixXd rval;
  igl::per_face_normals(_verts, _faces, rval);
  return rval;
}
Eigen::MatrixXd IglMesh::computeVertexNormals() const {
  Eigen::MatrixXd rval;
  igl::per_vertex_normals(_verts, _faces, rval);
  return rval;
}
Eigen::MatrixXd IglMesh::computeCornerNormals(float dihedral_angle) const {
  Eigen::MatrixXd rval;
  igl::per_corner_normals(_verts, _faces, dihedral_angle, rval);
  return rval;
}

//////////////////////////////////////////////////////////////////////////////
void submesh::igl_test() {
  auto trimesh = submesh();
  submeshTriangulate(*this, trimesh);
}
//////////////////////////////////////////////////////////////////////////////
} // namespace ork::meshutil
#endif