#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "nrutil.c"
#include "fitsio.h"
#define  LINEPIX 11
#define  LINEPIY 9
#define  NBUN 20
void spline(float x[], float y[], int n, float yp1, float ypn, float y2[]);
float splint(float xa[], float ya[], float y2a[], int n, float x);
void splie2(float x1a[], float x2a[], float **ya, int m, int n, float **y2a);
float splin2(float x1a[], float x2a[], float **ya, float **y2a, int m, int n, float x1, float x2);
double calc_vamp(float x1a[], float x2a[], float **ya, float **y2a, int mmax, int nmax, float **v1a, float **v2a, int pr, float **p1a, float **p2a);
void printerror( int status);

int main(int argc,char *argv[])
{
  FILE *fps,*fpt;
  int i,j,k,m,n,mmax,nmax,mm,nn,ct,idat,kmin;
  float *x1a,*x2a,**ya,**y2a,x1,x2,y;
  fitsfile *fp;
  int status,bitpix=-32;
  long fpixel, nelements,naxis=2;
  long naxes[2];
  float **v1a,**v2a,x1min,x2min,x1s,x2s,acc,**p1a,**p2a;
  float px[(LINEPIX-1)*(LINEPIY-1)],py[(LINEPIX-1)*(LINEPIY-1)],vx[(LINEPIX-1)*(LINEPIY-1)],vy[(LINEPIX-1)*(LINEPIY-1)],pxt,pyt,vxt,vyt;
  int np[(LINEPIX-1)*(LINEPIY-1)];
  double vamp,vampmin,dz,rsum,rsummin,xc,yc,dx,dy;
  static float buffer[(LINEPIX-1)*NBUN+1];
  char dmy[16],buf[1024],fname[32];

  x1a=vector(1,LINEPIX);
  x2a=vector(1,LINEPIY);
  ya=matrix(1,LINEPIX,1,LINEPIY);
  y2a=matrix(1,LINEPIX,1,LINEPIY);
  v1a=matrix(1,LINEPIX-1,1,LINEPIY-1);
  v2a=matrix(1,LINEPIX-1,1,LINEPIY-1);
  p1a=matrix(1,LINEPIX-1,1,LINEPIY-1);
  p2a=matrix(1,LINEPIX-1,1,LINEPIY-1);

  for(m=1;m<=LINEPIX;m++) x1a[m]=(float)m;
  for(n=1;n<=LINEPIY;n++) x2a[n]=(float)n;
  for(n=1;n<LINEPIY;n++){
    for(m=1;m<LINEPIX;m++){
      v1a[m][n]=-10000.;
      v2a[m][n]=-10000.;
    }
  }

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

  if((fpt = fopen(argv[1],"r")) == NULL){
    fprintf(stderr,"Cannot open %s\n",argv[1]);
    exit(0);
  }

  for(i=0;i<(LINEPIX-1)*(LINEPIY-1);i++) np[i]=0;
  idat=0;
  while(!feof(fpt)){
    if(fgets(buf,sizeof(buf),fpt)==NULL) break;
    sscanf(buf,"%s",fname);
    if((fps = fopen(fname,"r")) == NULL){
      fprintf(stderr,"Cannot open %s\n",fname);
      exit(0);
    }
    while(!feof(fps)){
      if(fgets(buf,sizeof(buf),fps)==NULL) break;
      if(strncmp(buf,"#",1)==0) continue;
      sscanf(buf,"%f %f %s %s %f %f",&pxt,&pyt,dmy,dmy,&vxt,&vyt);
      ct=0;
      //fprintf(stderr,"%f\t%f\t%f\t%f\n",vxt,vyt,pxt,pyt);
      for(i=0;i<idat;i++){
	if(fabs(vx[i]-vxt)<2 && fabs(vy[i]-vyt)<2){
	  vx[i]=(vx[i]*np[i]+vxt)/(np[i]+1);
	  vy[i]=(vy[i]*np[i]+vyt)/(np[i]+1);
	  px[i]=(px[i]*np[i]+pxt)/(np[i]+1);
	  py[i]=(py[i]*np[i]+pyt)/(np[i]+1);
	  np[i]++;
	  ct=1;
	  break;
	}
      }
      if(ct==0){
	vx[idat]=vxt;
	vy[idat]=vyt;
	px[idat]=pxt;
	py[idat]=pyt;
	np[idat]=1;
	idat++;
      }
    }
    fclose(fps);
  }
  fclose(fpt);
  //for(i=0;i<idat;i++)fprintf(stderr,"%d\t%d\t%f\t%f\t%f\t%f\n",m,np[i],vx[i],vy[i],px[i],py[i]);

  dz=0.001;rsummin=1e10;
  for(k=-1;k<=1;k++){
    xc=0.;yc=0.;
    for(i=0;i<idat;i++){
      xc+=(px[i]+vx[i]*dz*k)/idat;
      yc+=(py[i]+vy[i]*dz*k)/idat;
    }
    rsum=0.;
    for(i=0;i<idat;i++){
      dx=px[i]+vx[i]*dz*k-xc;
      dy=py[i]+vy[i]*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(i=0;i<idat;i++){
        xc+=(px[i]+vx[i]*dz*k)/idat;
        yc+=(py[i]+vy[i]*dz*k)/idat;
      }
      rsum=0.;
      for(i=0;i<idat;i++){
        dx=px[i]+vx[i]*dz*k-xc;
        dy=py[i]+vy[i]*dz*k-yc;
        rsum+=sqrt(dx*dx+dy*dy);
      }
      //fprintf(stderr,"%d %d rsum=%g rsummin=%g\n",k,kmin,rsum,rsummin);
    }
  }
  xc=0.;yc=0.;
  for(i=0;i<idat;i++){
    px[i]+=vx[i]*dz*(k-kmin);
    py[i]+=vy[i]*dz*(k-kmin);
    xc+=px[i]/idat;
    yc+=py[i]/idat;
  }
  for(i=0;i<idat;i++){
    px[i]-=xc;
    py[i]-=yc;
  }  

  x1min=999.;x2min=999.;
  for(i=0;i<idat;i++){
    if(x1min>vx[i]) x1min=vx[i];
    if(x2min>vy[i]) x2min=vy[i];
  }
  m=0;n=0;x1s=0.;x2s=0.;
  for(i=0;i<idat;i++){
    if(vx[i]<x1min+2){x1s+=vx[i];m++;}
    if(vy[i]<x2min+2){x2s+=vy[i];n++;}
  }
  x1min=x1s/m;
  x2min=x2s/n;
  mmax=1;nmax=1;
  for(i=0;i<idat;i++){
    m=(vx[i]-x1min)/4.+1.5;
    n=(vy[i]-x2min)/4.+1.5;
    if(m>=LINEPIX){fprintf(stderr,"m>=LINEPIX error\n");exit(0);}
    if(n>=LINEPIY){fprintf(stderr,"n>=LINEPIY error\n");exit(0);}
    if(v1a[m][n]>-9999.||v2a[m][n]>-9999.){
      fprintf(stderr,"Conflict Ray: %d %d (%g,%g)(%g,%g)\n",m,n,p1a[m][n],p2a[m][n],vx[i],vy[i]);
      exit(0);
    }
    v1a[m][n]=px[i];
    v2a[m][n]=py[i];
    p1a[m][n]=vx[i];
    p2a[m][n]=vy[i];
    if(mmax<m) mmax=m;
    if(nmax<n) nmax=n;
  }

  //for(n=1;n<=nmax;n++){
  //  for(m=1;m<=mmax;m++) fprintf(stderr,"%d\t%d\t%f\t%f\n",m,n,v1a[m][n],v2a[m][n]);
  //}
  mmax++;nmax++;

  for(n=1;n<=nmax;n++){
    for(m=1;m<=mmax;m++) ya[m][n]=0.;
  }
  vampmin=calc_vamp(x1a, x2a, ya, y2a, mmax, nmax, v1a, v2a, 0, p1a, p2a);

  acc=1.;
  while(acc>1e-8){
    ct=1;
    while(ct!=0){
      ct=0;
      for(nn=1;nn<=nmax;nn++){
	for(mm=1;mm<=mmax;mm++){
	  ya[mm][nn]+=acc;
	  vamp=calc_vamp(x1a, x2a, ya, y2a, mmax, nmax, v1a, v2a, 0, p1a, p2a);
	  if(vampmin>vamp){vampmin=vamp;ct++;}
	  else{
	    ya[mm][nn]-=acc*2;
	    vamp=calc_vamp(x1a, x2a, ya, y2a, mmax, nmax, v1a, v2a, 0, p1a, p2a);
	    if(vampmin>vamp){vampmin=vamp;ct++;}
	    else ya[mm][nn]+=acc;
	  }
	  //fprintf(stderr,"%d\t%d\t%d\t%lf\t%lf\n",mm,nn,ct,vamp,vampmin);
	}
      }
      fprintf(stderr,"%g\t%d\t%lf\n",acc,ct,vampmin);
    }
    acc/=10.;
  }
  printf("#vamp=%g\n",vampmin);
  vamp=calc_vamp(x1a, x2a, ya, y2a, mmax, nmax, v1a, v2a, 1, p1a, p2a);
  fprintf(stderr,"vamp=%g\n",vamp);

  /*
  for(n=1;n<=nmax;n++){
    for(m=1;m<=mmax;m++) fprintf(stderr,"\t%f",ya[m][n]);
    fprintf(stderr,"\n");
  }
 for(n=1;n<nmax;n++){
    for(m=1;m<mmax;m++) fprintf(stderr,"\t%f",v1a[m][n]);
    fprintf(stderr,"\n");
  }
 for(n=1;n<nmax;n++){
    for(m=1;m<mmax;m++) fprintf(stderr,"\t%f",v2a[m][n]);
    fprintf(stderr,"\n");
  }
  */

  fprintf(stderr,"\nWrite %s ... ",argv[2]);
  status=0;
  if (fits_create_file(&fp, argv[2], &status))
    printerror(status);
  naxes[0]=(mmax-1)*NBUN+1;
  naxes[1]=(nmax-1)*NBUN+1;
  if (fits_create_img(fp, bitpix, naxis, naxes, &status))
    printerror(status);
  fpixel=1;
  fprintf(stderr,"%ld x %ld format ",naxes[0],naxes[1]);
  nelements=naxes[0];
  for(n=1;n<=nmax;n++){
    for(i=0;i<NBUN;i++){
      if(n==nmax){
	if(i==0) x2=x2a[n];
	else continue;
      }else x2=(x2a[n]*(NBUN-i)+x2a[n+1]*i)/NBUN;
      for(m=1;m<=mmax;m++){
        for(j=0;j<NBUN;j++){
          if(m==mmax){
	    if(j==0) x1=x1a[m];
	    else continue;
	  }else x1=(x1a[m]*(NBUN-j)+x1a[m+1]*j)/NBUN;
	  y=0.;
	  if(v1a[m][n]>-9999. && v2a[m][n]>-9999.) y=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);
	  else{
	    if(m>1){
	      if(v1a[m-1][n]>-9999. && v2a[m-1][n]>-9999. && j==0) y=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);
	    }
	    if(n>1){
	      if(v1a[m][n-1]>-9999. && v2a[m][n-1]>-9999. && i==0) y=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);
	    }
	    if(m>1 && n>1){
	      if(v1a[m-1][n-1]>-9999. && v2a[m-1][n-1]>-9999. && j==0 && i==0) y=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);
	    }
	  }
          //fprintf(stderr,"\t%5.2f(%g,%g)",y,x1,x2);
          buffer[(m-1)*NBUN+j]=y*0.05;
        }
      }
      //fprintf(stderr,"\n");
      if(fits_write_img(fp, TFLOAT, fpixel, nelements, buffer, &status))
        printerror(status);
      fpixel+=nelements;
    }
  }

  if (fits_close_file(fp, &status))
    printerror(status);
  fprintf(stderr,"OK\n");
  return(0);
}


void spline(float x[], float y[], int n, float yp1, float ypn, float y2[])
{
  int i,k;
  float p,qn,sig,un,*u;

  u=vector(1,n-1);
  if (yp1 > 0.99e30)
    y2[1]=u[1]=0.0;
  else {
    y2[1] = -0.5;
    u[1]=(3.0/(x[2]-x[1]))*((y[2]-y[1])/(x[2]-x[1])-yp1);
  }
  for (i=2;i<=n-1;i++) {
    sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
    p=sig*y2[i-1]+2.0;
    y2[i]=(sig-1.0)/p;
    u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
    u[i]=(6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
  }
  if (ypn > 0.99e30)
    qn=un=0.0;
  else {
    qn=0.5;
    un=(3.0/(x[n]-x[n-1]))*(ypn-(y[n]-y[n-1])/(x[n]-x[n-1]));
  }
  y2[n]=(un-qn*u[n-1])/(qn*y2[n-1]+1.0);
  for (k=n-1;k>=1;k--)
    y2[k]=y2[k]*y2[k+1]+u[k];
  free_vector(u,1,n-1);
}

float splint(float xa[], float ya[], float y2a[], int n, float x)
{
  void nrerror(char error_text[]);
  int klo,khi,k;
  float h,b,a,y;

  klo=1;
  khi=n;
  while (khi-klo > 1) {
    k=(khi+klo) >> 1;
    if (xa[k] > x) khi=k;
    else klo=k;
  }
  h=xa[khi]-xa[klo];
  if (h == 0.0) nrerror("Bad xa input to routine splint");
  a=(xa[khi]-x)/h;
  b=(x-xa[klo])/h;
  y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0;
  return(y);
}

void splie2(float x1a[], float x2a[], float **ya, int m, int n, float **y2a)
{
  void spline(float x[], float y[], int n, float yp1, float ypn, float y2[]);
  int j;

  for (j=1;j<=m;j++)
    spline(x2a,ya[j],n,1.0e30,1.0e30,y2a[j]);
}

float splin2(float x1a[], float x2a[], float **ya, float **y2a, int m, int n, float x1, float x2)
{
  void spline(float x[], float y[], int n, float yp1, float ypn, float y2[]);
  float splint(float xa[], float ya[], float y2a[], int n, float x);
  int j,k;
  float *ytmp,*yytmp,y;

  if(m>n) k=m; else k=n;
  ytmp=vector(1,k);
  yytmp=vector(1,k);
  for (j=1;j<=m;j++)
    yytmp[j]=splint(x2a,ya[j],y2a[j],n,x2);
  spline(x1a,yytmp,m,1.0e30,1.0e30,ytmp);
  y=splint(x1a,yytmp,ytmp,m,x1);
  free_vector(yytmp,1,k);
  free_vector(ytmp,1,k);
  return(y);
}

double calc_vamp(float x1a[], float x2a[], float **ya, float **y2a, int mmax, int nmax, float **v1a, float **v2a, int pr, float **p1a, float **p2a)
{
  void splie2(float x1a[], float x2a[], float **ya, int m, int n, float **y2a);
  float splin2(float x1a[], float x2a[], float **ya, float **y2a, int m, int n, float x1, float x2);
  int m,n;
  float x1,x2,y1,y2,v1,v2;
  double vamp;

  splie2(x1a, x2a, ya, mmax, nmax, y2a);
  vamp=0.;
  for(n=1;n<nmax;n++){
    for(m=1;m<mmax;m++){
      if(v1a[m][n]<-9999. || v2a[m][n]<-9999.) continue;
      x2=x2a[n]*0.50+x2a[n+1]*0.50;
      x1=x1a[m]*0.75+x1a[m+1]*0.25;
      y1=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);      
      x1=x1a[m]*0.25+x1a[m+1]*0.75;
      y2=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);      
      v1=(y2-y1)/(x1a[n+1]-x1a[n])/2;
      x1=x1a[m]*0.50+x1a[m+1]*0.50;
      x2=x2a[n]*0.75+x2a[n+1]*0.25;
      y1=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);      
      x2=x2a[n]*0.25+x2a[n+1]*0.75;
      y2=splin2(x1a, x2a, ya, y2a, mmax, nmax, x1, x2);      
      v2=(y2-y1)/(x2a[n+1]-x2a[n])/2;
      if(pr) printf("%f\t%f\t%f\t%f\t%f\t%f\n",v1a[m][n],v2a[m][n],v1,v2,p1a[m][n],p2a[m][n]);
      vamp+=sqrt((v1a[m][n]-v1)*(v1a[m][n]-v1)+(v2a[m][n]-v2)*(v2a[m][n]-v2));
    }
  }
  return(vamp);
}

void printerror( int status)
{
  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);
  fprintf(stderr, "\nstatus = %d: %s\n", status, status_str);

  if ( fits_read_errmsg(errmsg) )
    {
      fprintf(stderr, "\nError message stack:\n");
      fprintf(stderr, " %s\n", errmsg);

      while ( fits_read_errmsg(errmsg) )
        fprintf(stderr, " %s\n", errmsg);
    }

  exit( status );
}