# include "gastro2.h"

static int NPAIR, Npair;
static int *idx1, *idx2;

void gfit (CmpCatalog *Target, RefCatalog *Ref, int order) {

  int i, j, j0;
  double Radius, Radius2;
  double dX, dY, dR;
  RefCatalog Subset;
  StarData *st, *sr;

  NFIELD = 0.1;
  gproject (Target, Ref, &Subset);
  if (PLOTSTUFF) plot_fullfield (Target, &Subset);

  if (Subset.N < 3) {
    fprintf (stderr, "ERROR: solution off target\n");
    exit (1);
  }

  /* need the stars sorted in X */
  sort_stars_X (Target[0].stars, Target[0].N);
  sort_stars_X (Subset.stars, Subset.N);

  Radius = MAX (2.0 * Target[0].answer.dR, 0.5);
  Radius2 = Radius*Radius;

  st = Target[0].stars;
  sr = Subset.stars;

  /* find the matched pairs of stars within the radius */
  pair_init ();
  for (i = j = 0; (i < Target[0].N) && (j < Subset.N);) {
    /* get in right X range */
    dX = st[i].X - sr[j].X;
    if (dX < -Radius) {
      i++;
      continue;
    }
    if (dX > Radius) {
      j++;
      continue;
    }

    /* check for pairs in this X range */
    j0 = j;
    for (; (dX > -Radius) && (j < Subset.N); j++) {
    
      dX = st[i].X - sr[j].X;
      dY = st[i].Y - sr[j].Y;

      dR = dX*dX + dY*dY;
      if (dR > Radius2) {
	j++;
	continue;
      }
      pair_add (i, j);
    }
    j = j0;
    i ++;
  }
  
# if (0)
  pair_init ();
  for (i = 0; i < Target[0].N; i++) {
    pair_add (i, i);
  }
# endif

  fit_init (order);
  fit_norm ();

  plot_resid_init (0, (double) Target[0].header.Naxis[0]);
  plot_resid_init (1, (double) Target[0].header.Naxis[1]);
  if (PLOTSTUFF) plot_resid (st, sr);

  /* find fit for matched pairs */
  fit_init (order);
  for (i = 0; i < Npair; i++) {
    fit_add (st[idx1[i]].X, st[idx1[i]].Y, sr[idx2[i]].X, sr[idx2[i]].Y, 1.0);
  }
  fit_eval ();

  Target[0].answer.dR = fit_scat (st, sr);
  Target[0].answer.N  = fit_adjust (&Target[0].coords);

  plot_resid_init (0, (double) Target[0].header.Naxis[0]);
  plot_resid_init (1, (double) Target[0].header.Naxis[1]);
  if (PLOTSTUFF) plot_resid (st, sr);
  free (idx1);
  free (idx2);
}

static int NTERM, NPOWR, NPARS, NORDER, Npts;
static double **sum, **xsum, **ysum;
static double **matrix, **vector;

void fit_init (int order) {

  int i;

  NORDER = order;
  NPOWR = NORDER + 1;
  NTERM = 2*NORDER + 1;
  NPARS = (NORDER + 1)*(NORDER + 2) / 2;
  Npts  = 0;

  /* allocate arrays for fit solution */
  ALLOCATE (sum, double *, NTERM);
  ALLOCATE (xsum, double *, NTERM);
  ALLOCATE (ysum, double *, NTERM);
  for (i = 0; i < NTERM; i++) {
    ALLOCATE (sum[i], double, NTERM);
    bzero (sum[i], NTERM*sizeof(double));
    ALLOCATE (xsum[i], double, NTERM);
    bzero (xsum[i], NTERM*sizeof(double));
    ALLOCATE (ysum[i], double, NTERM);
    bzero (ysum[i], NTERM*sizeof(double));
  }
  ALLOCATE (matrix, double *, NPARS);
  ALLOCATE (vector, double *, NPARS);
  for (i = 0; i < NPARS; i++) {
    ALLOCATE (matrix[i], double, NPARS);
    ALLOCATE (vector[i], double, 2);
    bzero (vector[i], 2*sizeof(double));
    bzero (matrix[i], NPARS*sizeof(double));
  }

}

void fit_add (double x1, double y1, double x2, double y2, double wt) {

  int n, m;
  double xterm, yterm, term;

  xterm = 1;
  for (n = 0; n < NTERM; n++) {
    yterm = 1;
    for (m = 0; m < NTERM; m++) {
      term = xterm*yterm;
      if (n+m < NTERM) {
	sum[n][m] += term;
      }
      if (n+m < NPOWR) {
	xsum[n][m] += x2*term;
	ysum[n][m] += y2*term;
      }
      yterm *= y1;
    }
    xterm *= x1;
  }
  Npts ++;

}

void fit_eval () {

  int i, j, n, m, M, N;
  double max;

  fprintf (stderr, "npts: %d\n", Npts);

  i = 0;
  for (m = 0; m < NPOWR; m++) {
    for (n = 0; n < NPOWR - m; n++, i++) {
      vector[i][0] = xsum[n][m];
      vector[i][1] = ysum[n][m];
    }	
  }
  j = 0;
  for (M = 0; M < NPOWR; M++) {
    for (N = 0; N < NPOWR - M; N++, j++) {
      i = 0;
      for (m = 0; m < NPOWR; m++) {
	for (n = 0; n < NPOWR - m; n++, i++) {
	  matrix[i][j] = sum[n+N][m+M];
	}	
      }
    }
  }       
  max = 0.0;
  for (i = 0; i < NPARS; i++) {
    for (j = 0; j < NPARS; j++) {
      max = MAX (max, fabs(matrix[i][j]));
    }
    max = MAX (max, fabs(vector[i][0]));
    max = MAX (max, fabs(vector[i][1]));
  }
  for (i = 0; i < NPARS; i++) {
    for (j = 0; j < NPARS; j++) {
      matrix[i][j] /= max;
    }
    vector[i][0] /= max;
    vector[i][1] /= max;
  }
# if (0)
  for (i = 0; i < NPARS; i++) {
    for (j = 0; j < NPARS; j++) {
      fprintf (stderr, "%10.4e  ", matrix[i][j]);
    }
    fprintf (stderr, "  %10.4e  ", vector[i][0]);
    fprintf (stderr, "  %10.4e  \n", vector[i][1]);
  }
# endif
  gaussj (matrix, NPARS, vector, 2); 
# if (0)
  fprintf (stderr, "\n\n");
  for (i = 0; i < NPARS; i++) {
    for (j = 0; j < NPARS; j++) {
      fprintf (stderr, "%11.4e  ", matrix[i][j]);
    }
    fprintf (stderr, "  %11.4e  ", vector[i][0]);
    fprintf (stderr, "  %11.4e  \n", vector[i][1]);
  }
# endif
  i = 0;
  for (m = 0; m < NPOWR; m++) {
    for (n = 0; n < NPOWR - m; n++, i++) {
      xsum[n][m] = vector[i][0];
      ysum[n][m] = vector[i][1];
    }	
  }
  i = 0;
  for (m = 0; m < NPOWR; m++) {
    for (n = 0; n < NPOWR - m; n++, i++) {
      fprintf (stderr, "RA x^%dy^%d: %10.4g    DEC x^%dy^%d: %10.4g \n", 
	       n, m, vector[i][0], n, m, vector[i][1]);
    }	
  }
}

void fit_norm () { 

  xsum[0][0] = 0;
  xsum[1][0] = 1;
  xsum[0][1] = 0;

  ysum[0][0] = 0;
  ysum[1][0] = 0;
  ysum[0][1] = 1;
}

void fit_apply (double *x, double *y, double X, double Y) {

  int m, n;
  double xterm, yterm;
  double Xo, Yo;

  Xo = Yo = 0;
  yterm = 1;
  for (m = 0; m < NPOWR; m++) { 
    xterm = 1;
    for (n = 0; n < NPOWR - m; n++) {
      Xo += xterm*yterm*xsum[n][m];
      Yo += xterm*yterm*ysum[n][m];
      xterm *= X;
    }	
    yterm *= Y;
  }
  
  *x = Xo;
  *y = Yo;
}


void pair_init () {

  Npair = 0;
  NPAIR = 100;
  ALLOCATE (idx1, int, NPAIR);
  ALLOCATE (idx2, int, NPAIR);

}

void pair_add (int i1, int i2) {

  idx1[Npair] = i1;
  idx2[Npair] = i2;

  Npair ++;

  if (Npair == NPAIR) {
    NPAIR += 100;
    REALLOCATE (idx1, int, NPAIR);
    REALLOCATE (idx2, int, NPAIR);
  }

}

double fit_scat (StarData *st, StarData *sr) {

  int i;
  double x, y, dx, dy, dX, dY, dX2, dY2, dR;
  
  dX = dY = dX2 = dY2 = 0;
  for (i = 0; i < Npair; i++) {

    fit_apply (&x, &y, sr[idx2[i]].X, sr[idx2[i]].Y);
    
    dx = x - st[idx1[i]].X;
    dy = y - st[idx1[i]].Y;
    
    dX += dx;
    dY += dy;
    dX2 += dx*dx;
    dY2 += dy*dy;
    
  }

  dX = dX / Npair;
  dY = dY / Npair;
  fprintf (stderr, "scatter: %f, %f\n", sqrt(dX2/Npair - dX*dX), sqrt(dY2/Npair - dY*dY));
  fprintf (stderr, "precise: %f, %f\n", sqrt((dX2/Npair - dX*dX) / Npair), sqrt((dY2/Npair - dY*dY)/Npair));

  dR = 0.5 * sqrt(fabs(dX2/Npair - dX*dX)) + 0.5 * sqrt (fabs(dY2/Npair - dY*dY));
  return (dR);
}

/* convert new terms to adjustments in coords and to polyterms */
int fit_adjust (Coords *coords) {

  int i, j, N;
  double S1, S2, p11, p12, p21, p22;
  double a0, a1, a2, b0, b1, b2, det;
  double X, Y;
  int Np, Nv;
    
  S1 = coords[0].cdelt1;
  S2 = coords[0].cdelt2;
  p11 = coords[0].pc1_1;    p12 = coords[0].pc1_2;
  p21 = coords[0].pc2_1;    p22 = coords[0].pc2_2;
    
  /* get the correct vector entries for the linear terms */
  N = mk_vector (0, 0, NORDER);
  a0 = vector[N][0];  b0 = vector[N][1];
  N = mk_vector (1, 0, NORDER);
  a1 = vector[N][0];  b1 = vector[N][1];
  N = mk_vector (0, 1, NORDER);
  a2 = vector[N][0];  b2 = vector[N][1];

  det = 1.0 / (a1*b2 - a2*b1);

  coords[0].pc1_1 = p11*a1 + p12*b1*(S2/S1);
  coords[0].pc2_1 = p21*a1 + p22*b1*(S2/S1);
    
  coords[0].pc1_2 = p12*b2 + p11*a2*(S1/S2);
  coords[0].pc2_2 = p22*b2 + p21*a2*(S1/S2);
    
  X = (coords[0].crpix1 - a0);
  Y = (coords[0].crpix2 - b0);
  coords[0].crpix1 = det*(X*b2 - Y*a2);
  coords[0].crpix2 = det*(Y*a1 - X*b1);

  coords[0].Npolyterms = NORDER;
  if (NORDER > 1) strcpy (coords[0].ctype, "DEC--PLY");

  /* generate higher order terms from vector */
  for (i = 0; i < NORDER + 1; i++) {
    for (j = 0; j < (NORDER - i + 1); j++) {
      if (i + j < 2) continue;
      Np = mk_polyterm (i, j, NORDER);
      Nv = mk_vector (i, j, NORDER);
      coords[0].polyterms[Np][0] = det*(vector[Nv][0]*b2  - vector[Nv][1]*a2);  /* x2 y0 */
      coords[0].polyterms[Np][1] = det*(vector[Nv][1]*a1  - vector[Nv][0]*b1);  /* x2 y0 */
    }
  }
  while (coords[0].crval1 < 0) coords[0].crval1 += 360.0;
  while (coords[0].crval1 > 360.0) coords[0].crval1 -= 360.0;

  /* test for valid solution */
  return (Npts);

}

int mk_polyterm (int n, int m, int norder) {
  
  int i, nt, N;
  
  N = 0;
  nt = n + m;
  for (i = 2; i < nt; i++) {
    N += i + 1;
  }
  N += m;
  return (N);
}

int mk_vector (int n, int m, int norder) {
  
  int i, N;
  
  N = 0;
  for (i = 0; i < m; i++) {
    N += (norder - i + 1);
  }
  N += n;
  return (N);
}

void plot_resid (StarData *st, StarData *sr) {

  int i;
  double x, y, dx, dy;
  float *xvect0, *yvect0, *xvect1, *yvect1, *xvect2, *yvect2;
  int Nvect;

  Nvect = Npair;
  ALLOCATE (xvect0, float, Nvect);
  ALLOCATE (yvect0, float, Nvect);
  ALLOCATE (xvect1, float, Nvect);
  ALLOCATE (yvect1, float, Nvect);

  ALLOCATE (xvect2, float, 2*Nvect);
  ALLOCATE (yvect2, float, 2*Nvect);
  
  for (i = 0; i < Npair; i++) {
    fit_apply (&x, &y, st[idx1[i]].X, st[idx1[i]].Y);
    
    dx = x - sr[idx2[i]].X;
    dy = y - sr[idx2[i]].Y;
    
    xvect0[i] = sr[idx2[i]].X;
    xvect1[i] = sr[idx2[i]].Y;
    yvect0[i] = dx;
    yvect1[i] = dy;

    xvect2[2*i+0] = sr[idx2[i]].X;
    yvect2[2*i+0] = sr[idx2[i]].Y;
    xvect2[2*i+1] = x;
    yvect2[2*i+1] = y;
  }

  plot_resid_plot (0, xvect0, yvect0, Nvect);
  plot_resid_plot (1, xvect1, yvect1, Nvect);
  plot_fullfield_pairs (xvect2, yvect2, 2*Nvect);

  free (xvect2);
  free (yvect2);
  free (xvect0);
  free (yvect0);
  free (xvect1);
  free (yvect1);

}
