# include "relastro.h"

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

  int i;

  double **A, **B;
  double wx, wy, Wx, Wy, Tx, Ty, Tx2, Ty2, Xs, Ys, XT, YT;
  double PR, PD, PRT, PDT, PRX, PDY, PR2, PD2;
  double chisq, Xf, Yf;

  A = array_init (5, 5);
  B = array_init (5, 1);

  PR = PD = PRT = PDT = PRX = PDY = PR2 = PD2 = 0.0;
  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;
    
    PR += pR[i]*wx;
    PD += pD[i]*wy;
    
    PRT += pR[i]*T[i]*wx;
    PDT += pD[i]*T[i]*wy;
    
    PRX += pR[i]*X[i]*wx;
    PDY += pD[i]*Y[i]*wy;
    
    PR2 += SQ(pR[i])*wx;
    PD2 += SQ(pD[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[0][4] = PR;

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

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

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

  A[4][0] = PR;
  A[4][1] = PRT;
  A[4][2] = PD;
  A[4][3] = PDT;
  A[4][4] = PR2 + PD2;

  B[0][0] = Xs;
  B[1][0] = XT;
  B[2][0] = Ys;
  B[3][0] = YT;
  B[4][0] = PRX + PDY;

  dgaussjordan (A, B, 5, 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  = B[4][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  = sqrt(A[4][4]);
  
  array_free (A, 5);
  array_free (B, 5);

  /* get the chisq from the matrix values */

  // 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] + fit[0].p*pR[i];
    Yf = fit[0].Do + fit[0].uD*T[i] + fit[0].p*pD[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 - 5)
  fit[0].chisq = chisq / (2.0*Npts - 5.0);
  return (TRUE);
}
