# include <ohana.h>
# include <dvo.h>
# define NCCD 12

static double **alpha, **talpha;
static double **beta, **tbeta;
static double *partry, *dyda;
static double ochisq, lambda;

double *x, *y, *p, *q;
double Px, Py, PS;
int PSfixed;

double chipfunc (int idx, double *P, double *Q, double *par, int Npar);
double chipfit (int idx, double *par, int Npar, double *dyda);
double mrqcof (int *index, int Npts, double *par, int Npar, double **ta, double **tb, double (funcs)(int, double *, int, double *));
double mrqmin (int *index, int Npts, double *par, int Npar,                           double (funcs)(int, double *, int, double *));
int mrqinit (int *index, int Npts, double *par, int Npar,                             double (funcs)(int, double *, int, double *));
int mrqfree (int Npar);
int fit (double *X, double *Y, int N, double *A, double *B);

main (int argc, char **argv) {

  int i, j, n;
  int Npar, Npts, NPTS, done;
  int *CCD, *idx;
  double Ro, Do, chisq, nchisq, gchisq, xf, yf;
  double Sx, Sy, Lo, Mo, Lp, Mp, dr, DSx, DSy;
  double *R, *D, *X, *Y, *L, *M, *tmp;
  double *par;
  FILE *f;
  Coords coords;
  double *Xo, *Yo, *To;

  /* load in RA, DEC, X, Y, CCD vectors */
  if ((argc != 2) && (argc != 7)) {
    fprintf (stderr, "USAGE: mosastro (file.mos) [Ro Do ps DSx DSy]\n");
    exit (2);
  }
  PS = DSx = DSy = 0.0;
  PSfixed = FALSE;
  if (argc == 7) {
    PSfixed = TRUE;
    Ro = atof (argv[2]);
    Do = atof (argv[3]);
    PS = atof (argv[4]);
    DSx = atof (argv[5]) / 1000000.0;
    DSy = atof (argv[6]) / 1e6;
  }
  
  ALLOCATE (Xo, double, NCCD);
  ALLOCATE (Yo, double, NCCD);
  ALLOCATE (To, double, NCCD);

  Npar = 4;
  if (PSfixed) Npar = 3;
  ALLOCATE (par, double, Npar);

  NPTS = 100;
  ALLOCATE (R, double, NPTS);
  ALLOCATE (D, double, NPTS);
  ALLOCATE (X, double, NPTS);
  ALLOCATE (Y, double, NPTS);
  ALLOCATE (CCD, int, NPTS);

  f = fopen (argv[1], "r");
  if (f == (FILE *) NULL) {
    fprintf (stderr, "can't open file %s\n", argv[1]);
    exit (1);
  }

  for (i = 0; fscanf (f, "%lf %lf %lf %lf %d", &R[i], &D[i], &X[i], &Y[i], &CCD[i]) != EOF; i++) {
    if (i == NPTS - 1) {
      NPTS += 100;
      REALLOCATE (R, double, NPTS);
      REALLOCATE (D, double, NPTS);
      REALLOCATE (X, double, NPTS);
      REALLOCATE (Y, double, NPTS);
      REALLOCATE (CCD, int, NPTS);
    }
  }
  NPTS = i;

  /* find median of RA and DEC for field center */
  if (!PSfixed) {
    ALLOCATE (tmp, double, NPTS);
    memcpy (tmp, R, NPTS*sizeof(double));
    sort (tmp, NPTS);
    Ro = tmp[(int)(0.5*NPTS)];
    memcpy (tmp, D, NPTS*sizeof(double));
    sort (tmp, NPTS);
    Do = tmp[(int)(0.5*NPTS)];
  }

  /* basic coords */
  strcpy (coords.ctype, "RA---TAN");
  coords.cdelt1 = coords.cdelt2 = 1.0 / 3600.0;
  coords.pc1_1  = 1.0;
  coords.pc2_2  = -1.0;
  coords.pc1_2  = coords.pc2_1 = 0.0;
  coords.crval1 = Ro;
  coords.crval2 = Do;
  coords.crpix1 = coords.crpix2 = 0;
  
  /* RA, DEC -> L, M: */
  /* only free parameters are Ro, Do */
  ALLOCATE (L, double, NPTS);
  ALLOCATE (M, double, NPTS);

  for (i = 0; i < NPTS; i++) {
    RD_to_XY (&Lp, &Mp, R[i], D[i], &coords);
    dr = hypot (Lp, Mp);
    L[i] = Lp * (1 + DSx*dr);
    M[i] = Mp * (1 + DSy*dr);
  }

  ALLOCATE (x, double, NPTS);
  ALLOCATE (y, double, NPTS);
  ALLOCATE (p, double, NPTS);
  ALLOCATE (q, double, NPTS);
  ALLOCATE (idx, int, NPTS);

  gchisq = 0;
  for (i = 0; i < NCCD; i++) {
    n = 0;
    for (j = 0; j < NPTS; j++) {
      if (CCD[j] == i) {
	x[n] = X[j];
	y[n] = Y[j];
	p[n] = L[j];
	q[n] = M[j];
	idx[n] = n;
	n ++;
      }
    }
    Npts = n;

    /* fit p = ax + b, q = ay + b */
    fit (x, p, Npts, &Sx, &Lo);
    fit (y, q, Npts, &Sy, &Mo);

    /* initial guesses for par[0], par[1], par[2], par[3] */
    par[0] = Lo;
    par[1] = Mo;
    par[2] = 0.0;
    if (!PSfixed) 
      par[3] = 0.5*fabs (Sx) + 0.5*fabs(Sy);

    Px = SIGN (Sx);
    Py = SIGN (Sy);
    /* fprintf (stderr, "%2d: %7.1f  %7.1f  %5.3f  %6.2f\n", i, par[0], par[1], par[2], par[3]); */

    chisq = mrqinit (idx, Npts, par, Npar, chipfit);
    
    done = FALSE;
    for (j = 0; (j < 20) && !done; j++) {
      nchisq = mrqmin (idx, Npts, par, Npar, chipfit);
      /* fprintf (stderr, "%7.1f: %7.1f  %7.1f  %5.3f  %6.2f\n", nchisq, par[0], par[1], par[2], par[3]); */
      /* if (chisq - nchisq < 0.1) done = TRUE; */
      if (nchisq > 0) chisq = nchisq;
    }  
    Xo[i] = par[0];
    Yo[i] = par[1];
    To[i] = par[2];
    gchisq += chisq;
    if (!PSfixed) PS += par[3];

    /*
    if (!PSfixed) 
      fprintf (stderr, "%2d: %7.1f  %7.1f  %6.2f  %7.5f\n", i, par[0], par[1], par[2], par[3]); 
    else 
      fprintf (stderr, "%2d: %7.1f  %7.1f  %6.2f  %7.5f\n", i, par[0], par[1], par[2], PS); 
    */

    for (j = 0; j < Npts; j++) {
      chipfunc (j, &xf, &yf, par, Npar);
      n = idx[j];
      fprintf (stdout, "%f %f  %f %f  %f %f\n", x[n], y[n], p[n], q[n], xf, yf);
    }

  }
  if (PSfixed) {
    fprintf (stderr, "%f   %f  %f  %f  %f %f\n", gchisq, Ro, Do, PS, 1e6*DSx, 1e6*DSy);
  } else {
    fprintf (stderr, "%f   %f  %f  %f  %f %f\n", gchisq, Ro, Do, PS / NCCD, 1e6*DSx, 1e6*DSy);
  }
  exit (0);
}

double mrqcof (int *index, int Npts, double *par, int Npar, double **ta, double **tb, 
	      double (funcs)(int, double *, int, double *)) {

  int k, j, i;
  double dchi, wt, chisq;

  for (j = 0; j < Npar; j++) {
    for (k = 0; k <= j; k++) ta[j][k] = 0.0;
    tb[j][0] = 0.0;
  }

  chisq = 0.0;
  for (i = 0; i < Npts; i++) {

    dchi = funcs (index[i], par, Npar, dyda);
    chisq += dchi;

    for (j = 0; j < Npar; j++) {
      wt = dyda[j];
      for (k = 0; k <= j; k++) ta[j][k] += wt * dyda[k];
      tb[j][0] += wt * dchi;
    }
  }

  for (j = 1; j < Npar; j++)
    for (k = 0; k < j; k++) 
      ta[k][j] = ta[j][k];

  return (chisq);

}

double mrqmin (int *index, int Npts, 
	      double *par, int Npar, 
	      double (funcs)(int, double *, int, double *)) {

  int i, j, k;
  double chisq;

  /* set up test matrixes for this run */
  for (j = 0; j < Npar; j++) {
    for (k = 0; k < Npar; k++) talpha[j][k] = alpha[j][k];
    talpha[j][j] = alpha[j][j] * (1.0 + lambda);
    tbeta[j][0] = beta[j][0];
  }

  gaussj (talpha, Npar, tbeta, 1);

  /* if we keep some fixed, set partry to just them 
     for (j = 0; j < Npar; j++) partry[j] = par[j]; */

  for (j = 0; j < Npar; j++) partry[j] = par[j] - tbeta[j][0];

  chisq = mrqcof (index, Npts, partry, Npar, talpha, tbeta, funcs);

  /* if good, save temp values */
  if (chisq < ochisq) {
    lambda *= 0.1;
    ochisq = chisq;
    for (j = 0; j < Npar; j++) {
      for (k = 0; k < Npar; k++) alpha[j][k] = talpha[j][k];
      beta[j][0] = tbeta[j][0];
      par[j] = partry[j];
    }
  } else {
    lambda *= 10.0;
    chisq = ochisq;
  }

  return (chisq);

}

int mrqinit (int *index, int Npts, double *par, int Npar, double (funcs)(int, double *, int, double *)) {

  int i;

  ALLOCATE (dyda, double, Npar);
  ALLOCATE (partry, double, Npar);
  ALLOCATE (alpha, double *, Npar);
  ALLOCATE (beta, double *, Npar);
  ALLOCATE (talpha, double *, Npar);
  ALLOCATE (tbeta, double *, Npar);
  for (i = 0; i < Npar; i++) {
    ALLOCATE (alpha[i], double, Npar);
    ALLOCATE (beta[i], double, Npar);
    ALLOCATE (talpha[i], double, Npar);
    ALLOCATE (tbeta[i], double, Npar);
  }
  lambda = 0.1;
  ochisq = mrqcof (index, Npts, par, Npar, alpha, beta, funcs);
}

int mrqfree (int Npar) {

  int i;

  for (i = 0; i < Npar; i++) {
    free (alpha[i]);
    free (talpha[i]);
    free (beta[i]);
    free (tbeta[i]);
  }
  free (alpha);
  free (talpha);
  free (beta);
  free (tbeta);
  free (partry);
  free (dyda);

}

int gaussj (double **a, int n, double **b, int m) {

  int *indxc,*indxr,*ipiv;
  int i, icol, irow, j, k, l, ll, status;
  double big,dum,pivinv;
  
  ALLOCATE (indxc, int, n);
  ALLOCATE (indxr, int, n);
  ALLOCATE (ipiv, int, n);
  for (j = 0; j < n; j++) 
    ipiv[j] = 0;

  for (i = 0; i < n; i++) {
    big = 0.0;
    for (j = 0; j < n; j++) {
      if (ipiv[j] != 1) {
	for (k = 0; k < n; k++) {
	  if (ipiv[k] == 0) {
	    if (fabs (a[j][k]) >= big) {
	      big  = fabs (a[j][k]);
	      irow = j;
	      icol = k;
	    }
	  } 
	  else 
	    if (ipiv[k] > 1) {
	      fprintf (stderr, "GAUSSJ: Singular Matrix! (1)\n");
	      return (0);
	    }
	}
      }
    }
    ipiv[icol]++;
    if (irow != icol) {
      for (l = 0; l < n; l++) 
	SWAP (a[irow][l], a[icol][l]);
      for (l = 0; l < m; l++) 
	SWAP (b[irow][l], b[icol][l]);
    }
    indxr[i] = irow;
    indxc[i] = icol;
    if (a[icol][icol] == 0.0) {
      fprintf (stderr, "GAUSSJ: Singular Matrix! (2)\n");
      return (0);
    }
    pivinv = 1.0 / a[icol][icol];
    a[icol][icol] = 1.0;
    for (l = 0; l < n; l++) 
      a[icol][l] *= pivinv;
    for (l = 0; l < m; l++) 
      b[icol][l] *= pivinv;
    for (ll = 0; ll < n; ll++) {
      if (ll != icol) {
	dum = a[ll][icol];
	a[ll][icol] = 0.0;
	for (l = 0; l < n; l++) 
	  a[ll][l] -= a[icol][l]*dum;
	for (l = 0; l < m; l++) 
	  b[ll][l] -= b[icol][l]*dum;
      }
    }
  }

  for (l = n - 1; l >= 0; l--) {
    if (indxr[l] != indxc[l])
      for (k = 0; k < n; k++)
	SWAP (a[k][indxr[l]], a[k][indxc[l]]);
  }
  free (ipiv);
  free (indxr);
  free (indxc);
  return (1);
}

  /* par:
     par[0] = Lo
     par[1] = Mo
     par[2] = ps
     par[3] = theta
  */

double chipfunc (int idx, double *P, double *Q, double *par, int Npar) {
  double Lo, Mo, ps, cs, sn;
  double X, Y;

  Lo = par[0];
  Mo = par[1];
  cs = cos (par[2]*RAD_DEG);
  sn = sin (par[2]*RAD_DEG);
  if (PSfixed) 
    ps = PS;
  else
    ps = par[3];

  X = x[idx];
  Y = y[idx];

  *P = Lo + Px * ps * (X*cs + Y*sn);
  *Q = Mo + Py * ps * (Y*cs - X*sn);
}

double chipfit (int idx, double *par, int Npar, double *dyda) {

  double Lo, Mo, ps, cs, sn;
  double X, Y, L, M, P, Q;
  double chisq, dp, dq;

  Lo = par[0];
  Mo = par[1];
  cs = cos (par[2]*RAD_DEG);
  sn = sin (par[2]*RAD_DEG);
  if (PSfixed) 
    ps = PS;
  else
    ps = par[3];

  X = x[idx];
  Y = y[idx];
  L = p[idx];
  M = q[idx];

  P = Lo + Px * ps * (X*cs + Y*sn);
  Q = Mo + Py * ps * (Y*cs - X*sn);

  chisq = SQ(L-P) + SQ(M-Q);

  dp = P - L;
  dq = Q - M;

  dyda[0] = 2*dp;
  dyda[1] = 2*dq;
  
  dyda[2] =  2*RAD_DEG*ps*(dp*Px*(Y*cs - X*sn) - dq*Py*(X*cs + Y*sn));
  if (!PSfixed)
    dyda[3] =  2   *(dp*Px*(X*cs + Y*sn) + dq*Py*(Y*cs - X*sn));
  
  return (chisq);
}

sort (double *value, int N) {

  int l,j,ir,i;
  double temp;
  
  if (N < 2) return;
  l = N >> 1;
  ir = N - 1;
  for (;;) {
    if (l > 0) {
      temp = value[--l];
    }
    else {
      temp = value[ir];
      value[ir] = value[0];
      if (--ir == 0) {
	value[0] = temp;
	return;
      }
    }
    i = l;
    j = (l << 1) + 1;
    while (j <= ir) {
      if (j < ir && value[j] < value[j+1]) ++j;
      if (temp < value[j]) {
	value[i]=value[j];
	j += (i=j) + 1;
      }
      else j = ir + 1;
    }
    value[i] = temp;
  }
}


fit (double *X, double *Y, int N, double *A, double *B) {

  int i;
  double **a, **b;

  ALLOCATE (a, double *, 2);
  ALLOCATE (b, double *, 2);
  ALLOCATE (a[0], double, 2);
  ALLOCATE (a[1], double, 2);
  ALLOCATE (b[0], double, 1);
  ALLOCATE (b[1], double, 1);
  a[0][0] = a[0][1] = a[1][0] = a[1][1] = 0;
  b[0][0] = b[1][0] = 0;

  for (i = 0; i < N; i++) {
    a[0][0] += X[i]*X[i];
    a[0][1] += X[i];
    a[1][1] += 1;
    b[0][0] += X[i]*Y[i];
    b[1][0] += Y[i];
  }

  a[1][0] = a[0][1];
  
  gaussj (a, 2, b, 1);

  *A = b[0][0];
  *B = b[1][0];
}
