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

#define LINEPIX 2048
#define NPIX 4194304
#define SATULATE 5000
#define MARK_SATULATE -2000000000
#define MARK_BAD      -2100000000
#define BAD_PIXEL     0
#define COSMIC_RAY    100
#define ZSHIFT        1000000

void printerror( int status);

int main(int argc,char *argv[])
{
  fitsfile *fp;
  int i,status,bitpix=-32;
  long fpixel,naxis=2;
  long naxes[2]={LINEPIX, LINEPIX};
  static unsigned short buffer[NPIX],buffers[NPIX];
  static int zs[NPIX],xzs[NPIX],cr[NPIX];
  int ihdu,sx,sx2,sigma,zex,zre,ncr;
  double a,b;
  FILE *fpa,*fpb;
  static int map[NPIX];

  if(argc!=4){
    printf("Usage: %s input input_sky output \n",argv[0]);
    exit(1);
    }

  if((fpb=fopen("DefaultMap", "rb")) == NULL){
    if((fpb=fopen("../DefaultMap", "rb")) == NULL){
      fprintf(stderr, "Can't open Map File: DefaultMap\n");
      exit(1);
    }
  }
  if(fread(map, sizeof(int), NPIX, fpb) != NPIX){
    fprintf(stderr, "Map load error !\n");
    exit(1);
  }
  fclose(fpb);

  fprintf(stderr,"Read %s  ... ",argv[1]);
  if((fpa=fopen(argv[1], "rb")) == NULL){
    fprintf(stderr, "Can't open File: %s\n",argv[1]);
    exit(1);
  }
  fprintf(stderr,"Read %s  ... \n",argv[2]);
  if((fpb=fopen(argv[2], "rb")) == NULL){
    fprintf(stderr, "Can't open File: %s\n",argv[2]);
    exit(1);
  }

  for(i=0;i<NPIX;i++){
    zs[i]=0;
    xzs[i]=0;
    cr[i]=-ZSHIFT;
  }
  sx=0;sx2=0;sigma=0;ihdu=0;
  while(1){
    if(fread(buffer, sizeof(short), 12, fpa) != 12) break;
    if(fread(buffer, sizeof(short), NPIX, fpa) != NPIX) break;
    if(fread(buffers, sizeof(short), 12, fpb) != 12) break;
    if(fread(buffers, sizeof(short), NPIX, fpb) != NPIX) break;
    for(i=0;i<NPIX;i++){
      if(xzs[map[i]]>MARK_SATULATE){
	if(buffer[i]>SATULATE && buffers[i]>SATULATE){
	  a=((double)xzs[map[i]]*ihdu-zs[map[i]]*sx)/sigma;
	  b=(zs[map[i]]-a*sx)/ihdu;
	  zex=(int)(a*ihdu+b);
	  zre=(int)buffer[i]-(int)buffers[i];
	  //if(i==1754){
	  //if(ihdu>1) fprintf(stderr,"%d\t%d\t%d\t%d\t",(int)(a*ihdu),(int)b,zre,zex);
	  //else fprintf(stderr,"\t\t\t\t");
	  //}
	  if(ihdu>3 && cr[map[i]]<=0){
	    if(ihdu>4 && abs(zre-zex)>COSMIC_RAY && abs((zre-zex)-(cr[map[i]]+ZSHIFT))>COSMIC_RAY){
	      cr[map[i]]=zre-zex+ZSHIFT;
	    }
	    else
	      cr[map[i]]=zre-zex-ZSHIFT;
	  }
	  //if(i==0&&j==0)getchar();
	  if(cr[map[i]]>0) zre=(int)buffer[i]-(int)buffers[i]-(cr[map[i]]-ZSHIFT);
	  else zre=(int)buffer[i]-(int)buffers[i];
	  zs[map[i]]+=zre;
	  xzs[map[i]]+=ihdu*zre;
	  //if(i==1754) fprintf(stderr,"%d\t%d\t%d\t%d\t%d\n",ihdu,(int)buffer[i],(int)buffers[i],cr[map[i]],zre);
	}
	else{
	  if(sigma==0){
	    zs[map[i]]=BAD_PIXEL;
	    xzs[map[i]]=MARK_BAD;
	  }
	  else{
	    zs[map[i]]=(int)(((double)xzs[map[i]]*ihdu-zs[map[i]]*sx)*1000/sigma);
	    xzs[map[i]]=MARK_SATULATE;
	  }
	}
      }
    }
    sx+=ihdu;
    sx2+=ihdu*ihdu;
    sigma=(ihdu+1)*sx2-sx*sx;
    ihdu++;
  }
  if(sigma==0) sigma=1;
  ncr=0;
  for(i=0;i<NPIX;i++){
    if(xzs[i]!=MARK_BAD){
      if(xzs[i]!=MARK_SATULATE)
	zs[i]=(int)(((double)xzs[i]*ihdu-zs[i]*sx)*(ihdu-1)/sigma);
      else zs[i]*=(double)(ihdu-1)/1000;
    }
    if(cr[map[i]]>0) ncr++;
  }
  fprintf(stderr,"CR rejected : %d pixels\n",ncr);
  fclose(fpa);
  fprintf(stderr,"OK\n");
  fclose(fpb);
  fprintf(stderr,"OK\n");

  fprintf(stderr,"\nWrite %s ... ",argv[3]);
  status=0;
  if (fits_create_file(&fp, argv[3], &status))
    printerror(status);
  if (fits_create_img(fp, bitpix, naxis, naxes, &status))
    printerror(status);
  fpixel=1;
  if(fits_write_img(fp, TINT, fpixel, NPIX, zs, &status))
    printerror(status);
  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 */
}