#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "nrutil.h"

//y is the input image, nx and ny are the pixels along x and y axis;
//yt and residu are the required matrix for the function
//para contains the needed parameters: centx=para[3], centy=para[4]


void star_gaus(float **y,int nx,int ny,double *para);

void Interp_bicubic(int nx,int ny,float **a0,int nbound, int nxt,int nyt, float **at,double *xcen);
void splie2(float **ya, int m, int n, float **y2a);
void spline(float y[], int n, float yp1, float ypn, float y2[]);
void splint(float ya[], float y2a[], int n, float x, float *y);


void centroidWgt(float *y, int nx, int ny, double *para)
{
  int i, j, k;
  float **yy;
  double **basef;
  double **coff;
  double ccol, crow;

  yy = matrix(0, nx-1, 0, ny-1);
  for(i=0; i<nx; i++)
    for(j=0; j<ny; j++)
    {
      k = j + i*nx;
      yy[i][j] = y[k];
    }

  star_gaus(yy, nx, ny, para);
  //ccol = para[4];
  //crow = para[3];
}

void imSplint(float *y, int nx, int ny, double *para, int nxt, int nyt, float *yat)
{
  int i, j, k;
  float **yy;
  double **basef;
  double **coff;
  double ccol, crow;

  yy = matrix(0, nx-1, 0, ny-1);
  for(i=0; i<nx; i++)
    for(j=0; j<ny; j++)
    {
      k = j + i*nx;
      yy[i][j] = y[k];
    }
  crow = para[3];
  ccol = para[4];

  int nbound;
  float **at;
  double xcen[2];
  at = matrix(0, nxt-1, 0, nyt-1);
  nbound = (int)((nx - nxt)/2);
  xcen[0]= crow;
  xcen[1]= ccol;
  Interp_bicubic(nx, ny, yy, nbound, nxt, nyt, at, xcen);
  for(i=0; i<nxt; i++)
    for(j=0; j<nyt; j++)
    {
      k = j + i*nxt;
      yat[k] = at[i][j];
    }
}





//void star_gaus(float **y,int nx,int ny,float **yt,float **residu,double *para)
void star_gaus(float **y,int nx,int ny,double *para)
{
  void gasfit_2D(double **x, double *y,int np,double *para,double bg0, int fbg);
  double **xs,*ys,ymax;
  int i,j,np,npt,im,jm,**xi,k;
  double kc,bgc,sigmac,xc,yc,sigma2v,det;
  double bg0=0;
  int fbg=0;
  np=nx*ny;
  xs=dmatrix(0,np-1,0,1);
  ys=dvector(0,np-1);
  xi=imatrix(0,np-1,0,1);
  ymax=y[0][0];
  for(i=0;i<nx;i++)for(j=0;j<ny;j++){
      if(ymax<y[i][j]){ymax=y[i][j];im=i;jm=j;}
    }

  int cutPix;
  cutPix = 23;
  npt=0;
  for(i=-cutPix;i<=cutPix;i++){
    for(j=-cutPix;j<=cutPix;j++){
      xs[npt][0]=xi[npt][0]=i+im;
      xs[npt][1]=xi[npt][1]=j+jm;
      ys[npt]=y[im+i][jm+j];
      npt++;
    }
  }
  gasfit_2D(xs, ys,npt,para,bg0,fbg);
  kc=para[0];sigmac=para[1];bgc=para[2];xc=para[3];yc=para[4];
 // printf("%e %e %e %e %e\n",kc,sigmac,bgc,xc,yc);
  /*
  sigma2v=-1./(2.*sigmac*sigmac);
  for(i=0;i<nx;i++){
    for(j=0;j<ny;j++){
    det=DSQR(i-xc)+DSQR(j-yc);
    yt[i][j]=kc*exp(det*sigma2v)+bgc;
    residu[i][j]=y[i][j]-yt[i][j];
    }
  }
  */
  free_dmatrix(xs,0,np-1,0,1);
  free_imatrix(xi,0,np-1,0,1);
  free_dvector(ys,0,np-1);
}

void gasfit_2D(double **x, double *y,int np,double *para,double bg0,int fbg)
{
  void search_2D(double **x,double *y,int np,double kc,double kd,double sigmac,
              double sigmad,double bgc,double bgd,double xc, double xd, 
		 double yc, double yd,double *para,double bg0,int fbg);
  int i,j,k,imax=0,isigma;
  double ymax,ymin,kc,kd,sigmac,sigmad,bgc,bgd,ysigma,xc,yc,xd,yd;
  double det,dett;
  ymin=ymax=y[imax];
  for(i=1;i<np;i++){
    if(ymax<y[i]){imax=i;ymax=y[i];}
    if(ymin>y[i])ymin=y[i];
    
  }
  kc=ymax;kd=ymax/12.;
  det=ysigma=kc*exp(-0.5);
  for(i=0;i<np;i++){
    dett=fabs(ysigma-y[i]);
    if(dett<det && y[i]!=kc){det=dett;isigma=i;}
    //if(dett<det){det=dett;isigma=i;}
  }
  xc=x[imax][0];yc=x[imax][1];
  sigmac=sqrt(DSQR(xc-x[isigma][0])+DSQR(yc-x[isigma][1]))*1.;
  xd=yd=sigmac*0.25;
  sigmad=0.25*sigmac;
  bgc=0.;bgd=fabs(ymin);
  for(i=0;i<4;i++){
    search_2D(x,y,np,kc,kd,sigmac,sigmad,bgc,bgd,xc,xd,yc,yd,para,bg0,fbg);
    kd*=0.33;sigmad*=0.33;bgd*=0.33;xd*=0.33;yd*=0.33;
    kc=para[0];sigmac=para[1];bgc=para[2];xc=para[3];yc=para[4]; 
    
  }
  if(fbg==0)para[2]=bg0;
}
void search_2D(double **x,double *y,int np,double kc,double kd,double sigmac,
	       double sigmad,double bgc,double bgd,double xc, double xd, 
	       double yc, double yd,double *para,double bg0,int fbg)
{
  double k,sigma,bg,k2,k20,sigma2v,det,xt,yt;
  int i,j,l,m,p,q;
  sigma2v=-1./(2.*sigmac*sigmac);
  bg=bgc;
  k20=0;
  for(m=0;m<np;m++){
    det=DSQR(x[m][0]-xc)+DSQR(x[m][1]-yc);
    det=kc*exp(det*sigma2v)+bgc-y[m];
    k20+=det*det;
  }
  for(i=-4;i<=4;i++){
    k=kc+i*kd;
    if(k>0){
      for(j=-4;j<=4;j++){
	sigma=sigmac+j*sigmad;
	if(sigma>0){
	  sigma2v=-1./(2.*sigma*sigma);
	  for(p=-4;p<=4;p++){
	    xt=xc+p*xd;
	    for(q=-4;q<=4;q++){
	      yt=yc+q*yd;
	      k2=0;
	      if(fbg==0){
		bg=bg0;
		for(m=0;m<np;m++){
		  det=DSQR(x[m][0]-xt)+DSQR(x[m][1]-yt);
		  det=k*exp(det*sigma2v)+bg-y[m];
		  k2+=det*det;
		}
	      }
	      else{
		for(l=-4;l<=4;l++){
		  bg=bgc+l*bgd;
		  for(m=0;m<np;m++){
		    det=DSQR(x[m][0]-xt)+DSQR(x[m][1]-yt);
		    det=k*exp(det*sigma2v)+bg-y[m];
		    k2+=det*det;
		  }
		}
	      }
	      //printf("k20=%e k2=%e\n",k20,k2);
	      if(k2<=k20){k20=k2;para[5]=k2;
		para[0]=k;para[1]=sigma;para[2]=bg;para[3]=xt;para[4]=yt;}
	    }
	  }
	}	  
      }
    }
  }
}

/////////////////////////////
//#include <math.h>
//#include <stdio.h>
//#include <stdlib.h>
//#include "nrutil.h"
//nx,ny is the pixels of x and y direction
//a0 is the input image in nx*ny
//nbound is the boundary limit
//nxt,nyt is out put image dimentions 
//at is the output image and cen represent the required center
void Interp_bicubic(int nx,int ny,float **a0,int nbound,
		    int nxt,int nyt, float **at,double *xcen)
{

  void splie2(float **ya, int m, int n, float **y2a);
  void spline(float y[], int n, float yp1, float ypn, float y2[]);
  void splint(float ya[], float y2a[], int n, float x, float *y);
  int i,j,m,n,ic,jc;
  float *ytmp,*yytmp,**y2a,x1,x2,shift1,shift2;
  y2a=matrix(0,nx,0,ny);
  ytmp=vector(0,ny);
  yytmp=vector(0,ny);
  splie2(a0,nx,ny,y2a);
  ic=nx*0.5;
  jc=ny*0.5;
  shift1=xcen[0]-ic;//-0.5;
  shift2=xcen[1]-jc;//-0.5;
  if(fabs(shift1)>nbound || fabs(shift2)>nbound){
    printf("bicubic shifts too much %e %e\n",shift1,shift2);
    getchar();
  }
  for (n=0;n<nyt;n++){
    x2=n+nbound+shift2;
    for(i=0;i<nx;i++){
      splint(a0[i],y2a[i],ny,x2,&yytmp[i]);
      spline(yytmp,ny,1.0e30,1.0e30,ytmp);
    }
    for (m=0;m<nxt;m++){
      x1=m+nbound+shift1;
      splint(yytmp,ytmp,ny,x1,&at[m][n]);
    }
  }
  free_vector(yytmp,0,ny);
  free_vector(ytmp,0,ny);
  free_matrix(y2a,0,nx,0,ny);

}
void splie2(float **ya, int m, int n, float **y2a)
{
  void spline(float y[], int n, float yp1, float ypn, float y2[]);
  int j;
  
  for (j=0;j<m;j++)spline(ya[j],n,1.0e30,1.0e30,y2a[j]);
}

void spline(float y[], int n, float yp1, float ypn, float y2[])
{
  int i,k;
  float p,qn,sig,un,*u;
  
  u=vector(0,n-1);
  if (yp1 > 0.99e30)
    y2[0]=u[0]=0.0;
  else {
    y2[0] = -0.5;
    u[0]=3.0*(y[1]-y[0]-yp1);
  }
  sig=0.5;
  for (i=1;i<=n-2;i++) {
    p=sig*y2[i-1]+2.0;
    y2[i]=(sig-1.0)/p;
    u[i]=(y[i+1]-2.*y[i]+y[i-1]);
    u[i]=(3.0*u[i]-sig*u[i-1])/p;
  }
  if (ypn > 0.99e30)
    qn=un=0.0;
  else {
    qn=0.5;
    un=3.0*(ypn-y[n-1]+y[n-2]);
  }
  y2[n-1]=(un-qn*u[n-2])/(qn*y2[n-2]+1.0);
  for (k=n-2;k>=0;k--)y2[k]=y2[k]*y2[k+1]+u[k];
  free_vector(u,0,n-1);
}
void splint(float ya[], float y2a[], int n, float x, float *y)
{
  void nrerror(char error_text[]);
  int klo,khi,k;
  float h,b,a;
  
  klo=x;
if( klo<0)klo=0;
if(klo==n-1)klo=n-2;
  khi=klo+1;
  if (klo<0 || khi>=n) printf("error:klo, khi, n: %d %d %d\n", klo, khi, n);
  if (klo<0 || khi>=n) nrerror("Bad xa input to routine splint");
  a=(khi-x);
  b=(x-klo);
  *y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])/6.0;
}