# include "relastro.h"

/* do we want an init function which does the alloc and a clear function to free? */
int FitPM (PMFit *fit, double *X, double *dX, double *Y, double *dY, double *T, int Npts, int XVERB) {

  int i;

  double **A, **B;
  double wx, wy, Wx, Wy, Tx, Ty, Tx2, Ty2, Xs, Ys, XT, YT;
  double chisq, Xf, Yf;

  /* do I need to do this as 2 2x2 matrix equations? */
  A = array_init (4, 4);
  B = array_init (4, 1);

  Wx = Wy = Tx = Ty = Tx2 = Ty2 = Xs = Ys = XT = YT = 0.0;
  for (i = 0; i < Npts; i++) {
    /* handle case where dX or dY = 0.0 */
    wx = 1.0 / SQ(dX[i]);
    wy = 1.0 / SQ(dY[i]);

    Wx += wx;
    Wy += wy;

    Tx += T[i]*wx;
    Ty += T[i]*wy;
    
    Tx2 += SQ(T[i])*wx;
    Ty2 += SQ(T[i])*wy;
    
    Xs += X[i]*wx;
    Ys += Y[i]*wy;

    XT += X[i]*T[i]*wx;
    YT += Y[i]*T[i]*wy;
  }

  A[0][0] = Wx;
  A[0][1] = Tx;

  A[1][0] = Tx;
  A[1][1] = Tx2;

  A[2][2] = Wy;
  A[2][3] = Ty;

  A[3][2] = Ty;
  A[3][3] = Ty2;

  B[0][0] = Xs;
  B[1][0] = XT;
  B[2][0] = Ys;
  B[3][0] = YT;

  dgaussjordan (A, B, 4, 1);

  fit[0].Ro = B[0][0];
  fit[0].uR = B[1][0];
  fit[0].Do = B[2][0];
  fit[0].uD = B[3][0];
  fit[0].p  = 0.0;
  
  fit[0].dRo = sqrt(A[0][0]);
  fit[0].duR = sqrt(A[1][1]);
  fit[0].dDo = sqrt(A[2][2]);
  fit[0].duD = sqrt(A[3][3]);
  fit[0].dp  = 0.0;
  
  array_free (A, 4);
  array_free (B, 4);

  // add up the chi square for the fit
  chisq = 0.0;
  for (i = 0; i < Npts; i++) {
    Xf = fit[0].Ro + fit[0].uR*T[i];
    Yf = fit[0].Do + fit[0].uD*T[i];
    chisq += SQ(X[i] - Xf) / SQ(dX[i]);
    chisq += SQ(Y[i] - Yf) / SQ(dY[i]);
    if (XVERB) fprintf (stderr, "chisq contrib : %f %f : %f %f : %f %f : %f %f : %f\n", Xf, Yf, X[i] - Xf, Y[i] - Yf, dX[i], dY[i], (X[i] - Xf) / dX[i], (Y[i] - Yf) / dY[i], chisq);
  }
  fit[0].Nfit = Npts;

  // the reduced chisq is divided by (Ndof = 2*Npts - 4)
  fit[0].chisq = chisq / (2.0*Npts - 4.0);
  return (TRUE);
}

// XXX this function should (optionally?) iterate and clip outlier detections
