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

#define LINEPIX 2100

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, LINEPIX},nbuffer;
  float nullval; 
  static float buffer[LINEPIX];

  static double z[LINEPIX][LINEPIX],zm[LINEPIX][LINEPIX];
  double thre,wt,wtsum,zsum,z2sum,zav,zsg;
  int dj,n,grow;
  int keys=1,rj[LINEPIX];
  char ctype1[16],cunit1[16],ctype2[16],cunit2[16];
  float crpix1,crval1,cd1_1,crpix2,crval2,cd2_2;

  if(argc!=5 && argc!=6){
    printf("Usage: %s input lower maskimage output [grow] (if threshold < 1)\n",argv[0]);
    printf("  or   %s input upper noiseimage output [grow] (if threshold > 1)\n",argv[0]);
    exit(1);
  }
  sscanf(argv[2],"%lf",&thre);
  if(argc==6) sscanf(argv[5],"%d",&grow);
  else grow=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);
  if(fits_read_key(fp, TSTRING, "CTYPE1", ctype1, NULL, &status)) keys=0;
  if(keys){if(fits_read_key(fp, TSTRING, "CUNIT1", cunit1, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TFLOAT, "CRPIX1", &crpix1, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TFLOAT, "CRVAL1", &crval1, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TFLOAT, "CD1_1", &cd1_1, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TSTRING, "CTYPE2", ctype2, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TSTRING, "CUNIT2", cunit2, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TFLOAT, "CRPIX2", &crpix2, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TFLOAT, "CRVAL2", &crval2, NULL, &status)) keys=0;}
  if(keys){if(fits_read_key(fp, TFLOAT, "CD2_2", &cd2_2, NULL, &status)) keys=0;}
  status=0;
  fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
  if((naxes[0]>=LINEPIX)||(naxes[1]>=LINEPIX)){
    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,"Read %s  ... ",argv[3]);
  status=0;
  if(fits_open_file(&fp, argv[3], 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]>=LINEPIX)){
    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++) zm[i][j]=buffer[j];
    fpixel+=naxes[0];
  }
  if(fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");

  fprintf(stderr,"Write %s  ... ",argv[4]);
  status=0;
  if (fits_create_file(&fp, argv[4], &status))
    printerror(status);
  if (fits_create_img(fp, bitpix, naxis, naxes, &status))
    printerror(status);
  fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
  if(keys){
    if (fits_write_key(fp, TSTRING, "CTYPE1", ctype1, "", &status))
      printerror(status);
    if (fits_write_key(fp, TSTRING, "CUNIT1", cunit1, "", &status))
      printerror(status);
    if (fits_write_key(fp, TFLOAT, "CRPIX1", &crpix1, "", &status))
      printerror(status);
    if (fits_write_key(fp, TFLOAT, "CRVAL1", &crval1, "", &status))
      printerror(status);
    if (fits_write_key(fp, TFLOAT, "CD1_1", &cd1_1, "", &status))
      printerror(status);
    if (fits_write_key(fp, TSTRING, "CTYPE2", ctype2, "", &status))
      printerror(status);
    if (fits_write_key(fp, TSTRING, "CUNIT2", cunit2, "", &status))
      printerror(status);
    if (fits_write_key(fp, TFLOAT, "CRPIX2", &crpix2, "", &status))
      printerror(status);
    if (fits_write_key(fp, TFLOAT, "CRVAL2", &crval2, "", &status))
      printerror(status);
    if (fits_write_key(fp, TFLOAT, "CD2_2", &cd2_2, "", &status))
      printerror(status);
  }
  fpixel=1;
  nelements=naxes[0];
  for(i=0;i<naxes[1];i++){
    if(thre>1){
      zav=0;zsg=10000;
      for(dj=0;dj<20;dj++){
	zsum=0;z2sum=0;n=0;
	for(j=0;j<naxes[0];j++){
	  if(fabs(zm[i][j]-zav)<3*zsg && zm[i][j]>1e-5){
	    zsum+=zm[i][j];
	    z2sum+=zm[i][j]*zm[i][j];
	    n++;
	  }
	}
	zav=zsum/n;
	zsg=sqrt(z2sum/n-zav*zav);
	//fprintf(stderr,"%d %d %d %g %g\n",i,dj,n,zav,zsg);
      }//getchar();
    }
    for(j=0;j<naxes[0];j++) rj[j]=0;
    for(j=0;j<naxes[0];j++){
      if((thre<1 && zm[i][j]<thre)||(thre>1 && zm[i][j]>zav*thre)){
	for(dj=-grow;dj<=grow;dj++){
	  if(j+dj>=0 && j+dj<naxes[0]) rj[j+dj]=1;
	}
      }
    }
    for(j=0;j<naxes[0];j++){
      if(rj[j]){
	//fprintf(stderr,"%d %d %g ",i,j,zm[i][j]);
	wtsum=0;zsum=0;n=0;dj=0;
	while(j-dj>0 && n<10+grow){
	  dj++;
	  if(rj[j-dj]) continue;
	  wt=1./dj;
	  wtsum+=wt;
	  zsum+=wt*z[i][j-dj];
	  n++;
	  //fprintf(stderr,"(%d)%g ",-dj,z[i][j-dj]);
	}
	n=0;dj=0;
	while(j+dj<naxes[0]-1 && n<10+grow){
	  dj++;
	  if(rj[j+dj]) continue;
	  wt=1./dj;
	  wtsum+=wt;
	  zsum+=wt*z[i][j+dj];
	  n++;
	  //fprintf(stderr,"(%d)%g ",dj,z[i][j+dj]);
	}
	buffer[j]=zsum/wtsum;
	//fprintf(stderr,"%g ",buffer[j]);getchar();
      }else{
	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 */
}