#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "fitsio.h"

#define LINEPIX 2100
#define LINEPIY 200
#define MAXODR 5
#define NOPIX -1e10

void printerror( int status );
void fit();
int  odr=MAXODR;

int main(int argc, char *argv[])
{
  fitsfile *fp;
  int i,j,status,nfound,anynull,bitpix=-32;
  long fpixel, nelements,naxis=2;
  long naxes[2]={LINEPIX, LINEPIY},nbuffer;
  float nullval;
  static float buffer[LINEPIX],zo[LINEPIY][LINEPIX],z[LINEPIY][LINEPIX],xc[LINEPIY],ec[LINEPIY],yc[LINEPIY],zc[LINEPIY],bpo[LINEPIY][LINEPIX],bp[LINEPIY][LINEPIX];

  float zav[LINEPIY],zno[LINEPIX],zod[10],dx,ddx,dxmaxl,dxmax,de,acc,accmin=0.001;
  double zamp,zampmin,zampmin2,zamp1,zamp2,zd,a[MAXODR],b[MAXODR];
  int dj,jmin,mode=5,imin,imax,k,center=100,nj,ni,bpm;
  int inc[6][2]={{0,0},{-1,0},{1,0},{0,-1},{0,1},{0,0}};
  char buf[128],fname[64];
  FILE *fpd;

  if(argc<3 || argc>4){
    fprintf(stderr,"Usage : %s input output\n",argv[0]);
    fprintf(stderr,"  or    %s input output param_file\n",argv[0]);
    exit(0);
  }
	
  if(argc==4){
    if((fpd=fopen(argv[3],"r"))==NULL){
      fprintf(stderr,"Can not open file: %s\n",argv[3]);
      exit(1);
    }
    i=0;
    while(!feof(fpd)){
      if(fgets(buf,sizeof(buf),fpd)==NULL) break;
      if(sscanf(buf,"%f %f",&xc[i],&ec[i])!=2){
	printf("Wrong format\n");
	exit(1);
      }
      i++;
    }
    fclose(fpd);
  }else{
    for(i=0;i<LINEPIY;i++){
      xc[i]=0.;ec[i]=0.;
    }
  }
  
  fprintf(stderr,"Read %s  ... ",argv[1]);
  status=0;
  if(fits_open_file(&fp,argv[1], READONLY, &status))
    printerror(status);
  if(fits_read_keys_lng(fp, "NAXIS", 1, 2, naxes, &nfound, &status))
    printerror(status);
  fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
  if((naxes[0]>LINEPIX)||(naxes[1]!=LINEPIY)){
    fprintf(stderr,"\nIllegal image format\n");
    exit(0);
  }
  fpixel = 1;
  nullval = 0;
  nbuffer = naxes[0];
  center=0;ni=0;
  for(i=0;i<naxes[1];i++){
    if (fits_read_img(fp, TFLOAT, fpixel, nbuffer, &nullval,
                      buffer, &anynull, &status))
      printerror(status);
    zav[i]=0.;nj=0;
    for(j=0;j<naxes[0];j++){
      zo[i][j]=buffer[j];
      if(buffer[j]>NOPIX*0.99){zav[i]+=buffer[j];nj++;}
    }
    if(nj>0) zav[i]/=nj;
    if(nj>naxes[0]/2+1){center+=i;ni++;}
    //fprintf(stderr,"%d %d %d %d\n",i,ni,nj,center);getchar();
    fpixel+=naxes[0];
  }
  center/=ni;
  imin=200;imax=0;
  for(i=0;i<naxes[1];i++){
    if(zav[i]>0.2*zav[center] && imin==200) imin=i+5;
    if(zav[i]<0.2*zav[center] && imin!=200 && imax==0) imax=i-5;
    //fprintf(stderr,"%d %f %d %d\n",i,zav[i],imin,imax);getchar();
  }
  if(imax==0) imax=i-5;
  if(fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");

  strcpy(fname,argv[1]);
  fname[strlen(argv[1])-5]='\0';
  sprintf(fname,"%s_bpm.fits",fname);
  status=0;
  fprintf(stderr,"Read %s  ... ",fname);
  if(fits_open_file(&fp, fname, READONLY, &status)){
    fprintf(stderr,"Failed! Bad Pixel Map is neglected.\n");
    for(i=0;i<naxes[1];i++){
      for(j=0;j<naxes[0];j++) bpo[i][j]=1;
    }
    bpm=0;
  }else{
    if(fits_read_keys_lng(fp, "NAXIS", 1, 2, naxes, &nfound, &status))
      printerror(status);
    fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
    if((naxes[0]>LINEPIX)||(naxes[1]!=LINEPIY)){
      fprintf(stderr,"\nIllegal image format\n");
      exit(0);
    }
    fpixel = 1;
    nullval = 0;
    nbuffer = naxes[0];
    for(i=0;i<naxes[1];i++){
      if (fits_read_img(fp, TFLOAT, fpixel, nbuffer, &nullval,
			buffer, &anynull, &status))
	printerror(status);
      for(j=0;j<naxes[0];j++) bpo[i][j]=buffer[j];
      fpixel+=naxes[0];
    }
    if(fits_close_file(fp, &status))
      printerror(status);
    fprintf(stderr,"OK\n");
    bpm=1;
  }
  
  for(i=0;i<LINEPIY;i++) yc[i]=(float)i;
  if(argc!=4){
    zampmin=1e10;
    for(j=900;j<1150;j++){
      zamp=0.;
      for(dj=-40;dj<40;dj++) zamp+=(zo[center][j+dj]/zav[center]+zo[center+1][j+dj]/zav[center+1])/2/80;
      if(zampmin>zamp) {zampmin=zamp;jmin=j;}
      if(j==900) zamp1=zamp;
      else if(j==1149) zamp2=zamp;
    }
    //fprintf(stderr,"%d %g %g %g\n",jmin,zampmin,zamp1,zamp2);
    if(zampmin<(zamp1+zamp2)/2/10){
      fprintf(stderr,"#### Low Resolution image ####\n");
    }else jmin=-100;
		
    for(j=50;j<naxes[0]-50;j++){
      for(i=0;i<10;i++) zod[i]=0;
      for(dj=-50;dj<50;dj++){
	zamp=(zo[center][j+dj]/zav[center]+zo[center+1][j+dj]/zav[center+1])/2;
	for(i=0;i<10;i++){
	  if(zamp > zod[i]){if(i==9)zod[i]=zamp;else zod[i]=zod[i+1];}
	  else {if(i!=0)zod[i-1]=zamp;i=10;}
	}
      }
      zno[j]=zod[0];
      if(zno[j]<0.1) zno[j]=1000.;
      //fprintf(stderr,"%d %g\n",j,zno[j]);if(j%10==0) getchar();
    }
    fprintf(stderr,"imin=%d imax=%d center=%d\n",imin,imax,center);
    de=0.;dx=0.;
    for(k=imin;k<imax;k++){
      if(k<=center)i=center+imin-k;
      else i=k;
      if(k==center+1){de=0.;dx=0.;}
      if(argc==3) mode=0;
      acc=1.;zampmin2=1e10;
      while(1){
	dx+=acc*inc[mode][0];
	de+=acc*inc[mode][1];
	zamp=0.;
	for(j=100;j<naxes[0]-100;j++){
	  if(abs(j-jmin)<100) continue;
	  //dj=(int)(dx+de*(j-1024)/1024*(j-1024)/1024+1000)-1000;
	  //ddx=dx+de*(j-1024)/1024*(j-1024)/1024-dj;
	  dj=(int)(dx+de*(j-1024)/1024+1000)-1000;
	  ddx=dx+de*(j-1024)/1024-dj;
	  if(zo[i][j+dj]/zav[i]<1e-4 || zo[i][j+dj+1]/zav[i]<1e-4 || zo[center][j]/zav[center]<1e-4 || zo[center+1][j]/zav[center+1]<1e-4) continue;
	  zd=((zo[i][j+dj]*(1-ddx)+zo[i][j+dj+1]*ddx)/zav[i]-(zo[center][j]/zav[center]+zo[center+1][j]/zav[center+1])/2)/zno[j];
	  zamp+=zd*zd/(naxes[0]-200);
	  //fprintf(stderr,"%d %d %f %f %f %f %f %f Amplitude=%g\n",j,dj,zo[i][j+dj],zo[i][j+dj+1],zav[i],(zo[center][j]/zav[center]+zo[center+1][j]/zav[center+1])/2,zno[j],zd,zamp);getchar();
	}
	zamp=sqrt(zamp);
	//fprintf(stderr,"%d %d %g %g Amplitude=%g\n",mode,i,dx,de,zamp);getchar();
	if(mode==5) break;
	else if(mode==0){
	  mode=1;
	  zampmin=zamp;
	}
	else if(zampmin<zamp){
	  dx-=acc*inc[mode][0];
	  de-=acc*inc[mode][1];
	  if(mode==4){
	    if(zampmin2-zampmin>1e-5){mode=0;zampmin2=zampmin;}
	    else if(acc>accmin){acc/=10.;mode=0;}
	  }
	  mode++;
	}else zampmin=zamp;
      }
      xc[i]=dx;ec[i]=de;
      //fprintf(stderr,"%d %g %g Amplitude=%g\n",i,dx,de,zampmin);getchar();
    }
    fit(yc,xc,imin,imax,a);
    fit(yc,ec,imin,imax,b);
    for(i=0;i<naxes[1];i++){
      //fprintf(stderr,"%d %f %f ",i,xc[i],ec[i]);
      xc[i]=0;ec[i]=0;
      for(j=0;j<MAXODR;j++){
	xc[i]+=a[j]*pow(yc[i],(double)j);
	ec[i]+=b[j]*pow(yc[i],(double)j);
      }
      //fprintf(stderr,"=> %f %f\n",xc[i],ec[i]);getchar();
    }
  }
	
  for(i=0;i<naxes[1];i++){
    if(i==0){dxmaxl=xc[i]-ec[i];dxmax=fabs(xc[i])+fabs(ec[i]);}
    else{
      if(dxmaxl<xc[i]-ec[i]) dxmaxl=xc[i]-ec[i];
      if(dxmax<fabs(xc[i])+fabs(ec[i])) dxmax=fabs(xc[i])+fabs(ec[i]);
    }
    //fprintf(stderr,"%f %f %f %f\n",xc[i],ec[i],dxmaxl,dxmax);
  }
	
  for(i=0;i<naxes[1];i++){
    for(j=0;j<LINEPIX;j++) z[i][j]=NOPIX;
    for(j=0;j<naxes[0];j++){
      dj=(int)(xc[i]+ec[i]*(j-1024)/1024+1000)-1000;
      ddx=xc[i]+ec[i]*(j-1024)/1024-dj;
      if(zo[i][j]>NOPIX*0.99){
	if(z[i][j-dj+(int)dxmaxl]<NOPIX*0.99){z[i][j-dj+(int)dxmaxl]=0;bp[i][j-dj+(int)dxmaxl]=0;}
	if(z[i][j-dj+(int)dxmaxl+1]<NOPIX*0.99){z[i][j-dj+(int)dxmaxl+1]=0;bp[i][j-dj+(int)dxmaxl+1]=0;}
	z[i][j-dj+(int)dxmaxl]+=zo[i][j]*ddx;bp[i][j-dj+(int)dxmaxl]+=bpo[i][j]*ddx;
	z[i][j-dj+(int)dxmaxl+1]+=zo[i][j]*(1-ddx);bp[i][j-dj+(int)dxmaxl+1]+=bpo[i][j]*(1-ddx);
      }
    }
    for(j=0;j<naxes[0];j++){
      dj=(int)(xc[i]+ec[i]*(j-1024)/1024+1000)-1000;
      ddx=xc[i]+ec[i]*(j-1024)/1024-dj;
      if(zo[i][j]<NOPIX*0.99){
	z[i][j-dj+(int)dxmaxl]=NOPIX;bp[i][j-dj+(int)dxmaxl]=0.;
	z[i][j-dj+(int)dxmaxl+1]=NOPIX;bp[i][j-dj+(int)dxmaxl+1]=0.;
      }
    }
  }
	
  naxes[0]+=(int)dxmax+1;

  for(j=0;j<naxes[0];j++){
    imin=200;imax=0;
    for(i=0;i<LINEPIY;i++) {
      if(i==0 || i==LINEPIY-1) zc[i]=z[i][j];
      else{
	if((z[i-1][j]<z[i][j] && z[i][j]<z[i+1][j])||(z[i+1][j]<z[i][j] && z[i][j]<z[i-1][j])) zc[i]=z[i][j];
	else if((z[i][j]<z[i-1][j] && z[i-1][j]<z[i+1][j])||(z[i+1][j]<z[i-1][j] && z[i-1][j]<z[i][j])) zc[i]=z[i-1][j];
	else zc[i]=z[i+1][j];
      }
      if(z[i][j]>NOPIX*0.99 && imin>i) imin=i;
      if(z[LINEPIY-i-1][j]>NOPIX*0.99 && imax<LINEPIY-i) imax=LINEPIY-i;
      //if(j==5){fprintf(stderr,"%d %d %d %f %d %d %f\n",j,i,imin,z[i][j],LINEPIY-i-1,imax,z[LINEPIY-i-1][j]);getchar();}
    }
    if(imax-imin-1<50){
      for(i=0;i<LINEPIY;i++) z[i][j]=0.;
    }else{
      if(argc==3 && j==naxes[0]/2){
	for(i=0;i<LINEPIY;i++){
	  printf("%f %f ",xc[i],ec[i]);
	  if(i>=imin && i<imax) printf("1\n");
	  else printf("0\n");
	}
      }
      for(i=imin;i<imax;i++){
	if(z[i][j]<NOPIX*0.99){z[i][j]=0.;bp[i][j]=0.;}
	if(zc[i]<NOPIX*0.99) zc[i]=0.;
	//if(j==1000){fprintf(stderr,"%d %d %d %d %f\n",j,i,imin,imax,z[i][j]);}
      }
      odr=2;
      if(imin>0){
	fit(yc,zc,imin,imin+50,a);
	for(i=0;i<imin;i++){
	  z[i][j]=0;
	  for(k=0;k<odr;k++){
	    z[i][j]+=a[k]*pow(yc[i],(double)k);
	    //if(j==1000) fprintf(stderr,"%d %d %d %f %f %f\n",j,i,k,yc[i],a[k],z[i][j]);
	  }
	}
      }
      if(imax<naxes[1]){
	//if(j==1000){for(i=imax-1-50;i<imax;i++) fprintf(stderr,"%d %d %f %f\n",j,i,yc[i],zc[i]);getchar();}
	fit(yc,zc,imax-1-50,imax-1,a);
	for(i=imax;i<naxes[1];i++){
	  z[i][j]=0;
	  for(k=0;k<odr;k++){
	    z[i][j]+=a[k]*pow(yc[i],(double)k);
	    //if(j==1000) fprintf(stderr,"%d %d %d %f %f %f\n",j,i,k,yc[i],a[k],z[i][j]);
	  }
	}
      }
    }
  }
  
  fprintf(stderr,"Write %s  ... ",argv[2]);
  status=0;
  if (fits_create_file(&fp, argv[2], &status))
    printerror(status);
  if (fits_create_img(fp, bitpix, naxis, naxes, &status))
    printerror(status);
  fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
  fpixel=1;
  nelements=naxes[0];
  for(i=0;i<naxes[1];i++){
    if (fits_write_img(fp, TFLOAT, fpixel, nelements, z[i], &status))
      printerror(status);
    fpixel+=naxes[0];
  }
  if (fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");

  if(bpm){
    strcpy(fname,argv[2]);
      fname[strlen(fname)-5]='\0';
      sprintf(fname,"%s_bpm.fits",fname);
      fprintf(stderr,"Write %s  ... ",fname);
      status=0;
      if (fits_create_file(&fp, fname, &status))
	printerror(status);
      if (fits_create_img(fp, bitpix, naxis, naxes, &status))
	printerror(status);
      fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
      fpixel=1;
      nelements=naxes[0];
      for(i=0;i<naxes[1];i++){
	if (fits_write_img(fp, TFLOAT, fpixel, nelements, bp[i], &status))
	  printerror(status);
	fpixel+=naxes[0];
      }
      if (fits_close_file(fp, &status))
	printerror(status);
      fprintf(stderr,"OK\n");
  }
  return(0);
}

void printerror( int status)
{
    /*****************************************************/
    /* Print out cfitsio error messages and exit program */
    /*****************************************************/

    char status_str[FLEN_STATUS], errmsg[FLEN_ERRMSG];

    if (status)
      fprintf(stderr, "\n*** Error occurred during program execution ***\n");

    fits_get_errstatus(status, status_str);   /* get the error description */
    fprintf(stderr, "\nstatus = %d: %s\n", status, status_str);

    /* get first message; null if stack is empty */
    if ( fits_read_errmsg(errmsg) )    {
         fprintf(stderr, "\nError message stack:\n");
         fprintf(stderr, " %s\n", errmsg);

         while ( fits_read_errmsg(errmsg) )  /* get remaining messages */
             fprintf(stderr, " %s\n", errmsg);
    }

    exit( status );       /* terminate the program, returning error status */
}

void fit(float x[], float y[], int nmin, int nmax, double xx[])
{
 double sx[MAXODR*2],xkai[MAXODR*2],dy,yfit;
 double p[MAXODR][MAXODR],q[MAXODR][MAXODR],xs[MAXODR],yy[MAXODR],ys[MAXODR],fac;
 int    i,j,k,n,ndata;

 ndata=nmax-nmin;
 xkai[0]=1;
 sx[0]=1;
 for(i=1;i<odr*2;i++) sx[i]=0;
 for(i=0;i<odr;i++) yy[i]=0;
 for(n=nmin;n<nmax;n++){
   for(i=1;i<odr*2;i++) {
     xkai[i]=xkai[i-1]*x[n];
     sx[i]+=xkai[i]/ndata;}
   for(i=0;i<odr;i++) yy[i]+=xkai[i]*y[n]/ndata;
 }

 for(j=0;j<odr;j++){
   for(i=0;i<odr;i++){
     p[i][j]=sx[i+j];
     if(i==j) q[i][j]=1;
     else q[i][j]=0;}}

 for(k=0;k<odr;k++){
   for(j=k+1;j<odr;j++){
     fac=p[k][j]/p[k][k];
     for(i=0;i<odr;i++){
       p[i][j]-=fac*p[i][k];
       q[i][j]-=fac*q[i][k];}
   }
 fac=p[k][k];
 for(i=0;i<odr;i++){
   p[i][k]/=fac;
   q[i][k]/=fac;}
 }
 for(k=1;k<odr;k++){
   //fprintf(stderr,"\norder:%d\n",k);
   //fprintf(stderr,"y=");
   for(j=0;j<k+1;j++){
     ys[j]=0;
     for(i=0;i<k+1;i++) ys[j]+=q[i][j]*yy[i];}
   for(j=k;j>=0;j--){
     xs[j]=0;
     for(i=j+1;i<k+1;i++) xs[j]+=p[i][j]*xx[i];
     xx[j]=ys[j]-xs[j];
     //if(j!=k && xx[j]>=0) fprintf(stderr,"+");
     //fprintf(stderr,"%12.9e",xx[j]);
     //if(j>1) fprintf(stderr,"*x**%d",j);
     //else if(j==1) fprintf(stderr,"*x");
     }
   //fprintf(stderr,"\n");
   dy=0;
   for(n=nmin;n<nmax;n++){
     yfit=xx[0];
     xkai[0]=1;
     for(i=1;i<=k;i++){
       xkai[i]=xkai[i-1]*x[n];
       yfit+=xx[i]*xkai[i];}
     dy+=(y[n]-yfit)*(y[n]-yfit)/ndata;}
   //fprintf(stderr,"Order=%d Sigma=%g\n",k,sqrt(dy));
 }
}