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

#define LINEPIX 2100
#define LINEPIY 200
#define MAXMODE 25
#define MAXODR 5

void fit();
void printerror( int status);

int main(int argc,char *argv[])
{
  fitsfile *fp;
  int i,j,status,nfound,anynull,bitpix=-32;
  long fpixel, nelements,naxis=2,npixels;
  long naxes[2]={LINEPIX, LINEPIX},nbuffer;
  float nullval; 
  static float buffer[LINEPIX];

  FILE *fpd;
  char buf[1024];
  int n,nmax,k,mode,imax,ii,iimax,nloop;
  double lmp[2][1600],wlc,dw,dw2,dw3,wl1,wl2,wl0c,dw0,dw02,dw03,lev=10000.;
  static double z[LINEPIY][LINEPIX],p[LINEPIY][LINEPIX],p0[LINEPIX];
  float w[9]={0.07,0.25,0.6,1,1,1,0.6,0.25,0.07},dwx[4][LINEPIY],yc[LINEPIY];
  //float w2[29];
  double zamp,zampmax,zampmax2,zamp0,acc,accmin,step,a[MAXODR];
  int inc[MAXMODE+2][4]={{0,0,0,0},{-1,0,0,0},{1,0,0,0},{0,-1,0,0},{0,1,0,0},{-1,1,0,0},{1,-1,0,0},{1,1,0,0},{-1,-1,0,0},{0,0,-1,0},{0,0,1,0},{0,-1,1,0},{0,1,-1,0},{0,1,1,0},{0,-1,-1,0},{0,0,0,-1},{0,0,0,1},{0,1,0,-1},{0,-1,0,1},{0,1,0,1},{0,-1,0,-1},{0,0,1,-1},{0,0,-1,1},{0,0,1,1},{0,0,-1,-1},{0,0,0,0},{0,0,0,0}};

  if(argc!=7 && argc!=8 && argc!=9){
    printf("Usage: %s CAL_1Dx200 WL_map linelist step wlc dw [dw2 [dw3]]\n",argv[0]);
    exit(1);
    }

  if((fpd = fopen(argv[3],"r")) == NULL){
    printf("Cannot open %s\n",argv[3]);
    exit(0);
  }
  n=0;
  while(!feof(fpd)){
    if(fgets(buf,sizeof(buf),fpd)==NULL) break;
    if(strncmp(buf,"#",1)==0){
      if(strcmp(buf,"#ThAr\n")==0) lev=10000.;
      else if(strcmp(buf,"#HgI\n")==0) lev=500.;
      else if(strcmp(buf,"#HgII\n")==0) lev=15000.;
      else if(strcmp(buf,"#KrI\n")==0) lev=100.;
      else if(strcmp(buf,"#NeI\n")==0) lev=10000.;
      else if(strcmp(buf,"#NeII\n")==0) lev=150.;
      //fprintf(stderr,"lev=%g\n",lev);getchar();
      continue;
    }
    sscanf(buf,"%lf  %lf",&lmp[0][n],&lmp[1][n]);
    lmp[0][n]/=1000.;
    if(lmp[1][n]<lev) continue;
    else lmp[1][n]=1.;
    //fprintf(stderr,"%lf  %lf\n",lmp[0][n],lmp[1][n]);
    n++;
  }
  //getchar();
  fclose(fpd);
  nmax=n;
  sscanf(argv[4],"%lf",&step);
  sscanf(argv[5],"%lf",&wlc);
  sscanf(argv[6],"%lf",&dw);
  if(argc>7) sscanf(argv[7],"%lf",&dw2);
  else dw2=0.;
  if(argc>8) sscanf(argv[8],"%lf",&dw3);
  else dw3=0.;
  //for(k=0;k<29;k++){
  //  if(k<2) w2[k]=w[0]*((k+1)%3)/9;
  //  else if(k>25) w2[k]=w[8]*(3-(k+1)%3)/9;
  //  else w2[k]=w[(k-2)/3]*(3-(k+1)%3)/9+w[(k+1)/3]*((k+1)%3)/9;
    //fprintf(stderr,"%2d %5.3f\n",k,w2[k]);
  //}

  fprintf(stderr,"Read %s  ... ",argv[1]);
  npixels=naxes[0]*naxes[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];
  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++) z[i][j]=buffer[j];
    fpixel+=naxes[0];
  }
  if(fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");

  fprintf(stderr,"Search initial (wcen,dw) pair from (%g,%g) to (%g,%g)",wlc-dw*step*50,dw-dw*step*40,wlc+dw*step*50,dw+dw*step*40);
  zampmax=0;
  for(i=-100;i<=100;i++){
  for(ii=-40;ii<=40;ii++){
    p0[0]=0.;
    for(j=1;j<naxes[0];j++){
      p0[j]=0.;
      wl1=(double)(j-1)*2/naxes[0]-1;
      wl1=(wlc+dw*step*0.5*i)+(dw+dw*step*ii)*wl1+dw2*wl1*wl1+dw3*wl1*wl1*wl1;
      wl2=(double)j*2/naxes[0]-1;
      wl2=(wlc+dw*step*0.5*i)+(dw+dw*step*ii)*wl2+dw2*wl2*wl2+dw3*wl2*wl2*wl2;
      for(n=0;n<nmax;n++){
	if(wl1<=lmp[0][n] && lmp[0][n]<wl2){
	  //p0[j-1]+=lmp[1][n]*(wl2-lmp[0][n])/(wl2-wl1);
	  //p0[j]+=lmp[1][n]*(lmp[0][n]-wl1)/(wl2-wl1);
	  p0[j-1]+=(wl2-lmp[0][n])/(wl2-wl1);
	  p0[j]+=(lmp[0][n]-wl1)/(wl2-wl1);
	}
      }
    }
    for(j=0;j<naxes[0];j++) p[99][j]=0.;
    for(j=0;j<naxes[0];j++){
      for(k=-4;k<5;k++){
	if(j+k>=0 && j+k<naxes[0]) p[99][j+k]+=p0[j]*w[k+4];
      }
    }
    zamp=0.;
    for(j=50;j<naxes[0]-50;j++){
      if(p[99][j]>1.) p[99][j]=1.;
      zamp+=z[99][j]*p[99][j];
    }
    if(zampmax<zamp){
      zampmax=zamp;
      imax=i;
      iimax=ii;
      //fprintf(stderr,"%d %d %d %d %g %g Amplitude=%g\n",i,imax,ii,iimax,wlc+dw*step*0.5*i,dw+dw*step*ii,zamp);
      //getchar();
    }
  }if(i%10==0)fprintf(stderr,".");}fprintf(stderr,"\n");
  wlc+=dw*step*0.5*imax;
  dw+=dw*step*iimax;
  zamp0=zampmax;

  for(ii=0;ii<naxes[1];ii++){
    if(ii<naxes[1]/2) i=naxes[1]/2-1-ii;
    else i=ii;
    if(i==naxes[1]/2){
      wlc=wl0c;
      dw=dw0;
      dw2=dw02;
      dw3=dw03;
    }
    acc=step;accmin=1e-7;nloop=0;
    if(step<1e-8) mode=MAXMODE+1; else mode=0;
    while(1){
      wlc+=acc*inc[mode][0];
      dw +=acc*inc[mode][1];
      dw2+=acc*inc[mode][2];
      dw3+=acc*inc[mode][3];
      if(mode!=MAXMODE){
	inc[MAXMODE][0]+=inc[mode][0];
	inc[MAXMODE][1]+=inc[mode][1];
	inc[MAXMODE][2]+=inc[mode][2];
	inc[MAXMODE][3]+=inc[mode][3];
      }

      p0[0]=0.;
      for(j=1;j<naxes[0];j++){
	p0[j]=0.;
	wl1=(double)(j-1)*2/naxes[0]-1;
	wl1=wlc+dw*wl1+dw2*wl1*wl1+dw3*wl1*wl1*wl1;
	wl2=(double)j*2/naxes[0]-1;
	wl2=wlc+dw*wl2+dw2*wl2*wl2+dw3*wl2*wl2*wl2;
	for(n=0;n<nmax;n++){
	  if(wl1<=lmp[0][n] && lmp[0][n]<wl2){
	    //p0[j-1]+=lmp[1][n]*(wl2-lmp[0][n])/(wl2-wl1);
	    //p0[j]+=lmp[1][n]*(lmp[0][n]-wl1)/(wl2-wl1);
	    p0[j-1]+=(wl2-lmp[0][n])/(wl2-wl1);
	    p0[j]+=(lmp[0][n]-wl1)/(wl2-wl1);
	  }
	}
      }
      for(j=0;j<naxes[0];j++) p[i][j]=0.;
      for(j=0;j<naxes[0];j++){
	for(k=-4;k<5;k++){
	  if(j+k>=0 && j+k<naxes[0]) p[i][j+k]+=p0[j]*w[k+4];
	}
      }
      zamp=0.;
      for(j=50;j<naxes[0]-50;j++){
	if(p[i][j]>1.) p[i][j]=1.;
	zamp+=z[i][j]*p[i][j];
      }
      //if(i==41) fprintf(stderr,"%d %d %g %g %g %g %g Amplitude=%g(%g)\n",mode,nloop,acc,wlc,dw,dw2,dw3,zamp,zamp-zampmax);

      if(mode==MAXMODE+1){zamp0=zamp;break;}
      else if(mode==0){
	if(zamp<zamp0/10) break;
	mode=1;
	zampmax=zamp;
	zampmax2=zampmax;
	inc[MAXMODE][0]=0;
	inc[MAXMODE][1]=0;
	inc[MAXMODE][2]=0;
	inc[MAXMODE][3]=0;
      }else if(zamp-zampmax<1e-12){
	wlc-=acc*inc[mode][0];
	dw -=acc*inc[mode][1];
	dw2-=acc*inc[mode][2];
	dw3-=acc*inc[mode][3];
	if(mode==MAXMODE){
	  if(zampmax-zampmax2>1e-12){mode=0;zampmax2=zampmax;}
	  else if(acc>accmin){acc/=10.;mode=0;
	    //fprintf(stderr," %g",acc);
	  }
	  //if(i==41){fprintf(stderr,"%d/%d/%d/%d",inc[MAXMODE][0],inc[MAXMODE][1],inc[MAXMODE][2],inc[MAXMODE][3]);getchar();}
	  inc[MAXMODE][0]=0;
	  inc[MAXMODE][1]=0;
	  inc[MAXMODE][2]=0;
	  inc[MAXMODE][3]=0;
	}else{
	  inc[MAXMODE][0]-=inc[mode][0];
	  inc[MAXMODE][1]-=inc[mode][1];
	  inc[MAXMODE][2]-=inc[mode][2];
	  inc[MAXMODE][3]-=inc[mode][3];
	}
	mode++;
	nloop=0;
      }else{
	zampmax=zamp;
	if(nloop++>10){acc*=10.;nloop=0;
	  //fprintf(stderr," %g",acc);
	}
      }
    }
    fprintf(stderr,"%d %g %g %g %g Amplitude=%g\n",i,wlc,dw,dw2,dw3,zamp);
    dwx[0][i]=wlc;
    dwx[1][i]=dw;
    dwx[2][i]=dw2;
    dwx[3][i]=dw3;
    for(j=0;j<naxes[0];j++){
      wl2=(double)j*2/naxes[0]-1;
      z[i][j]=wlc+dw*wl2+dw2*wl2*wl2+dw3*wl2*wl2*wl2;
    }
    //getchar();
    if(ii==0){
      wl0c=wlc;
      dw0=dw;
      dw02=dw2;
      dw03=dw3;
    }
  }

  for(i=0;i<naxes[1];i++) yc[i]=(float)i;
  for(k=1;k<4;k++){
    fit(yc,dwx[k],0,200,a);
    for(i=0;i<naxes[1];i++){
      dwx[k][i]=0.;
      for(j=0;j<MAXODR;j++) dwx[k][i]+=a[j]*pow(yc[i],(double)j);
    }
  }
  for(i=0;i<naxes[1];i++){
    for(j=0;j<naxes[0];j++){
      wl2=(double)j*2/naxes[0]-1;
      z[i][j]=dwx[0][i]+dwx[1][i]*wl2+dwx[2][i]*wl2*wl2+dwx[3][i]*wl2*wl2*wl2;
      //fprintf(stderr,"%d %d %g %g %g %g %g %g\n",i,j,wl2,dwx[0][i],dwx[1][i],dwx[2][i],dwx[3][i],z[i][j]);
      if(j==0) p0[0]=0.;
      else{
	p0[j]=0.;
	wl1=z[i][j-1];
	wl2=z[i][j];
	for(n=0;n<nmax;n++){
	  if(wl1<=lmp[0][n] && lmp[0][n]<wl2){
	    //p0[j-1]+=lmp[1][n]*(wl2-lmp[0][n])/(wl2-wl1);
	    //p0[j]+=lmp[1][n]*(lmp[0][n]-wl1)/(wl2-wl1);
	    p0[j-1]+=(wl2-lmp[0][n])/(wl2-wl1);
	    p0[j]+=(lmp[0][n]-wl1)/(wl2-wl1);
	    //fprintf(stderr,"%d %d %d %g %g %g %g %g\n",i,j,n,wl1,lmp[0][n],wl2,p0[j-1],p0[j]);getchar();
	  }
	}
      }
      p[i][j]=0.;
    }
    for(j=0;j<naxes[0];j++){
      for(k=-4;k<5;k++){
	if(j+k>=0 && j+k<naxes[0]) p[i][j+k]+=p0[j]*w[k+4];
      }
    }
  }

  fprintf(stderr,"\nWrite check.fits ... ");
  status=0;
  if (fits_create_file(&fp, "check.fits", &status))
    printerror(status);
  if (fits_create_img(fp, bitpix, naxis, naxes, &status))
    printerror(status);
  fpixel=1;
  fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
  nelements=naxes[0];
  for(i=0;i<naxes[1];i++){
    for(j=0;j<naxes[0];j++) buffer[j]=p[i][j];
    if(fits_write_img(fp, TFLOAT, fpixel, nelements, buffer, &status))
      printerror(status);
    fpixel+=naxes[0];
  }
  if (fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");

  fprintf(stderr,"\nWrite %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);
  fpixel=1;
  fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
  nelements=naxes[0];
  for(i=0;i<naxes[1];i++){
    for(j=0;j<naxes[0];j++) buffer[j]=z[i][j];
    if(fits_write_img(fp, TFLOAT, fpixel, nelements, buffer, &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<MAXODR*2;i++) sx[i]=0;
  for(i=0;i<MAXODR;i++) yy[i]=0;
  for(n=nmin;n<nmax;n++){
    for(i=1;i<MAXODR*2;i++) {
      xkai[i]=xkai[i-1]*x[n];
      sx[i]+=xkai[i]/ndata;}
    for(i=0;i<MAXODR;i++) yy[i]+=xkai[i]*y[n]/ndata;
  }

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

  for(k=0;k<MAXODR;k++){
    for(j=k+1;j<MAXODR;j++){
      fac=p[k][j]/p[k][k];
      for(i=0;i<MAXODR;i++){
	p[i][j]-=fac*p[i][k];
	q[i][j]-=fac*q[i][k];}
    }
    fac=p[k][k];
    for(i=0;i<MAXODR;i++){
      p[i][k]/=fac;
      q[i][k]/=fac;}
  }
  for(k=1;k<MAXODR;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));
  }
}