/* RRIを等時間間隔にリサンプリング  */
/* q次の非ガウス指標を計算 */
/* 16/2/2009 Ken KIYONO */
/* 移動平均 */ 
/* 粗視化時系列と分布を計算，指標の推定*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "nr.h"
#include "nrutil.h"
#include "nrutil.c"
#include "intde2.c"
#define SQRTPI 1.7724538509055160272981674833411
#define FUNC(x) ((*funk)(-log(x))/(x))
#define PI 3.1415926535897932384626433832795
#define SQR2PI 0.39894228040143267793994605993439
#define NUMAW 100000

void help(void);
void splint(double xa[], double ya[], double y2a[], int n, double x, double *y);
void spline(double x[], double y[], int n, double yp1, double ypn, double y2[]);
int read_data(char filename[], double x[]);
void svdfit(double x[], double y[], int ndata, double a[], int ma, double *chisq);
void svdvar(double **v, int ma, double w[], double **cvm);
double polyfunc(double x, double p[], int np);
void lfit(double x[], double y[], int ndat, double a[], int ma, double *chisq);
double gammln(double xx);
void sort(unsigned long n, double arr[]);
double P_L(double s, double sgm);
double Gau(double x);
double G_r(double sg);
void intdeini(int lenaw, double tiny, double eps, double *aw);
void intdei(double (*f)(double), double a, 
        double *aw, double *nint, double *err);
int pdf(long n, double x[], int bin, double y[],double py[]);

/* Global variables. */
char *pname;	/* this program's name (for use in error messages) */
double *data;
double extx;
double lambda;
double lambda2;
int nfunc;
double tiny, *aw, nint, err;
int lenaw;

main(int argc, char **argv)
{
    long nn,i,j,k,i0;
	long hrlength,ttmp;
	long scale, ncg;
	long nbin;
	double dscale,average,meanhr;
    double y, yp = 0.0, ysum = 0.0;
	double *hr,tt,sd;
	double rint;
	double unit;
	double totalt;
	double intt;
	double slope;
	double yp1,ypn,*xs,*ys,*ys2,ysp;
	double *cgs;
	double *px,*py,*pa,chisq;
	double mom,q,shfty;
	double *u,*Pu,*Put;
	int linear;
	int dp,pn;
	int methd,stdz;
	char fname1[253];
	char fname2[256];
	char fname3[256];
	char fname4[256];
	FILE *fp2, *fp3, *fp4;

	
	/* Read and interpret the command line. */
    pname = argv[0];
    rint = 0.25;
	linear = 0;
    dp = 3; /* 多項式の次数 */
    dscale = 25;
    methd = 1;
    nbin = 100;
    stdz = 0;
    q = 0.25;
    shfty = 1;
    
    for (i = 1; i < argc && *argv[i] == '-'; i++) {
      switch(argv[i][1]) {
	case 's':
		dscale = atof(argv[++i]);
		if(dscale < 2) dscale = 2;
		break;
 	case 'r':
		shfty = atof(argv[++i]);
		if(shfty == 0) shfty = 1;
		break;
	case 'd':
		dp = atoi(argv[++i])+1; /* 多項式のパラメータ数 */
		break;
	case 'q':
		q = atof(argv[++i]); 
		break;
	case 'a':
		stdz = 2;
		break;
	case 'v':
		stdz = 1;
		break;
	case 'm':
		methd = atoi(argv[++i]);
		if(methd < 0){
			methd = 1;
		}else if(methd > 2) methd = 2;
		break;
  	case 'n':
		nbin = atol(argv[++i]);
		if(nbin < 10) scale = 10;
		break;
  	case '0':
		linear = 0;
		break;
  	case '3':
		linear = 2;
		break;
  	case '1':
		linear = 1;
		break;
  	case 't':
		rint = atof(argv[++i]);
		if(rint < 0.01) rint = 0.01;
		break;
	case 'h':	/* print usage information and quit */
        default:
	  help();
	  exit(1);
      }
    }
	
	if(argc < 2){
	  help();
	  exit(1);
	}
	strcpy_s(fname1,253,argv[argc-1]);
	strcpy_s(fname2,253,argv[argc-1]);
	strcpy_s(fname3,253,argv[argc-1]);
	strcpy_s(fname4,253,argv[argc-1]);
	strcat_s(fname2,256,".tms");
	strcat_s(fname3,256,".dis");
	strcat_s(fname4,256,".lmd");
	
//	strcpy(fname1,argv[argc-1]);
	nn = read_data(fname1, data);
	
	ysum = 0;
	for(i=1;i<=nn;i++){
		ysum += data[i];
	}

	if(data[1] > 100) unit = 1000; /* 単位　msec　と　sec の判別 */
	else unit = 1;

	totalt = (ysum-data[nn])/unit;

	hrlength = (long)((totalt)/rint);
	hr = vector(1,hrlength);
	ysum = ysum/(double)(nn);

	if(unit > 1){
		for(j=1;j<=nn;j++){
			data[j] = data[j]/unit;
		}
	}

	fprintf(stderr,"number of input data: %d\n", nn);
	fprintf(stderr,"total time: %lf\n", totalt);
	fprintf(stderr,"resampling interval (frequency): %lf (%lf)\n", rint, 1/rint);

	scale = (long)(dscale/rint+0.5);
	ttmp = 1;
	tt = data[ttmp];

	if(linear < 1){ /* ステップ補間 */
		fprintf(stderr, "Resamping as a step function\n");	
		for(k = 1;k <= hrlength; k++){
			intt = rint*(double)(k-1);
//			if(tt < intt){
			while(tt < intt){
				ttmp++;
				tt = tt + data[ttmp];
			}
			hr[k] = data[ttmp];
		}
	}else if(linear == 1){ /* 線形補完 */
		fprintf(stderr, "Resamping with linear interpolation.\n");	
		for(k = 1;k <= hrlength; k++){
			intt = rint*(double)(k-1);

			while(tt < intt){
				ttmp++;
				tt = tt + data[ttmp];
			}
			slope = (data[ttmp+1]-data[ttmp])/data[ttmp];
			hr[k] = data[ttmp+1]+slope*(intt - tt);	
		}
	}else{
		
		fprintf(stderr, "Resamping with cubic spline interpolation.\n");	
			
//		ttmp = 3;
		xs=vector(1,nn);
		ys=vector(1,nn);
		ys2=vector(1,nn);

		tt = 0;
		for(i = 1;i <= nn;i++){
			xs[i] = tt;
			ys[i] = data[i];
			tt += data[i];
		}
		
		yp1 = 1e30; /* 自然スプラインの境界条件 */
		ypn = yp1; /* 自然スプラインの境界条件 */
		spline(xs,ys,nn,yp1,ypn,ys2);
		
		for(k = 1;k<= hrlength; k++){
			tt = rint*(double)(k-1);
			splint(xs,ys,ys2,nn,tt,&ysp);
			hr[k] = ysp;
		}
		free_vector(ys2,1,nn);
		free_vector(ys,1,nn);
		free_vector(xs,1,nn);

	}
	
    /* 粗視化 */
   	average = 0;
   	nn = hrlength;
    for(i=1;i<=nn;i++){
		average += hr[i];
	}
	average = average/(double)nn;

	fprintf(stderr, "average: %lf\n", average);
	meanhr = average;
	for(i=1;i<=nn;i++){
		hr[i] = hr[i] - average;
	}
	
	fprintf(stderr, "scale: %5.5lf (%ld pt)\n", dscale, scale);

	ncg = scale*(int)(nn/scale - 1);
//	fprintf(stderr,"nn = %ld, ncg = %ld\n", nn, ncg);
	cgs = vector(1,ncg);
	
	/*  */
	if(dp <= 1){ /* detrendなし */
		fprintf(stderr, "WITHOUT detrending\n");

		for(i=1;i<=ncg;i++){
			cgs[i] = 0;
			for(j=i;j<=i+scale-1;j++){
				cgs[i] += hr[j];
			}
//			cgs[i] = cgs[i]/(double)(scale-1);
		}	
	
	}else{
		if(nn < scale) nrerror("The data length is not enough!\n");
		fprintf(stderr, "detrend using polynomial fits of order %d\n", dp-1);

		for(i=1;i<=ncg;i++){
			cgs[i] = 0;
			for(j=i;j<=i+scale-1;j++){
				cgs[i] += hr[j];
			}
//			fprintf(stdout, "%lf\n", cgs[i]);
//			cgs[i] = cgs[i]/(double)(scale-1);
		}	

		pn = dp;
		px = vector(1,scale);
		py = vector(1,scale);
		pa = vector(1,pn);

		for(i=1;i<=scale;i++) px[i] = (double)i;
		    
		for(i=1; i<= ncg; i += scale){
			for(j=1;j<=scale;j++){
				py[j]=cgs[j+i-1];
			}
			
			if(methd > 1){
				svdfit(px, py, scale, pa, pn, &chisq);
			}else{
				lfit(px, py, scale, pa, pn, &chisq);
			}
			
			for(j=1;j<=scale;j++){
				cgs[j+i-1] = py[j] - polyfunc(px[j],pa,pn);
			}
		}
		
		free_vector(px,1,scale);
		free_vector(py,1,scale);
		free_vector(pa,1,pn);
	}
	
	average = 0;
	for(i=1;i<=ncg;i++){
		average += cgs[i];
	}
	
	average = average/(double)ncg;
	
	sd = 0;
	for(i=1;i<=ncg;i++){
		sd += (cgs[i]-average)*(cgs[i]-average);
	}
	
	sd = sqrt(sd/(double)ncg);
	fprintf(stderr, "sd: %lf \n", sd);

    if((fp2 = fopen(fname2,"w")) == NULL){
		fprintf(stderr,"No output file %s!\n",fname2);
		return 0;
	}else{
		fprintf(stderr,"resampled and coase-grained series: %s \n",fname2);
	}

	for(k = 1;k<= hrlength; k++){
		hr[k] = 1000*hr[k];
		
		if(k <= ncg){
			cgs[i] = cgs[i] - average; /* zero mean */

			if(stdz < 1){
				fprintf(fp2,"%lf\t%lf\t%lf\n",rint*(double)(k-1), hr[k]+meanhr, cgs[k]*1000);
			}else{
				fprintf(fp2,"%lf\t%lf\t%lf\n",rint*(double)(k-1), hr[k]+meanhr, cgs[k]/sd);
			}
			
		}else{
			fprintf(fp2,"%lf\t%lf\n",rint*(double)(k-1), hr[k]+meanhr);
		}
	}
	fclose(fp2);

	/* q次非ガウス指標 */
	for(i = 1;i <= ncg; i++){
		cgs[i] = cgs[i]/sd;
	}
	
	mom = 0;
	for(i = 1; i <= ncg; i++){
		mom += pow(fabs(cgs[i]),q); 
	}

	mom = mom/(double)ncg;
	lambda = 2/(q * (q-2)) * (log(SQRTPI * mom) - 0.5 * q * log(2) - gammln((q+1)/2));
	if(lambda > 0){
		lambda = sqrt(lambda);
	}else{
		fprintf(stderr, "square of lambda = %lf\n", lambda);
		lambda = 0;
	}
	
//	fprintf(stdout, "%s\t%lf\t%lf\t%lf\n",argv[argc-1], q, sd, lambda);
	
	/* PDFの推定と近似 */

	u = vector(1,nbin);
	Pu = vector(1,nbin);
	Put = vector(1,nbin);
	
	i0 = pdf(ncg, cgs, nbin, u, Pu);
	
    lenaw = NUMAW;
	aw = vector(1,NUMAW);
	
    tiny = 1.0e-307;
	lambda2 = lambda*lambda;
	
    intdeiini(lenaw, tiny, 1.0e-16, aw);
	for(i = 1; i <= nbin; i++){			
				extx = u[i];
				nfunc = 0;
				intdei(G_r, 0.0, aw, &nint, &err);
				Put[i] = nint;
	}	

    if((fp3 = fopen(fname3,"w")) == NULL){
		fprintf(stderr,"No output file %s!\n",fname3);
		return 0;
	}else{
		fprintf(stderr,"estimated probability density: %s \n",fname3);
	}
	
	for(i=1;i<=nbin;i++){
		fprintf(fp3, "%lf\t%8.7lf\t%12.11lf\n", u[i], Pu[i]/shfty, Put[i]/shfty);
	}
	fclose(fp3);

	fprintf(stderr, "file=%s ; scale=%5.1lf ; SD=%7.1lf ; lmd=%4.3lf\n", fname1, dscale, sd*1000, lambda);	
	fprintf(stdout, "%s\t%5.1lf\t%7.1lf\t%5.4lf\n", fname1, dscale, sd*1000, lambda);

	free_vector(u,1,nbin);
	free_vector(Pu,1,nbin);
	free_vector(Put,1,nbin);
	free_vector(aw,1,NUMAW);

	free_vector(hr,1,hrlength);
	free_vector(cgs,1,ncg);

    exit(0);
}

double gammln(double xx)
{
	double x,y,tmp,ser;
	static double cof[6]={76.18009172947146,-86.50532032941677,
		24.01409824083091,-1.231739572450155,
		0.1208650973866179e-2,-0.5395239384953e-5};
	int j;

	y=x=xx;
	tmp=x+5.5;
	tmp -= (x+0.5)*log(tmp);
	ser=1.000000000190015;
	for (j=0;j<=5;j++) ser += cof[j]/++y;
	return -tmp+log(2.5066282746310005*ser/x);
}

int read_data(char filename[], double x[]){
	/* データの入力 */
    long maxdat = 0L, N = 0L;
	double y;
	FILE *fp1;
		
    if((fp1 = fopen(filename,"r")) == NULL){
		fprintf(stderr,"File %s not found!\n",filename);
		return 0;
	}else{
		fprintf(stderr,"Input file: %s.\n",filename);
	}

	data = vector(1,10000);
	
	while (fscanf(fp1,"%lf", &y) == 1) {
        if (++N >= maxdat) {	    
			double *s;
			maxdat += 10000;	/* allow the input buffer to grow (the
				   increment is arbitrary) */
			if ((s = realloc(data, maxdat * sizeof(double))) == NULL) {
				fprintf(stderr,
					"%s: insufficient memory, truncating input at row %d\n",
					pname, N);
				break;
			}
			data = s;
		}
		data[N] = y;
    }
    
    fclose(fp1);
	if (N < 1){
		fprintf(stderr,"No data was read!\n");
	    return 0;
	}

	return N;
}


static char *help_strings[] = {
 "使用法: %s [OPTIONS ...] データファイル名\n",
 "設定できるOPTION:",
 " -d N             多項式の次数N",
 " -s W             粗視化スケール",
 " -0               ステップ補間 (ゼロ)",
 " -3               3次スプライン補間 (サン)",
 " -1               線形補間 (イチ)",
 " -t T             リサンプリング間隔をTに設定",
 " -v               分散を1に標準化",
 " -m M             最小２乗フィティングアルゴリズム(M=1:正規方程式，M=2:特異値分解)",
 " -h               オプションの一覧を表示",
NULL
};

void help(void)
{
    int i;

    (void)fprintf(stderr, help_strings[0], pname);
    for (i = 1; help_strings[i] != NULL; i++)
	(void)fprintf(stderr, "%s\n", help_strings[i]);
}

#define NRANSI
#include "nrutil.h"

void spline(double x[], double y[], int n, double yp1, double ypn, double y2[])
{
	int i,k;
	double p,qn,sig,un,*u;

	u=vector(1,n-1);
	if (yp1 > 0.99e30)
		y2[1]=u[1]=0.0;
	else {
		y2[1] = -0.5;
		u[1]=(3.0/(x[2]-x[1]))*((y[2]-y[1])/(x[2]-x[1])-yp1);
	}
	for (i=2;i<=n-1;i++) {
		sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
		p=sig*y2[i-1]+2.0;
		y2[i]=(sig-1.0)/p;
		u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		u[i]=(6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
	}
	if (ypn > 0.99e30)
		qn=un=0.0;
	else {
		qn=0.5;
		un=(3.0/(x[n]-x[n-1]))*(ypn-(y[n]-y[n-1])/(x[n]-x[n-1]));
	}
	y2[n]=(un-qn*u[n-1])/(qn*y2[n-1]+1.0);
	for (k=n-1;k>=1;k--)
		y2[k]=y2[k]*y2[k+1]+u[k];
	free_vector(u,1,n-1);
}
#undef NRANSI

void splint(double xa[], double ya[], double y2a[], int n, double x, double *y)
{
	void nrerror(char error_text[]);
	int klo,khi,k;
	double h,b,a;

	klo=1;
	khi=n;
	while (khi-klo > 1) {
		k=(khi+klo) >> 1;
		if (xa[k] > x) khi=k;
		else klo=k;
	}
	h=xa[khi]-xa[klo];
	if (h == 0.0) nrerror("Bad xa input to routine splint");
	a=(xa[khi]-x)/h;
	b=(x-xa[klo])/h;
	*y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0;
}

#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
#define TOL 1.0e-10 /* 特異値分解 */

void svdfit(double x[], double y[], int ndata, double a[], int ma, double *chisq)
{
	void svbksb(double **u, double w[], double **v, int m, int n, double b[],
		double x[]);
	void fpoly(double x, double p[], int np);
	void svdcmp(double **a, int m, int n, double w[], double **v);
	int j,i;
	double wmax,thresh,sum,*b,*afunc,tmp;
	double **u, **v, *w;

	b=vector(1,ndata);
	afunc=vector(1,ma);
	u = matrix(1,ndata,1,ma);
	v = matrix(1,ma,1,ma);
	w = vector(1,ma);
	
	for (i=1;i<=ndata;i++) {
		fpoly(x[i],afunc,ma);
		for (j=1;j<=ma;j++) u[i][j]=afunc[j];
		b[i]=y[i];
	}
	svdcmp(u,ndata,ma,w,v);
	wmax=0.0;
	for (j=1;j<=ma;j++)
		if (w[j] > wmax) wmax=w[j];
	thresh=TOL*wmax;
	for (j=1;j<=ma;j++)
		if (w[j] < thresh) w[j]=0.0;
	svbksb(u,w,v,ndata,ma,b,a);
	*chisq=0.0;
	for (i=1;i<=ndata;i++) {
		fpoly(x[i],afunc,ma);
		for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
		*chisq += (tmp=(y[i]-sum),tmp*tmp);
	}
	free_vector(afunc,1,ma);
	free_vector(b,1,ndata);
	free_matrix(u,1,ndata,1,ma);
	free_matrix(v, 1,ma,1,ma);
	free_vector(w,1,ma);

}


void svdvar(double **v, int ma, double w[], double **cvm)
{
	int k,j,i;
	double sum,*wti;

	wti=vector(1,ma);
	for (i=1;i<=ma;i++) {
		wti[i]=0.0;
		if (w[i]) wti[i]=1.0/(w[i]*w[i]);
	}
	for (i=1;i<=ma;i++) {
		for (j=1;j<=i;j++) {
			for (sum=0.0,k=1;k<=ma;k++) sum += v[i][k]*v[j][k]*wti[k];
			cvm[j][i]=cvm[i][j]=sum;
		}
	}
	free_vector(wti,1,ma);
}

void svbksb(double **u, double w[], double **v, int m, int n, double b[], double x[])
{
	int jj,j,i;
	double s,*tmp;

	tmp=vector(1,n);
	for (j=1;j<=n;j++) {
		s=0.0;
		if (w[j]) {
			for (i=1;i<=m;i++) s += u[i][j]*b[i];
			s /= w[j];
		}
		tmp[j]=s;
	}
	for (j=1;j<=n;j++) {
		s=0.0;
		for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
		x[j]=s;
	}
	free_vector(tmp,1,n);
}

void svdcmp(double **a, int m, int n, double w[], double **v)
{
	double pythag(double a, double b);
	int flag,i,its,j,jj,k,l,nm;
	double anorm,c,f,g,h,s,scale,x,y,z,*rv1;

	rv1=vector(1,n);
	g=scale=anorm=0.0;
	for (i=1;i<=n;i++) {
		l=i+1;
		rv1[i]=scale*g;
		g=s=scale=0.0;
		if (i <= m) {
			for (k=i;k<=m;k++) scale += fabs(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][i]=f-g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
					f=s/h;
					for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
				}
				for (k=i;k<=m;k++) a[k][i] *= scale;
			}
		}
		w[i]=scale *g;
		g=s=scale=0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f=a[i][l];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][l]=f-g;
				for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
				for (j=l;j<=m;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
					for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
				}
				for (k=l;k<=n;k++) a[i][k] *= scale;
			}
		}
		anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++)
					v[j][i]=(a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
					for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
				}
			}
			for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
		}
		v[i][i]=1.0;
		g=rv1[i];
		l=i;
	}
	for (i=IMIN(m,n);i>=1;i--) {
		l=i+1;
		g=w[i];
		for (j=l;j<=n;j++) a[i][j]=0.0;
		if (g) {
			g=1.0/g;
			for (j=l;j<=n;j++) {
				for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
				f=(s/a[i][i])*g;
				for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
			}
			for (j=i;j<=m;j++) a[j][i] *= g;
		} else for (j=i;j<=m;j++) a[j][i]=0.0;
		++a[i][i];
	}
	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag=1;
			for (l=k;l>=1;l--) {
				nm=l-1;
				if ((double)(fabs(rv1[l])+anorm) == anorm) {
					flag=0;
					break;
				}
				if ((double)(fabs(w[nm])+anorm) == anorm) break;
			}
			if (flag) {
				c=0.0;
				s=1.0;
				for (i=l;i<=k;i++) {
					f=s*rv1[i];
					rv1[i]=c*rv1[i];
					if ((double)(fabs(f)+anorm) == anorm) break;
					g=w[i];
					h=pythag(f,g);
					w[i]=h;
					h=1.0/h;
					c=g*h;
					s = -f*h;
					for (j=1;j<=m;j++) {
						y=a[j][nm];
						z=a[j][i];
						a[j][nm]=y*c+z*s;
						a[j][i]=z*c-y*s;
					}
				}
			}
			z=w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					for (j=1;j<=n;j++) v[j][k] = -v[j][k];
				}
				break;
			}
			if (its == 30) nrerror("no convergence in 30 svdcmp iterations");
			x=w[l];
			nm=k-1;
			y=w[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=pythag(f,1.0);
			f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1.0;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=rv1[i];
				y=w[i];
				h=s*g;
				g=c*g;
				z=pythag(f,h);
				rv1[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g = g*c-x*s;
				h=y*s;
				y *= c;
				for (jj=1;jj<=n;jj++) {
					x=v[jj][j];
					z=v[jj][i];
					v[jj][j]=x*c+z*s;
					v[jj][i]=z*c-x*s;
				}
				z=pythag(f,h);
				w[j]=z;
				if (z) {
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=c*g+s*y;
				x=c*y-s*g;
				for (jj=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	free_vector(rv1,1,n);
}
#undef SWAP

double polyfunc(double x, double p[], int np)
{
	int j;
	double y;
	
	y = p[1];
	for (j=2;j<=np;j++) y += p[j]*pow(x,j-1);
	
	return y;
}

void fpoly(double x, double p[], int np)
{
	int j;

	p[1]=1.0;
	for (j=2;j<=np;j++) p[j]=p[j-1]*x;
}



double pythag(double a, double b)
{
	double absa,absb;
	absa=fabs(a);
	absb=fabs(b);
	if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
	else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
}

void lfit(double x[], double y[], int ndat, double a[], int ma, double *chisq)
{
	void covsrt(double **covar, int ma);
	void gaussj(double **a, int n, double **b, int m);
	void fpoly(double x, double p[], int np);
	int i,j,k,l,m;
	double ym,wt,sum,**beta,*afunc;
	double **covar;

	beta=matrix(1,ma,1,1);
	afunc=vector(1,ma);
	covar = matrix(1,ma,1,ma);
	
	for (j=1;j<=ma;j++) {
		for (k=1;k<=ma;k++) covar[j][k]=0.0;
		beta[j][1]=0.0;
	}
	for (i=1;i<=ndat;i++) {
		fpoly(x[i],afunc,ma);
		ym=y[i];

		for (j=0,l=1;l<=ma;l++) {
			wt=afunc[l];
			for (j++,k=0,m=1;m<=l;m++)
				covar[j][++k] += wt*afunc[m];
			beta[j][1] += ym*wt;
		}
	}
	for (j=2;j<=ma;j++)
		for (k=1;k<j;k++)
			covar[k][j]=covar[j][k];
	gaussj(covar,ma,beta,1);
	for (j=0,l=1;l<=ma;l++)
		a[l]=beta[++j][1];
	*chisq=0.0;
	for (i=1;i<=ndat;i++) {
		fpoly(x[i],afunc,ma);
		for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
		*chisq += SQR((y[i]-sum));
	}
	covsrt(covar,ma);
	free_vector(afunc,1,ma);
	free_matrix(beta,1,ma,1,1);
}

#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
void covsrt(double **covar, int ma)
{
	int i,j,k;
	double swap;

	k=ma;
	for (j=ma;j>=1;j--) {
		for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j]);
		for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i]);
		k--;
	}
}
#undef SWAP
#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
void gaussj(double **a, int n, double **b, int m)
{
	int *indxc,*indxr,*ipiv;
	int i,icol,irow,j,k,l,ll;
	double big,dum,pivinv,temp;

	indxc=ivector(1,n);
	indxr=ivector(1,n);
	ipiv=ivector(1,n);
	for (j=1;j<=n;j++) ipiv[j]=0;
	for (i=1;i<=n;i++) {
		big=0.0;
		for (j=1;j<=n;j++)
			if (ipiv[j] != 1)
				for (k=1;k<=n;k++) {
					if (ipiv[k] == 0) {
						if (fabs(a[j][k]) >= big) {
							big=fabs(a[j][k]);
							irow=j;
							icol=k;
						}
					} else if (ipiv[k] > 1) nrerror("gaussj: Singular Matrix-1");
				}
		++(ipiv[icol]);
		if (irow != icol) {
			for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
			for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
		}
		indxr[i]=irow;
		indxc[i]=icol;
		if (a[icol][icol] == 0.0) nrerror("gaussj: Singular Matrix-2");
		pivinv=1.0/a[icol][icol];
		a[icol][icol]=1.0;
		for (l=1;l<=n;l++) a[icol][l] *= pivinv;
		for (l=1;l<=m;l++) b[icol][l] *= pivinv;
		for (ll=1;ll<=n;ll++)
			if (ll != icol) {
				dum=a[ll][icol];
				a[ll][icol]=0.0;
				for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
				for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
			}
	}
	for (l=n;l>=1;l--) {
		if (indxr[l] != indxc[l])
			for (k=1;k<=n;k++)
				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
	}
	free_ivector(ipiv,1,n);
	free_ivector(indxr,1,n);
	free_ivector(indxc,1,n);
}

#undef SWAP

#define SWAP(a,b) temp=(a);(a)=(b);(b)=temp;
#define M 7
#define NSTACK 50

void sort(unsigned long n, double arr[])
{
	unsigned long i,ir=n,j,k,l=1;
	int jstack=0,*istack;
	double a,temp;

	istack=ivector(1,NSTACK);
	for (;;) {
		if (ir-l < M) {
			for (j=l+1;j<=ir;j++) {
				a=arr[j];
				for (i=j-1;i>=l;i--) {
					if (arr[i] <= a) break;
					arr[i+1]=arr[i];
				}
				arr[i+1]=a;
			}
			if (jstack == 0) break;
			ir=istack[jstack--];
			l=istack[jstack--];
		} else {
			k=(l+ir) >> 1;
			SWAP(arr[k],arr[l+1])
			if (arr[l] > arr[ir]) {
				SWAP(arr[l],arr[ir])
			}
			if (arr[l+1] > arr[ir]) {
				SWAP(arr[l+1],arr[ir])
			}
			if (arr[l] > arr[l+1]) {
				SWAP(arr[l],arr[l+1])
			}
			i=l+1;
			j=ir;
			a=arr[l+1];
			for (;;) {
				do i++; while (arr[i] < a);
				do j--; while (arr[j] > a);
				if (j < i) break;
				SWAP(arr[i],arr[j]);
			}
			arr[l+1]=arr[j];
			arr[j]=a;
			jstack += 2;
			if (jstack > NSTACK) nrerror("NSTACK too small in sort.");
			if (ir-i+1 >= j-l) {
				istack[jstack]=ir;
				istack[jstack-1]=i;
				ir=j-1;
			} else {
				istack[jstack]=j-1;
				istack[jstack-1]=l;
				l=i;
			}
		}
	}
	free_ivector(istack,1,NSTACK);
}
#undef M
#undef NSTACK
#undef SWAP

double G_r(double sg)
{
    extern int nfunc;
	double logs;
	logs = log(sg) + lambda2; // To standardize the PDF, this is necessary.
	logs = logs*logs;
    nfunc++;
	return (SQR2PI)* exp(- logs / (2*lambda2))*P_L(extx,sg)/ (sg*sg*lambda); 
}

double Gau(double x)
{
	return (SQR2PI) * exp(- (x * x) / 2); 
}

double P_L(double s, double sgm)
{
	return (SQR2PI) * exp(- (s*s)/(2*sgm*sgm));
}

int pdf(long n, double x[], int bin, double y[],double py[])
{
	long i;
	int bn,y0;
	double max,min,dy,d0;
	
	min = x[1];
	max = x[1];	
	for(i=2;i<= n;i++){
		if(x[i]<min) min = x[i]; /* unit variance */
		if(x[i]>max) max = x[i]; /* unit variance */
	}
	
	if(fabs(max) > fabs(min)){
		min = - max;
	}else{
		max = -min;
	}
	dy = (max-min)/(double)(bin-1);
	d0 = max-floor(max/dy)*dy;

	if(d0 > dy/2){
		min = min - d0 + dy/2;
	}else{
		min = min - d0 - dy/2;
	}

	for(i = 1;i<=bin;i++){
		y[i] = min + dy * ((double)i);
		py[i] = 0;
	}
	
	for(i = 1;i<=n;i++){
		bn = (int)((x[i] - min)/dy+0.5);
		py[bn]++;
	}

	for(i = 1;i<=bin;i++){
		py[i]=py[i]/(double)n/dy;
	}

	y0 = (int)((0 - min)/dy+0.5);

	return y0;
}