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

#define LINEPIX 2100
#define LINEPIY 2200
#define NOPIX  -1e10

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;
  long naxes[2]={LINEPIX, LINEPIY},nbuffer;
  float nullval;
  static float buffer[LINEPIX],zo[LINEPIY][LINEPIX],z[LINEPIY][LINEPIX];

  int k,dj,di,dimax,dyi,kmax,is,ie,ndat;
  float dyld,dylu,dyrd,dyru,dycd,dycu,dy,dyl,dyr,dyd[2],dyu[2],dya[2],dyb[2],dyc,dx,ys[2];
  float xc[200],ec[200],yc[2][200],sc,ddx,dxmaxl,dxmax,wt,zm[1024],zmav,zmsg,thre,zsum,z2sum;
  float w[9]={0.06,0.21,0.5,0.84,1,0.84,0.5,0.21,0.06};
  char buf[128],fname[64];
  FILE *fpd;

  if(argc!=6 && argc!=7){
    fprintf(stderr,"Usage : %s input output threshold align_param_file extract_param_file [extract_param_file2]\n",argv[0]);
    fprintf(stderr,"           threshold < 0 means pick up mode.\n");
    exit(0);
  }

  sscanf(argv[3],"%f",&thre);
  
  if((fpd=fopen(argv[4],"r"))==NULL){
    fprintf(stderr,"Can not open file: %s\n",argv[4]);
    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);

  kmax=argc-5;
  for(k=0;k<kmax;k++){
    sprintf(fname,"extract_%s",argv[5+k]);
    if((fpd=fopen(fname,"r"))==NULL){
      fprintf(stderr,"Can not open file: %s\n",argv[5+k]);
      fprintf(stderr,"  Using y-shift=0\n");
      ys[k]=0.;
    }else{
      fscanf(fpd,"%f",&ys[k]);
      fprintf(stderr,"y-shift=%g\n",ys[k]);
      fclose(fpd);
    }
    if((fpd=fopen(argv[5+k],"r"))==NULL){
      fprintf(stderr,"Can not open file: %s\n",argv[5+k]);
      exit(1);
    }
    fgets(buf,sizeof(buf),fpd);
    if(sscanf(buf,"%f %f %f %f",&dyd[k],&dyu[k],&dya[k],&dyb[k])!=4){
      fprintf(stderr,"Wrong format\n");
      exit(1);
    }
    while(!feof(fpd)){
      if(fgets(buf,sizeof(buf),fpd)==NULL) break;
      if(sscanf(buf,"%d %f",&i,&sc)!=2){
	fprintf(stderr,"Wrong format\n");
	exit(1);
      }
      yc[k][i]=sc+ys[k];
    }
    fclose(fpd);
  }
  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){
    fprintf(stderr,"\nNumber of columns must be < %d\n",LINEPIX);
    exit(1);
  }
  if(naxes[1]!=1800&&naxes[1]!=200){
    fprintf(stderr,"\nNumber of lines must be 1800 or 200\n");
    exit(1);
  }
  if(naxes[1]==1800){
    w[0]=1;
    dimax=1;
  }else{
    /*
    if((fpd=fopen("extract.dat","r"))==NULL){
      fprintf(stderr,"Can not open file: extract.dat\n");
      exit(1);
    }
    if(fgets(buf,sizeof(buf),fpd)==NULL){
      fprintf(stderr,"extract.dat is empty file!\n");
      exit(1);
    }
    if(sscanf(buf,"%f %f %f %f %f %f %f %f %f",&w[0],&w[1],&w[2],&w[3],&w[4],&w[5],&w[6],&w[7],&w[8])!=9){
      fprintf(stderr,"Wrong format in extract.dat (%s)\n",buf);
      exit(1);
    }
    fclose(fpd);
    */
    dimax=9;
    wt=0;
    for(di=0;di<dimax;di++) wt+=w[di];
    if(wt<1e-5) wt=1;
    for(di=0;di<dimax;di++) w[di]/=wt;
  }
  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++){
      for(di=0;di<dimax;di++) z[i*dimax+di][j]=buffer[j]*w[di];
    }
    fpixel+=naxes[0];
  }
  if(fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");

  for(i=0;i<200;i++){
    zm[i]=0;
    for(di=0;di<9;di++){
      for(j=0;j<naxes[0];j++) zm[i]+=z[i*9+di][j];
    }
  }
  zmav=0.;zmsg=1e10;
  for(k=0;k<5;k++){
    zsum=0.;z2sum=0.;ndat=0;
    for(i=0;i<200;i++){
      if(fabs(zm[i]-zmav)<zmsg*5){
	zsum+=zm[i];
	z2sum+=zm[i]*zm[i];
	ndat++;
      }
    }
    zmav=zsum/ndat;
    zmsg=sqrt(z2sum/ndat-zmav*zmav);
    //fprintf(stderr,"Loop=%d Average=%g Sigma=%g\n",k+1,zmav,zmsg);
  }
  fprintf(stderr,"Average=%g Sigma=%g\n",zmav,zmsg);
  for(i=0;i<200;i++){
    if(thre>0){
      if(fabs(zm[i]-zmav)>zmsg*thre){
	fprintf(stderr,"Object #%d(%g) is rejected.\n",i,zm[i]);
	for(di=0;di<9;di++){
	  for(j=0;j<naxes[0];j++) z[i*9+di][j]=NOPIX;
	}
      }
    }else{
      if(zm[i]-zmav<-zmsg*thre){
	for(di=0;di<9;di++){
	  for(j=0;j<naxes[0];j++) z[i*9+di][j]=0;
	}
      }
    }
  }

  for(i=0;i<200;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<200;i++){
    for(j=0;j<2048;j++){
      dj=(int)(xc[i]+ec[i]*(j-1024)/1024+1000)-1000;
      ddx=xc[i]+ec[i]*(j-1024)/1024-dj;
      if(j-dj+(int)dxmaxl>=0 && j-dj+(int)dxmaxl+1<naxes[0]){
	for(di=0;di<9;di++) zo[i*9+di][j]=z[i*9+di][j-dj+(int)dxmaxl]*ddx+z[i*9+di][j-dj+(int)dxmaxl+1]*(1-ddx);
      }
    }
  }

  naxes[0]=2048/kmax;
  naxes[1]=2048;
  for(k=0;k<kmax;k++){
    dyrd= dyd[k];dyru= dyu[k];
    dyld=-dyd[k];dylu=-dyu[k];
    dycd= dya[k];dycu= dyb[k];
    dy = dyld;
    if(dyrd < dy) dy = dyrd;
    dy=floor(dy);
    if(dycd<0)dy+=dycd;
    dyld-=dy;
    dylu-=dy;
    dyrd-=dy;
    dyru-=dy;
    for(i=0;i<LINEPIY;i++){
      for(j=0;j<naxes[0];j++) z[i][k*1024+j]=NOPIX;
    }
    for(j=0;j<naxes[0];j++){
      for(i=0;i<200;i++){
	dy=yc[k][i]-(i*9+4);
	dyi=(int)dy;
	for(di=0;di<9;di++){
	  if(i*9+di+dyi>=0 && i*9+di+dyi+1<LINEPIY && zo[i*9+di][k*1024+j]>NOPIX*0.99 ){
	    if(z[i*9+di+dyi][k*1024+j]<NOPIX*0.99) z[i*9+di+dyi][k*1024+j]=0.;
	    if(z[i*9+di+dyi+1][k*1024+j]<NOPIX*0.99) z[i*9+di+dyi+1][k*1024+j]=0.;
	    z[i*9+di+dyi][k*1024+j]+=zo[i*9+di][k*1024+j]*(1-(dy-dyi));
	    z[i*9+di+dyi+1][k*1024+j]+=zo[i*9+di][k*1024+j]*(dy-dyi);
	  }
	}
      }
    }
    for(i=0;i<LINEPIY;i++){
      for(j=0;j<naxes[0];j++) zo[i][k*1024+j]=NOPIX;
    }
    for(i=0;i<naxes[1];i++){
      dx=(float)i/(naxes[1]-1);
      dyl=(1-dx)*dyld+dx*dylu;
      dyr=(1-dx)*dyrd+dx*dyru;
      dyc=(1-dx)*dycd+dx*dycu;
      for(j=0;j<naxes[0];j++){
        dx=(float)j/(naxes[0]-1);
        dy=(1-dx)*dyl+dx*dyr+4*dyc*(dx-0.5)*(dx-0.5);
        dyi=(int)dy;
        if(dy<0) dyi--;
	if(i+dyi>=0 && i+dyi+1<LINEPIY){
	  if(z[i+dyi][k*1024+j]<NOPIX*0.99 || z[i+dyi+1][k*1024+j]<NOPIX*0.99){
	    if(dy-dyi>0.5) dy=dyi+1;
	    else dy=dyi;
	  }
	  zo[i][k*1024+j]=z[i+dyi][k*1024+j]*(1-(dy-dyi))+z[i+dyi+1][k*1024+j]*(dy-dyi);
	}
      }
    }
  }
  naxes[0]=2048;

  for(i=0;i<naxes[1]/2;i++){
    for(j=0;j<naxes[0]/2;j++){
      if(zo[i][j+naxes[0]/2]<NOPIX*0.99) z[i][j]=zo[naxes[1]-i-1][naxes[0]/2-j-1];
      else if(zo[naxes[1]-i-1][naxes[0]/2-j-1]<NOPIX*0.99) z[i][j]=zo[i][j+naxes[0]/2];
      else z[i][j]=(zo[i][j+naxes[0]/2]+zo[naxes[1]-i-1][naxes[0]/2-j-1])/2;
    }
    zm[i]=0;
    is=0;
    for(j=0;j<naxes[0]/2;j++){
      if(z[i][j]>NOPIX*0.99){zm[i]+=z[i][j];is++;}
    }
    if(is!=0) zm[i]/=is;
    else zm[i]=NOPIX;
  }
  is=-99;ie=0;
  for(i=0;i<naxes[1]/2;i++){
    if(zm[i]<NOPIX*0.99 && is==-99){is=i-1;ie=-99;}
    if((zm[i]>NOPIX*0.99 || i==naxes[1]/2-1) && ie==-99){
      ie=i;
      if(ie==naxes[1]/2-1 && is==-1){
	fprintf(stderr,"All the pixels have NO data!\n");
	exit(1);
      }
      else if(is==-1){
	is=0;zm[is]=zm[ie];
      }
      else if(ie==naxes[1]/2-1) zm[ie]=zm[is];
      for(di=is+1;di<ie;di++){
	//fprintf(stderr,"%d %d %d %g",di,is,ie,zm[di]);
	zm[di]=zm[is]*(ie-di)/(ie-is)+zm[ie]*(di-is)/(ie-is);
	//fprintf(stderr," %g %g %g\n",zm[di],zm[is],zm[ie]);
      }
      is=-99;
    }
    //if(ie!=-99)fprintf(stderr,"%d %d %d %g\n",i,is,ie,zm[i]);if(i%30==0)getchar();
  }
  for(i=0;i<naxes[1]/2;i++){
    for(j=0;j<naxes[0]/2;j++){
      z[naxes[1]/2-j-1][i]=zm[i];
      z[i][j+naxes[0]/2]=zm[i];
      z[naxes[1]-i-1][naxes[0]/2-j-1]=zm[i];
      z[j+naxes[1]/2][naxes[0]-i-1]=zm[i];
    }
  }

  naxes[0]=2048;
  naxes[1]=2048;

  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(thre<0){
      for(j=0;j<naxes[0];j++){
	if(zo[i][j]<NOPIX*0.99) zo[i][j]=0.;
      }
    }
    if (fits_write_img(fp, TFLOAT, fpixel, nelements, zo[i], &status))
      printerror(status);
    fpixel+=naxes[0];
  }
  if (fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");

  fprintf(stderr,"Write 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);
  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");
  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 */
}