#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;
  long fpixel;
  long naxes[2]={LINEPIX, LINEPIX},nbuffer;
  float nullval; 
  static float buffer[LINEPIX];
  static int rj[LINEPIX][LINEPIX];
  FILE *fpd;
  int keys=1,grow,dj;
  char ctype1[16],cunit1[16],ctype2[16],cunit2[16];
  float crpix1,crval1,cd1_1,crpix2,crval2,cd2_2;

  int n;
  double zav,zsg,zsum,z2sum,sx2,sx,sxy,sy,a,b;
  static double z[LINEPIX][LINEPIX],zm[LINEPIX][LINEPIX],wl[LINEPIX];
  float thre;

  fprintf(stderr,"OK\n");

  if(argc!=2 && argc!=3 && argc!=4 && argc!=5){
    printf("Usage: %s input [lower] [maskimage] [grow] (if threshold < 1)\n",argv[0]);
    printf("  or   %s input [upper] [noiseimage] [grow] (if threshold > 1)\n",argv[0]);
    exit(1);
  }
  if(argc>=3) sscanf(argv[2],"%f",&thre);
  else thre=-1e99;
  if(argc==5) sscanf(argv[4],"%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;}
  if(keys){
    for(j=0;j<naxes[0];j++) wl[j]=(crval1+(j+1-crpix1)*cd1_1)/10000;
  }else{
    for(j=0;j<naxes[0];j++) wl[j]=j;
  }
  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");

  if(argc>=4){
    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");

    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[i][j]=0;
      for(j=0;j<naxes[0];j++){
	if((thre<1 && zm[i][j]<thre)||(thre>1 && (zm[i][j]>zav*thre||zm[i][j]<1e-5))){
	  for(dj=-grow;dj<=grow;dj++){
	    if(j+dj>=0 && j+dj<naxes[0]) rj[i][j+dj]=1;
	  }
	}
      }
    }
  }

  for(j=0;j<naxes[0];j++){
    printf("%g ",wl[j]);
    for(i=0;i<naxes[1];i++){
      if(argc>=4){
	if(rj[i][j]==0) printf("%g ",z[i][j]);
	else printf("-9999. ");
      }else{
        if((thre<1 && z[i][j]>thre)||(thre>1 && z[i][j]<thre)) printf("%g ",z[i][j]);
        else printf("%g ",thre);
      }
    }
    printf("\n");
  }

  if(1){
    if((fpd=fopen("check.txt","w"))==NULL){
      fprintf(stderr,"Can not open file: check.txt\n");
      exit(1);
    }
    for(i=0;i<naxes[1];i++){
      n=0;sx2=0.;sx=0.,sxy=0.;sy=0.;
      for(j=0;j<naxes[0];j++){
	if((argc>=4 && rj[i][j]==0)||(argc<4&&((thre<1 && z[i][j]>thre)||(thre>1 && z[i][j]<thre)))){
	  sx2+=wl[j]*wl[j];
	  sx+=wl[j];
	  sxy+=wl[j]*z[i][j];
	  sy+=z[i][j];
	  n++;
	  //if(i==69){fprintf(stderr,"%d %d %g %g\n",i,j,wl[j],z[i][j]);getchar();}
	}
      }
      a=(sxy*n-sx*sy)/(sx2*n-sx*sx);
      b=(sx2*sy-sxy*sx)/(sx2*n-sx*sx);
      //fprintf(stderr,"%d a=%g b=%g (%g %g %g %g)\n",i+1,a,b,sx2/n,sx/n,sxy/n,sy/n);
      n=0;zav=0;
      for(j=0;j<naxes[0];j++){
	if(1.15<=wl[j] && wl[j]<1.3){
	  if((argc>=4 && rj[i][j]==0)||(argc<4&&((thre<1 && z[i][j]>thre)||(thre>1 && z[i][j]<thre)))){
	    zav+=z[i][j];
	    n++;
	  }
	}
      }
      if(n!=0) zav/=n;
      else zav=a*1.225+b;
      fprintf(fpd,"%d\t%g ",i+1, zav);
      n=0;zav=0;
      for(j=0;j<naxes[0];j++){
	if(1.575<=wl[j] && wl[j]<1.725){
	  if((argc>=4 && rj[i][j]==0)||(argc<4&&((thre<1 && z[i][j]>thre)||(thre>1 && z[i][j]<thre)))){
	    zav+=z[i][j];
	    n++;
	  }
	}
      }
      if(n!=0) zav/=n;
      else zav=a*1.65+b;
      fprintf(fpd,"%g\n",zav);
    }
    fclose(fpd);
  }
  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 */
}