/*
 *
	computeSigmaV.cpp

	by Mark C. Wyman, University of Chicago

	v1, released March 8, 2009.
	vCurrent, last modified 13 March 2012

	Computes the power spectrum at z=0 from the power spectra at z=49.9 and z=50.0
	in the modified gravity model under study; these were generated using CAMB

	Requires two text files containing your theory's linear power spectra at
	those two redshifts.  Requires you to hard code in the rc and alpha values
	of the theory you wish to model. 

	The nonlinear correction applied is that for
	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].

	The version of the halofitting algorithm which we began with is that of Martin Kilbinger,
	http://www.astro.uni-bonn.de/~kilbinge/cosmology/smith2/readme ;

	If used, please cite J. Khoury and M. Wyman, arXiv:0903.1292

	It outputs a file named "Pknow.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.


	**********************************
	Notes on current version:
	as compared with the public version, the current version also computes gaussian averaged peculiar velocity flows
	as discussed in Wyman and Khoury, arXiv:1004.2046 

	Perhaps more usefully, it also calculates sigma_8 and outputs it.

	To return to GR, you should make rc very large and set Dim=4.


 */

#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 double PGROW= -0.20;
const double PSHRNK = -0.25;
const double FCOR = 0.0666666666667;
const double SAFETY = 0.9;
const double ERRCON = 6.0e-4;
const double hmax = 0.2;
const double TINY = 1.0e-30;

// MODIFIED GRAVITY MODEL PARAMETERS
const double rc =
1089;
const double alpha =
0.0;
const double Dim =
5;

const double H0 =1/2998.; // Hubble/c in Mpc^-1
const double h = 0.73;

const double OmR = 0.000;
const double OmM = 0.24;
//const double OmL = 1- OmM - OmR

const int Nmodes = 130;
//const int Nmodes = 4345;

double Hubble(double a);
double g(double a);
double galt(double a);
double f(double a, double k);
double beta(double a);
double betaalt(double a);
double growth(double a);
double dlog(double x);
double eq1(double a, double k, double capD, double del);
double eq1sg(double a, double k, double capD, double del);
double integrate(vector<double> F, vector<double> k);
void NL(vector<double> P, vector<double> k, vector<double> & PNL);
void rk4(double delin, double Din, double & delout, double & Dout, double k, double N, double h);
void rk4sg(double delin, double Din, double & delout, double & Dout, 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);
void rkqc(vector<double> & y, vector<double> & yscal, vector<double> & dy, double & N, double htry, double & hdid, double & hnext, double eps, double sga);
double sigma8(vector<double> y);
void evolve(double atarg, vector<double> & y,vector<double> & yscal, vector<double> & dy, double & N, double htry, double & hdid, double & hnext, double eps, double sga);

int main()
{
  double anow, aprev, znow = 49.0,Hnow,dN,N,daft,dNfirst,win,twin,sig8;
  double htry, hdid, hnext,eps=0.0001,factor,ksqr,sigval,rmid,LOM,ksin;
  double ztarg,atarg,Ntarg;
  
  double zc = 3, dz, Nsave, sga;
  int zsteps=15;

  dz = zc/(1.0*zsteps);
  
  int count,j,i,ncrit;
  ifstream p_in,p_in2;

  ofstream s_out,g_out;
  p_in.open("NbodyICs.txt");
  p_in2.open("z49p9.txt");
  s_out.open("sigma8z.txt");
  g_out.open("growth.txt");
   
	anow = 1./(1.+49.9);
	aprev = 1./(1.+50.);
	Hnow=Hubble(anow);
	htry = 0.0001; // fiducial step size in units of efolding time N = H dt
	N = log(1/(1+znow));

	dNfirst = log(anow) - log(aprev);
	vector<double> Pk(Nmodes), delta(Nmodes), k(Nmodes),Pk2(Nmodes),deldot(Nmodes),PNL(Nmodes),DL(Nmodes);
	vector<double> y(3*Nmodes),dy(3*Nmodes),yscal(2*Nmodes),F(Nmodes),sv(50),delorig(Nmodes);
	
	vector<double> deltasave(Nmodes), deldotsave(Nmodes), ysav(3*Nmodes);
	
	LOM = log(OmM);
	
	for(int i=0;i<Nmodes;i++)
	  {
	    p_in >> k[i] >> Pk[i];
	    p_in2 >> daft >> Pk2[i];
	  }
	for(int i=0;i<Nmodes;i++)
	  {
	    delta[i] = sqrt(k[i]*k[i]*k[i]*Pk[i]/(2.*3.1415*3.1415));
	    delorig[i] = delta[i];
		daft = (-delta[i] + sqrt(k[i]*k[i]*k[i]*Pk2[i]/(2.*3.1415*3.1415)))/dNfirst;
		deldot[i] = delta[i]*pow(OmM*pow(anow,-3.)*pow((h*H0)/Hubble(anow),2.0),0.55);
		if(i%20==0) cout << delta[i] << " " << deldot[i] << " " << daft << endl;
	}
	cout << OmM*pow(anow,-3.)*pow((h*H0)/Hubble(anow),2.0) << " current omega matter" << endl;

	count =0;
	
	for(j=0;j<Nmodes;j++)
	  {
	    yscal[j]=delta[j];
	    yscal[j+Nmodes]=deldot[j];
	    y[j+2*Nmodes] = k[j];
	  }
	for(int i=0;i<Nmodes;i++)
      	  {
	    y[i] = delta[i];
	    y[i+Nmodes] = deldot[i];
	  }


	atarg = 1/(1.0+zc);
	sga = 1.0;
	evolve(atarg,y,yscal,dy,N,htry,hdid,hnext,eps,sga);
      
	for(int i=0;i<Nmodes;i++)
	  {
	    
	    deltasave[i] = y[i];
	    deldotsave[i] = y[i+Nmodes];
	    ysav[i] = y[i];
	    ysav[i+Nmodes] = y[i+Nmodes];
	  }	
	Nsave=N;
	
	for(int j =0; j<=zsteps; j++)
	  {

	    N = Nsave;
	    if(j>0)
	      {
		for(int i=0;i<Nmodes;i++)
		  {
		    y[i] = deltasave[i];
		    y[i+Nmodes] = deldotsave[i];
		  }
	      }
	    sga=1/(1+zc-j*dz);
	    evolve(sga,y,yscal,dy,N,htry,hdid,hnext,eps,sga);
	    cout <<  zc-j*dz << " " << y[10+Nmodes]/y[10] << " " << 0.02*y[10]/delorig[10] <<  endl; 
	    for(int i=0;i<Nmodes;i++)
	      {		
		Nsave = N;
		deltasave[i] = y[i];
		deldotsave[i] = y[i+Nmodes];
		ysav[i] = y[i];
		ysav[i+Nmodes] = y[i+Nmodes];
	      }	    
	    evolve(1.0,y,yscal,dy,N,htry,hdid,hnext,eps,sga);
	    sig8 = sigma8(y);
	    s_out << 1/sga-1 << " " << sga << " " << sig8 << " " << beta(sga) << endl;
	  }

	for(int i=0;i<Nmodes;i++)
	  {
	    delta[i] = y[i];
	    deldot[i] = y[i+Nmodes];
	  }	
	
	for(int i=0;i<Nmodes;i++)
	  {
	    Pk[i] =twopi*pi*pow(delta[i],2.0)/(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] = twopi*pi*PNL[i]/(k[i]*k[i]*k[i]);
// 	    p_out << k[i] << " " << Pk[i] << " " << PNL[i] << endl;
// 	  }
	
// 	for(j=0;j<50;j++)
// 	  {
// 	    rmid = 10. + 10.*j;
// 	    for(i=0;i<Nmodes;i++)
// 	      {
// 		if(k[i]<(1/rmid)) ncrit=i;
// 		ksqr = 2.0*pow(k[i]*rmid,2.0);
// 		//win = sin(ksqr)/ksqr;
// 		//win = 3*(sin(ksqr)-ksqr*cos(ksqr))*pow(ksqr,-3.0);
// 		win=exp(-ksqr);
// 		twin = deldot[i]/(delta[i]);
// 		F[i] = win*Pk[i]*pow(twin,2.0)*k[i]/(twopi*pi);
// 	      }
// 	    sigval=integrate(F,k);
// 	    //sv[j] = pow(73*73/(twopi*pi),0.5)*sqrt(sigval);
// 	    sv[j] = sqrt(1/3.)*100*sqrt(sigval);
// 	    //sv[j]=sqrt(sigval);
	    
	    
// 	    v_out << rmid << " " << k[ncrit] << " " << (deldot[ncrit]/delta[ncrit]) << " " << sv[j] << endl;
// 	  }
	rmid =8.;
	
	for(i=0;i<Nmodes;i++)
	  {
	    //	       	if(k[i]<(1/rmid)) ncrit=i;
	    ksin = pow(k[i]*rmid,1.0);
	    //win = sin(ksqr)/ksqr;
	    win = 3*(sin(ksin)-ksin*cos(ksin))*pow(ksin,-3.0);
	    //      	twin = deldot[i]*exp(-ksqr)*k[i]/delta[i];
	    F[i] = pow(win*delta[i],2.0);
	    g_out << k[i] << " " << deldot[i]/delta[i] << endl;
	  }
	sigval = integrate(F,k);
	sig8= sqrt(sigval);
	cout << sig8 << " " << rc << " " << Dim << " " << alpha <<  endl;
	
	
}

double sigma8(vector<double> y)
{
  vector<double> k(Nmodes), F(Nmodes);
  double sig8, rmid, ksin, win, sigval;
  rmid=8;

	for(int i=0;i<Nmodes;i++)
	  {
	    //	       	if(k[i]<(1/rmid)) ncrit=i;
	    k[i] = y[i+2*Nmodes];
	    ksin = pow(k[i]*rmid,1.0);
	    //win = sin(ksqr)/ksqr;
	    win = 3*(sin(ksin)-ksin*cos(ksin))*pow(ksin,-3.0);
	    //      	twin = deldot[i]*exp(-ksqr)*k[i]/delta[i];
	    F[i] = pow(win*y[i],2.0);
	  }
	sigval = integrate(F,k);
	sig8= sqrt(sigval);

	return sig8;
}

void evolve(double atarg, vector<double> & y,vector<double> & yscal, vector<double> & dy, double & N, double htry, double & hdid, double & hnext, double eps, double sga)
{
  double anow;

  while(anow<atarg)
    {
      rkqc(y,yscal,dy,N,htry,hdid,hnext,eps,sga);
      htry = hnext;
      anow = exp(N);
      if(exp(N+htry)>atarg)
	{
	  htry=log(atarg)-N;
	  rkqc(y,yscal,dy,N,htry,hdid,hnext,eps,sga);
	  break;
	}
    }
}

double Hubble(double a)
{
double H;

H = H0*h*sqrt(OmM*pow(a,-3.0)+OmR*pow(a,-4.0)+(1-OmM-OmR));

return H;
}

double g(double a)
{
double gval;

gval = -(Dim-4)/(beta(a)*(Dim-2));

return gval;
}
double galt(double a)
{
double gval;

gval = -(Dim-4)/(betaalt(a)*(Dim-2));

return gval;
}

double f(double a, double k)
{
double fval;

// fval = pow((h*k*rc/(a)),2.*(alpha-1.));
 fval = 0;
return fval;
}

double beta(double a)
{
double betaval;

betaval = 1. + 2.*pow(Hubble(a)*rc,2.*(1-alpha))*(1. + growth(a)/3.);

return betaval;
}
double betaalt(double a)
{
  double betaval,rcalt;
  rcalt=2750;

betaval = 1. + 2.*pow(Hubble(a)*rcalt,2.*(1-alpha))*(1. + growth(a)/3.);

return betaval;
}
double growth(double a)
{
double growval;

growval = -1.5*(OmM*pow(a,-3.)/(OmM*pow(a,-3.)+1-OmM));

return growval;
}

double eq1(double a, double k, double capD, double del)
{
double eqval,dcrit;


eqval = -1*(2.+growth(a))*capD + 1.5*(1.-g(a))*OmM*pow(a,-3.)*pow((h*H0)/Hubble(a),2.0)*del/(1+f(a,k));

return eqval;
}
double eq1sg(double a, double k, double capD, double del)
{
double eqval,dcrit;

 eqval = -1*(2.+growth(a))*capD + 1.5*(1-galt(a))*OmM*pow(a,-3.)*pow((h*H0)/Hubble(a),2.0)*del;

return eqval;
}

void rk4(double delin, double  Din, double & delout, double & Dout, double k, double N, double h)
{
  double k1=0,k2=0,k3=0,k4=0,l1=0,l2=0,l3=0,l4=0, at, Ltemp,lat,vtemp,dla,newla;
  double a, L, v, la, newdel, newD;

  double tempN;

	vtemp = Din;
	Ltemp = delin;

  a = exp(N);
  k1 = h*vtemp;
  l1 = h*eq1(a,k,vtemp,Ltemp);

  tempN = N + h*0.5;
  at = exp(tempN);
  vtemp = vtemp + 0.5*l1;
  Ltemp = Ltemp + 0.5*k1;
  k2 = h*vtemp;
  l2 = h*eq1(at,k,vtemp,Ltemp);

  vtemp = Din + 0.5*l2;
  Ltemp = delin + 0.5*k2;
  k3 = h*vtemp;
  l3 = h*eq1(at,k,vtemp,Ltemp);

  vtemp = Din + l3;
  Ltemp = delin + k3;
  tempN = N + h;
  at = exp(tempN);
  k4 = h*vtemp;
  l4 = h*eq1(at,k,vtemp,Ltemp);


  newdel = delin+ k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
  newD = Din + l1/6.0 + l2/3.0 + l3/3.0 + l4/6.0;


	delout = newdel;
	Dout = newD;
}
void rk4sg(double delin, double  Din, double & delout, double & Dout, double k, double N, double h)
{
  double k1=0,k2=0,k3=0,k4=0,l1=0,l2=0,l3=0,l4=0, at, Ltemp,lat,vtemp,dla,newla;
  double a, L, v, la, newdel, newD;

  double tempN;

	vtemp = Din;
	Ltemp = delin;

  a = exp(N);
  k1 = h*vtemp;
  l1 = h*eq1sg(a,k,vtemp,Ltemp);

  tempN = N + h*0.5;
  at = exp(tempN);
  vtemp = vtemp + 0.5*l1;
  Ltemp = Ltemp + 0.5*k1;
  k2 = h*vtemp;
  l2 = h*eq1sg(at,k,vtemp,Ltemp);

  vtemp = Din + 0.5*l2;
  Ltemp = delin + 0.5*k2;
  k3 = h*vtemp;
  l3 = h*eq1sg(at,k,vtemp,Ltemp);

  vtemp = Din + l3;
  Ltemp = delin + k3;
  tempN = N + h;
  at = exp(tempN);
  k4 = h*vtemp;
  l4 = h*eq1sg(at,k,vtemp,Ltemp);


  newdel = delin+ k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
  newD = Din + l1/6.0 + l2/3.0 + l3/3.0 + l4/6.0;


	delout = newdel;
	Dout = newD;
}

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;
//		  upper = table_slope[i] = n_spec-4.0;
	       } 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);

		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=1; 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;
		sum += 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,beta2,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 = 1.4861 + 1.83693*rn + 1.67618*nsqr + 0.7940*rn*nsqr
     + 0.1670756*nsqr*nsqr - 0.620695*rncur;
   a = pow(10,a);
   b = pow(10,(0.9463+0.9466*rn+0.3084*nsqr-0.940*rncur));
   c = pow(10,(-0.2807+0.6669*rn+0.3214*nsqr-0.0793*rncur));
   xmu = pow(10,(-3.54419+0.19086*rn));
   xnu = pow(10,(0.95897+1.2857*rn));
   alpha2 = 1.38848+0.3701*rn-0.1452*nsqr;
   beta2 = 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 = 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,beta2)/(1+plin*alpha2)*exp(-y/4.0-ysqr/8.0);

	pnl = pq + ph;

}

void rkqc(vector<double> & y, vector<double> & yscal, vector<double> & dy, double & N, double htry, double & hdid, double & hnext, double eps, double sga)
{
  int i,max;
  vector<double> ysav(3*Nmodes),ytemp(3*Nmodes),dysav(3*Nmodes),v(Nmodes),del(Nmodes),delsav(Nmodes),vsav(Nmodes);
  vector<double> vtemp(Nmodes),deltemp(Nmodes),k(Nmodes),delscal(Nmodes),vscal(Nmodes);
  double xsav, hh,h,temp, errmax, anow;


  anow = exp(N);

  for(i=0;i<3*Nmodes;i++)
    {
      ysav[i] = y[i];
      dysav[i] = dy[i];
    }

  for(i=0;i<Nmodes;i++)
		{
		del[i]=y[i];
		v[i] = y[i+Nmodes];
		k[i] = y[i+2*Nmodes];
		deltemp[i] = del[i];
		delsav[i] = del[i];
		vtemp[i] = v[i];
		vsav[i] = v[i];
		delscal[i] = yscal[i];
		vscal[i] = yscal[i+Nmodes];
		}


  h = htry;
  for(;;)
    {
      hh = 0.5*h;
      
      if(anow<sga)
	{
		for(i=0;i<Nmodes;i++)
			{
				rk4(delsav[i],vsav[i],deltemp[i],vtemp[i],k[i],N,hh);
				rk4(deltemp[i],vtemp[i],del[i],v[i],k[i],N,hh);
//      if((eta+h) == etaold) cout << "WARNING: Step Size TOO SMALL!" << endl;

				rk4(delsav[i],vsav[i],deltemp[i],vtemp[i],k[i],N,h);
			}
	}
      else
	{
		for(i=0;i<Nmodes;i++)
			{
				rk4sg(delsav[i],vsav[i],deltemp[i],vtemp[i],k[i],N,hh);
				rk4sg(deltemp[i],vtemp[i],del[i],v[i],k[i],N,hh);
//      if((eta+h) == etaold) cout << "WARNING: Step Size TOO SMALL!" << endl;

				rk4sg(delsav[i],vsav[i],deltemp[i],vtemp[i],k[i],N,h);
			}
	}

			errmax = 0.0;
      for(i=0;i<Nmodes;i++)
			{
			deltemp[i] = del[i]-deltemp[i];
			temp= fabs(deltemp[i]/delscal[i]);
			if(errmax<temp) errmax = temp;
			vtemp[i] = v[i]-vtemp[i];
			temp = fabs(vtemp[i]/vscal[i]);
			if(errmax<temp) errmax = temp;
			dy[i] = del[i]-delsav[i];
			dy[i+Nmodes] = v[i]-vsav[i];
			}
      errmax /= eps;
      if(errmax <= 1.0)
			{
			hdid = h;

			if(errmax > ERRCON)
				{
					hnext = SAFETY*h*exp(PGROW*log(errmax));
				}
			else
				{
					hnext = 4.0 * h;
				}
			if(hnext > hmax) hnext = hmax;
			break;
			}

			h = SAFETY*h*exp(PSHRNK*log(errmax));
		}

  for(i=0;i<Nmodes;i++)
  {
   y[i] = del[i]+  deltemp[i]*FCOR;
	y[i+Nmodes] = v[i] + vtemp[i]*FCOR;
	}
  N += hdid;
}