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

#define LINEPIX 2560
#define LINEPIY 1920
#define NFRAME 10

void printerror( int status );

int main(int argc, char *argv[])
{
  fitsfile *fp;
  int i,j,status,nfound,anynull;
  long fpixel;
  long naxes[2]={LINEPIX, LINEPIY},nbuffer;
  float nullval;
  static float buffer[LINEPIX],z[NFRAME][LINEPIY][LINEPIX];
  int n,ndat,di,dj,mx,m,mdat,vi,vj,nzs,k,kmin,mode,djmax[NFRAME],ns;
  double pos[NFRAME],pc[2][NFRAME],zs,p0[2],v0[2],vt[2],pt[2],vo[2],po[2],pg[3][NFRAME];
  double ax2,ax,axy,ay,ss,px[63],py[63],vx[63],vy[63],dp,dv,dvt;
  double dx,dy,dz,xc,yc,rsum,rsummin,pct[2][NFRAME],zsmax;
  char buf[128],filename[32];
  FILE *fpd;

  if(argc!=2){
    fprintf(stderr,"Usage : %s input_list\n",argv[0]);
    exit(0);
  }

  if((fpd=fopen(argv[1],"r"))==NULL){
    fprintf(stderr,"Can not open file: %s\n",argv[1]);
    exit(1);
  }

  for(n=0;i<NFRAME;i++){
  for(i=0;i<LINEPIY;i++){
    for(j=0;j<LINEPIX;j++) z[n][i][j]=0.;
  }}
  n=0;
  while(!feof(fpd)){
    if(fgets(buf,sizeof(buf),fpd)==NULL) break;
    if(sscanf(buf,"%s %lf",filename,&pos[n])!=2) break;
    if(fabs(pos[n])<5.1) continue;
    fprintf(stderr,"Read %s  ... ",filename);
    status=0;
    if(fits_open_file(&fp,filename, READONLY, &status)){
      fprintf(stderr,"Failed! Exit this process...\n");
      exit(0);
    }
    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]>=LINEPIY)){
      fprintf(stderr,"\nIllegal image format\n");
      exit(0);
    }
    fpixel = 1;
    nullval = 0;
    nbuffer = naxes[0];
    pc[0][n]=0.;pc[1][n]=0.;zs=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[n][i][j]+=buffer[j];
	pc[0][n]+=j*z[n][i][j];
	pc[1][n]+=i*z[n][i][j];
	zs+=z[n][i][j];
      }
      fpixel+=naxes[0];
    }
    pc[0][n]/=zs;
    pc[1][n]/=zs;
    if(fits_close_file(fp, &status))
      printerror(status);
    for(j=0;j<naxes[0];j++){
      buffer[j]=0.;
      for(i=0;i<naxes[1];i++) buffer[j]+=z[n][i][j];
    }
    zsmax=0.;
    for(dj=10;dj<100;dj++){
      zs=0.;
      for(j=0;j<naxes[0]-100;j++) zs+=buffer[j]*buffer[j+dj];
      if(zsmax<zs){
	zsmax=zs;
	djmax[n]=dj;
      }
      //fprintf(stderr,"%2d %g %2d %g\n",dj,zs,djmax[n],zsmax);
    }
    if(djmax[n]==10){
      fprintf(stderr,"NG(%d)\tSpots are too close.\n",n);
      continue;
    }
    fprintf(stderr,"OK(%d)\tposition=%g\tcenter=(%7.2lf,%7.2lf)\tz_sum=%g\n",n,pos[n],pc[0][n]+1,pc[1][n]+1,zs);
    n++;
    //getchar();
  }
  ndat=n;
  fclose(fpd);

  ax2=0.;ax=0.;axy=0.;ay=0.;
  for(n=0;n<ndat;n++){
    ax2+=pos[n]*pos[n]/ndat;
    ax +=pos[n]/ndat;
    axy+=pos[n]*djmax[n]/ndat*pos[n]/fabs(pos[n]);
    ay +=(double)djmax[n]/ndat*pos[n]/fabs(pos[n]);
  }
  ss=-(ax2*ay-ax*axy)/(axy-ax*ay);
  //fprintf(stderr,"%g\n",ss);
  for(n=0;n<ndat;n++){
    pos[n]-=ss;
    //fprintf(stderr,"%g\n",pos[n]);
  }

  if((fpd=fopen("rayfit.dat","r"))!=NULL){
    fprintf(stderr,"Read: rayfit.dat\n");
    if(fscanf(fpd,"%lf %lf %lf %lf",&vo[0],&vo[1],&po[0],&po[1])!=4){
      fprintf(stderr,"Wrong format\n");
      exit(1);
    }
    fclose(fpd);
  }else{
    vo[0]=0.;
    vo[1]=0.;
    po[0]=0.;
    po[1]=0.;
  }

  vt[0]=100;
  vt[1]=100;
  mdat=0;ns=0;
  for(mode=0;mode<=1;mode++){
    for(vi=-3;vi<=3;vi++){
    for(vj=-4;vj<=4;vj++){
      if(mode){
	v0[0]=4.*vj;
	v0[1]=4.*vi;
	p0[0]=0;
	p0[1]=0;
      }else{
	v0[0]=0.7*vj+vo[0];
	v0[1]=0.7*vi+vo[1];
	p0[0]=po[0];
	p0[1]=po[1];
      }
      //fprintf(stderr,"vx=%3g vy=%3g px=%3g py=%3g\n",v0[0],v0[1],p0[0],p0[1]);
      for(m=0;m<10;m++){
	nzs=0;
	for(n=0;n<ndat;n++){
	  mx=(int)fabs(pos[n]);
	  pg[0][n]=0.;pg[1][n]=0.;zs=0.;
	  for(di=-mx;di<=mx;di++){
	  for(dj=-mx;dj<=mx;dj++){
	    j=pc[0][n]+(v0[0]*pos[n]+p0[0])+dj;
	    i=pc[1][n]+(v0[1]*pos[n]+p0[1])+di;
	    if(j<0) j=0;else if(j>naxes[0]-1) j=naxes[0]-1;
	    if(i<0) i=0;else if(i>naxes[1]-1) i=naxes[1]-1;
	    pg[0][n]+=(j-pc[0][n])*z[n][i][j];
	    pg[1][n]+=(i-pc[1][n])*z[n][i][j];
	    zs+=z[n][i][j];
	  }}
	  pg[2][n]=zs;
	  if(zs>100.) nzs++;
	  //if(mode==0 && zs>1) fprintf(stderr,"position=%6.2lf  a_cen=(%6.1lf,%6.1lf)  g_cen=(%6.1lf,%6.1lf)  z_sum=%g\n",pos[n],(v0[0]*pos[n]+p0[0])+pc[0][n]+1,(v0[1]*pos[n]+p0[1])+pc[1][n]+1,pg[0][n]/pg[2][n]+pc[0][n]+1,pg[1][n]/pg[2][n]+pc[1][n]+1,zs);
	}
	if(nzs<2) break;
	for(k=0;k<2;k++){
	  ax2=0.;ax=0.;axy=0.;ay=0.;ss=0.;
	  for(n=0;n<ndat;n++){
	    ax2+=pos[n]*pos[n]*pg[2][n]/ndat;
	    ax +=pos[n]*pg[2][n]/ndat;
	    axy+=pos[n]*pg[k][n]/ndat;
	    ay +=pg[k][n]/ndat;
	    ss +=pg[2][n]/ndat;
	  }
	  v0[k]=(axy*ss-ax*ay)/(ax2*ss-ax*ax);
	  if(m>5) p0[k]=(ax2*ay-ax*axy)/(ax2*ss-ax*ax);
	}
      }
      if(nzs>=2){
	if(mode){
	  if(fabs(v0[0]-4.*vj)<1 && fabs(v0[1]-4.*vi)<1){
	    px[mdat]=p0[0];
	    py[mdat]=p0[1];
	    vx[mdat]=v0[0];
	    vy[mdat]=v0[1];
	    //fprintf(stderr,"%10.5lf %10.5lf %10.5lf %10.5lf\n",px[mdat],py[mdat],vx[mdat],vy[mdat]);getchar();
	    mdat++;
	  }
	}else{
	  //fprintf(stderr,"%2d %2d %10.5lf %10.5lf %10.5lf %10.5lf %10.5lf %10.5lf\n",vi,vj,p0[0],p0[1],v0[0],v0[1],vo[0],vo[1]);getchar();
	  dv =sqrt((v0[0]-vo[0])*(v0[0]-vo[0])+(v0[1]-vo[1])*(v0[1]-vo[1]));
	  dvt=sqrt((vt[0]-vo[0])*(vt[0]-vo[0])+(vt[1]-vo[1])*(vt[1]-vo[1]));
	  dp =sqrt((p0[0]-po[0])*(p0[0]-po[0])+(p0[1]-po[1])*(p0[1]-po[1]));
	  if(dv<dvt || (ns==0&&dp<5)){
	    if(ns==0||dp<5){
	      vt[0]=v0[0];
	      vt[1]=v0[1];
	      pt[0]=p0[0];
	      pt[1]=p0[1];
	      for(n=0;n<ndat;n++){
		pct[0][n]=pg[0][n]/pg[2][n]+pc[0][n];
		pct[1][n]=pg[1][n]/pg[2][n]+pc[1][n];
	      }
	      if(dp<5) ns=1;
	    }
	  }
	}
      }
    }}
    if(mode==0){
      for(n=0;n<ndat;n++){
	fprintf(stderr,"position: %g center: %10.5lf %10.5lf\n",pos[n],pct[0][n]+1,pct[1][n]+1);
	pc[0][n]=pct[0][n];
	pc[1][n]=pct[1][n];
      }
      fprintf(stderr,"Write: rayfit.dat\n");
      if((fpd=fopen("rayfit.dat","w"))==NULL){
	fprintf(stderr,"Connot open: rayfit.dat\n");
	exit(1);
      }
      fprintf(fpd,"%lf %lf %lf %lf\n",vt[0],vt[1],pt[0],pt[1]);
      fclose(fpd);
      fprintf(stderr,"%lf %lf %lf %lf\n",vt[0],vt[1],pt[0],pt[1]);
      //getchar();
    }
  }
  dz=0.01;rsummin=1e10;
  for(k=-1;k<=1;k++){
    xc=0.;yc=0.;
    for(m=0;m<mdat;m++){
      xc+=(px[m]+vx[m]*dz*k)/mdat;
      yc+=(py[m]+vy[m]*dz*k)/mdat;
    }
    rsum=0.;
    for(m=0;m<mdat;m++){
      dx=px[m]+vx[m]*dz*k-xc;
      dy=py[m]+vy[m]*dz*k-yc;
      rsum+=sqrt(dx*dx+dy*dy);
    }
    if(rsummin>rsum){
      rsummin=rsum;
      kmin=k;
    }
    //fprintf(stderr,"%d %d %g %g rsum=%g rsummin=%g\n",k,kmin,xc,yc,rsum,rsummin);
  }
  if(kmin!=0){
    rsum=rsummin;
    rsummin=1e10;
    k=kmin;
    while(rsummin>rsum){
      rsummin=rsum;
      k+=kmin;
      xc=0.;yc=0.;
      for(m=0;m<mdat;m++){
	xc+=(px[m]+vx[m]*dz*k)/mdat;
	yc+=(py[m]+vy[m]*dz*k)/mdat;
      }
      rsum=0.;
      for(m=0;m<mdat;m++){
	dx=px[m]+vx[m]*dz*k-xc;
	dy=py[m]+vy[m]*dz*k-yc;
	rsum+=sqrt(dx*dx+dy*dy);
      }
      //fprintf(stderr,"%d %d rsum=%g rsummin=%g\n",k,kmin,rsum,rsummin);
    }
  }
  printf("#dz=%g(um)\n",dz*(k-kmin)*1000);
  for(m=0;m<mdat;m++) printf("%10.5lf %10.5lf %10.5lf %10.5lf %10.5lf %10.5lf\n",px[m]+vx[m]*dz*(k-kmin),py[m]+vy[m]*dz*(k-kmin),px[m],py[m],vx[m],vy[m]);
  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 */
}