#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 main()
{
    int i, j;
    int IMAGE_WIDTH, IMAGE_HEIGHT;
    float center_x, center_y;
    float R, dis;
    float **Gauss_map;
    double *para;     
    //double para[10];     

    IMAGE_WIDTH = 180;
    IMAGE_HEIGHT = 180;
    center_x = IMAGE_WIDTH/2 -0.;
    center_y = IMAGE_HEIGHT/2+0.;

    R = sqrt(center_x*center_x + center_y*center_y);
    //Gauss_map = np.zeros((IMAGE_HEIGHT, IMAGE_WIDTH))
    Gauss_map = matrix(0, IMAGE_HEIGHT-1, 0, IMAGE_WIDTH-1);
    para = dvector(0,10);

    for(i=0; i<IMAGE_HEIGHT; i++)
    {
        for(j=0; j<IMAGE_WIDTH; j++)
        {
            //#dis = np.sqrt((i-center_y)**2+(j-center_x)**2)
            dis = (i-center_y)*(i-center_y) + (j-center_x)*(j-center_x);
            //dis = sqrt(dis);
            Gauss_map[i][j] = exp(-0.5*dis/R);
        }
    }
    star_gaus(Gauss_map, IMAGE_HEIGHT, IMAGE_WIDTH, para);
    printf("ok?\n");
    printf("para:%f %f\n", para[3], para[4]);

    //free_matrix(Gauss_map,0,IMAGE_HEIGHT-1,0,IMAGE_WIDTH-1);
    //free_dvector(para,0,4);
}


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

  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);
}


//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;}
    }printf("x=%d,y=%d,ymax=%e\n",im,jm,ymax);

  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++;
    }
  }
  printf("check:: %f %f %f %d\n",ys[0], ys[10], ys[100], 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("xc, yc: %f %f\n",yc, xc);
  //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];
    
  }
  //printf("ymax=%e,imax=%d\n",ymax,imax);
  kc=ymax;kd=ymax/12.;
  det=ysigma=kc*exp(-0.5);//printf("det=%e,kc=%e\n",det,kc);
  for(i=0;i<np;i++){
    dett=fabs(ysigma-y[i]);//printf("dett=%e,det=%e,i=%d\n",dett,det,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];printf("isigma=%d,imax=%d\n",isigma,imax);
  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);
  //printf("sigma=%e\n",sigmac);
  for(i=0;i<4;i++){
    printf("i = %d\n", i);
    //printf("%e %e %e %e %e k2=%e\n",kc,sigmac,bgc,xc,yc,para[5]);
    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;}
	    }
	  }
	}	  
      }
    }
  }
}