/*
 *  The algorithm of R.E. Smith et al, [The Virgo Consortium Collaboration],
  ``Stable clustering, the halo model and nonlinear cosmological power spectra,''
  Mon. Not. Roy. Astron. Soc.  {\bf 341}, 1311 (2003)
  [arXiv:astro-ph/0207664]. 
  
	 Modified by Mark Wyman, Perimeter Institute for Theoretical Physics,  5 March 2009. 
	 
	 If used, please cite J. Khoury and M. Wyman, arXiv:0903.1292
	 
	 Accepts a power spectrum file named "Pklinear.txt" with the wavenumber, k, in the first column 
	 and the linear power spectrum, P(k) = k^3 Delta^2(k)/2 pi^2, in the second column.
	 
	 It outputs a file named "PknonlinearDGP.txt" with three columns: 
	 
	 the wavenumber, the linear power spectrum, and the nonlinearly corrected power spectrum.
	 
	 In its present form, the code requires number of lines in the linear power spectrum to be 
	 hand coded in as Nmodes. Note that the iterative bisection necessary to locate 
	 the nonlinear scale requires a goodly number of subdivisions in wavenumber, k, to converge.
	 So don't be shy about asking CAMB or your favorite linear P(k) generator to give you a 
	 power spectrum densely sampled in wavenumber. 
	
	 This version of the code has the parameters for growth due to the standard branch in the 
	 braneworld model of Dvali-Gabadadze-Porrati (alpha=0.5 in our paper's parameterization)
	  *
 *  
 *
 */

#include <math.h>
#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <vector>

//#include <malloc.h>
#include <stdio.h>
#include <assert.h>

//#include "smith2.h"


#define pi     3.14159265358979323846
#define pi_sqr 9.86960440108935861883
#define twopi  6.28318530717958647693
#define ln2    0.69314718
#define arcmin 2.90888208665721580e-4

#define epsilon  1e-5
#define epsilon2 1e-18

#define DSQR(a) ( (darg=(a))==0.0 ? 0.0 : darg*darg )



using namespace std;

const int Nmodes = 4328;
double dlog(double x);
double integrate(vector<double> F, vector<double> k);
void NL(vector<double> P, vector<double> k, vector<double> & PNL);
void rk4(double & delnow, double & Dnow, double k, double N, double h);
void halofit(double rk, double rn, double rncur, double rknl, double plin,double om_m, double om_v, double & pnl);


int main()
{
	ifstream p_in;
	ofstream p_out;
	p_in.open("Pklinear.txt");
	p_out.open("PknonlinearDGP.txt");
	
  vector<double> Pk(Nmodes), delta(Nmodes), k(Nmodes),PNL(Nmodes),DL(Nmodes);  

  for(int i=0;i<Nmodes;i++)
	{
		p_in >> k[i] >> Pk[i];
		Pk[i] = Pk[i];
		Pk[i]  = Pk[i];	
		delta[i] = sqrt(k[i]*k[i]*k[i]*Pk[i]/(2.*3.1415*3.1415));
	}
  
	   for(int i=0;i<Nmodes;i++)
	{
		Pk[i] = pow(delta[i],2.0)*(2.*3.1415*3.1415)/(k[i]*k[i]*k[i]);
		DL[i] = pow(delta[i],2.0);
	}
	
	NL(DL,k,PNL);
	
	for(int i=0;i<Nmodes;i++)
	{
		PNL[i] = PNL[i]/(k[i]*k[i]*k[i]/(2.*3.1415*3.1415));
		p_out << k[i] << " " << Pk[i] << " " << PNL[i] << endl;
	}
	 
  
}

void NL(vector<double> P, vector<double> k, vector<double> & PNL)
{

   const double kNLstern = 1.e6;       /* h/Mpc */
	vector<double> F(Nmodes);
   static double logkmin = 0., logkmax = 0., dk = 0., da = 0.;
   double Delta_NL, Delta_L, k_L, lnk_NL;
	static double upper;

   double	omm, omv, amp,pl,pnonl;
   double logr1, logr2 ,diff, rmid, sig, d1, d2;
   double	rknl, rneff, rncur;
      
   double aa, klog, val, logrmidtmp, logrmid, logr1start, logr2start, ksqr;
   int i,j, iter, golinear;
   const double logstep = 5.0;
   const int itermax    = 20;

   /* find non-linear scale with iterative bisection */
	    logr1 = -2.0;
	    logr2 =  3.5;
		
		iterstart:


	    logr1start = logr1;
	    logr2start = logr2;

	    iter = 0;
	    do
		 {
			logrmid = (logr2+logr1)/2.0;
	       rmid    = pow(10,logrmid);
			 for(i=0;i<Nmodes;i++)
				{
					ksqr = pow(k[i]*rmid,2.0);
					F[i] = P[i]*exp(-ksqr);
				}
					
			sig = integrate(F,k);
			diff = sig - 1.0;
			cout << rmid << " " << 1./rmid << " " << sig << endl;

	       if(diff>0.001)
		 logr1 = dlog(rmid);
	       if(diff<-0.001)
		 logr2 = dlog(rmid);

	    }while (fabs(diff)>=0.001 && ++iter<itermax);

	    if (iter>=itermax) {
	       logrmidtmp = (logr2start+logr1start)/2.0;

	       if (logrmid<logrmidtmp) {
		  logr1 = logr1start-logstep;
		  logr2 = logrmid;
	       } else if (logrmid>=logrmidtmp) {
		  logr1 = logrmid;
		  logr2 = logr2start+logstep;
	       }

	       if (1/pow(10, logr2)>kNLstern) {
				cout << " Trouble in dodge! " << endl;
		  golinear = 1;
			} else {
		  goto iterstart;
	       }
	    }
		
		 
		  // find rneff and rncur 
		for(i=0;i<Nmodes;i++)
				{
					ksqr = pow(k[i]*rmid,2.0);
					F[i] = 2.0*ksqr*P[i]*exp(-ksqr);
				}
				
			d1 = -integrate(F,k);			
			rneff = -3 - d1;
			
		for(i=0;i<Nmodes;i++)
				{
					ksqr = pow(k[i]*rmid,2.0);
					F[i] = 4.0*ksqr*(1.0-ksqr)*P[i]*exp(-ksqr);
				}
					
			rncur =  d1*d1 + integrate(F,k);
	
		cout << rneff << " and a two " << rncur << endl;


		for(i=0;i<Nmodes;i++)
			{
			pl = P[i];
			k_L = k[i];
			halofit(k_L,rneff,rncur,1.0/rmid,pl,0.3,0.7,pnonl);
			PNL[i] = pnonl;
			}
			

}

double dlog(double x)
{
   return log(x)/log(10.0);
}

double integrate(vector<double> F, vector<double> k)
{

	double sum=0, dlk,dk;
	for(int i=0; i<Nmodes; i++)
		{
		dlk = (k[i] - k[i-1])/(0.5*k[i]+0.5*k[i-1]);
		sum += F[i]*dlk + 0.5*(F[i]-F[i-1])*dlk;
		}
		
	return sum;
}


void halofit(double rk, double rn, double rncur, double rknl, double plin, double om_m, double om_v, double & pnl)
{
   double gam,a,b,c,xmu,xnu,alpha2,beta,f1,f2,f3;
   double y, ysqr;
   double f1a,f2a,f3a,f1b,f2b,f3b,frac,pq,ph;
   double nsqr;

   nsqr = rn*rn;
   gam = 0.86485 + 0.2989*rn + 0.1631*rncur;
   a = 0.84*(1.4861 + 1.83693*rn + 1.67618*nsqr + 0.7940*rn*nsqr
     + 0.1670756*nsqr*nsqr - 0.620695*rncur);
   a = pow(10,a);
   b = 1.1*pow(10,(0.9463+0.9466*rn+0.3084*nsqr-0.940*rncur));
   c = 1.05*pow(10,(-0.2807+0.6669*rn+0.3214*nsqr-0.0793*rncur));
   xmu = 0.95*pow(10,(-3.54419+0.19086*rn));
   xnu = 0.95*pow(10,(0.95897+1.2857*rn));
   alpha2 = 0.8*(1.38848+0.3701*rn-0.1452*nsqr);
   beta = 1.9*(0.8291+0.9854*rn+0.3400*nsqr);

   if(fabs(1-om_m)>0.01) {
      f1a = pow(om_m,(-0.0732));
      f2a = pow(om_m,(-0.1423));
      f3a = pow(om_m,(0.0725));
      f1b = pow(om_m,(-0.0307));
      f2b = pow(om_m,(-0.0585));
      f3b = pow(om_m,(0.0743));
  
      frac = om_v/(1.-om_m);  
      f1 = frac*f1b + (1-frac)*f1a;
      f2 = frac*f2b + (1-frac)*f2a;
      f3 = frac*f3b + (1-frac)*f3a;
   } else {      /* EdS Universe */
      f1 = f2 = f3 = 1.0;
   }

   y = 1.05*rk/rknl;
   ysqr = y*y;
   ph = a*pow(y,f1*3)/(1+b*pow(y,f2)+pow(f3*c*y,3-gam));
   ph = ph/(1+xmu/y+xnu/ysqr);
   pq = plin*pow(1+plin,beta)/(1+plin*alpha2)*exp(-y/4.0-ysqr/8.0);
   
	pnl = pq + ph;

//   assert(finite(*pnl));
}