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

#define LINEPIX 2048

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],z[LINEPIX][LINEPIX],s[LINEPIX][LINEPIX];
  int k,nn;
  double zav,zav2,zamp,zampmin,ww,ws,we,zt;

  if(argc!=4){
    fprintf(stderr,"Usage : %s input_file1(A-B1) input_file2(A-B2) output_file\n",argv[0]);
    fprintf(stderr,"       or\n");
    fprintf(stderr,"Usage : %s input_file1(A1-B) input_file2(A2-B) output_file\n",argv[0]);
    exit(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);
  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[2]);
  status=0;
  if(fits_open_file(&fp,argv[2], 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++) s[i][j]=buffer[j]-z[i][j];
    fpixel+=naxes[0];
  }
  if(fits_close_file(fp, &status)) printerror(status);
  fprintf(stderr,"OK\n");

  ww=0.;ws=1;we=0.001;zampmin=1e100;nn=0;
  while(1){
    zamp=0.;
    for(k=0;k<4;k++){
      zav=0.;zav2=0.;
      for(i=(k/2)*(naxes[1]/2);i<(k/2+1)*(naxes[1]/2);i++){
	for(j=(k%2)*(naxes[1]/2);j<(k%2+1)*(naxes[1]/2);j++){
	  zt=z[i][j]+s[i][j]*ww;
	  //zav2+=zt*zt/LINEPIX/LINEPIX*4;
	  zav+=zt/LINEPIX/LINEPIX*4;
	}
      }
      //zamp+=(zav2-zav*zav)/4;
      for(i=(k/2)*(naxes[1]/2);i<(k/2+1)*(naxes[1]/2);i++){
	for(j=(k%2)*(naxes[1]/2);j<(k%2+1)*(naxes[1]/2);j++){
	  zt=z[i][j]+s[i][j]*ww;
	  zav2+=fabs(zt-zav)/LINEPIX/LINEPIX*4;
	}
      }
      zamp+=zav2/4;
    }
    zamp=sqrt(zamp);
    //fprintf(stderr,"weight=%g zamp=%g zampmin=%g\n",ww,zamp,zampmin);
    if(zampmin>zamp){
      zampmin=zamp;
      ww+=ws;
    }
    else{
      ww-=ws;
      if(nn==0){
	ws*=-1;
	ww+=ws;
	nn=1;
      }
      else{
	if(fabs(ws)<we) break;
	ws/=2;
	ww+=ws;
	nn=0;
      }
    }
  }
  if(ww<-1) ww=-1;
  else if(ww>2) ww=2;
  fprintf(stderr,"weight=%g zampmin=%g\n",ww,zampmin);
  for(i=0;i<naxes[1];i++){
    for(j=0;j<naxes[0];j++) z[i][j]+=s[i][j]*ww;
  }

  remove(argv[3]);  
  fprintf(stderr,"Write %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;
  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 */
}