# include "relastro.h"

static off_t   Nmax;
static double *X, *dX;
static double *Y, *dY;
static double *R, *dR;
static double *D, *dD;
static double *pX;
static double *pY;
static double *T;
static double *dT;

void initObjectData (Catalog *catalog, int Ncatalog) {

  off_t i, j;
  
  Nmax = 0;
  for (i = 0; i < Ncatalog; i++) {
    for (j = 0; j < catalog[i].Naverage; j++) {
      Nmax = MAX (Nmax, catalog[i].average[j].Nmeasure);
    }
  }

  ALLOCATE (R, double, MAX (1, Nmax));
  ALLOCATE (D, double, MAX (1, Nmax));
  ALLOCATE (T, double, MAX (1, Nmax));
  ALLOCATE (X, double, MAX (1, Nmax));
  ALLOCATE (Y, double, MAX (1, Nmax));

  ALLOCATE (dR, double, MAX (1, Nmax));
  ALLOCATE (dD, double, MAX (1, Nmax));
  ALLOCATE (dT, double, MAX (1, Nmax));
  ALLOCATE (dX, double, MAX (1, Nmax));
  ALLOCATE (dY, double, MAX (1, Nmax));

  ALLOCATE (pX, double, MAX (1, Nmax));
  ALLOCATE (pY, double, MAX (1, Nmax));
}  

int UpdateObjects (Catalog *catalog, int Ncatalog) {

  off_t j, k, m;
  int i, N, Nsecfilt, mode, result, status, XVERB;
  StatType statsR, statsD;
  Coords coords;
  PMFit fitAve, fitPM, fitPAR, fit;
  time_t T2000;
  off_t Nave, Npm, Npar, Nskip;
  off_t NaveSum, NpmSum, NparSum, NskipSum;
  double Tmin, Tmax, Tmean, Trange;

  memset (&fit,    0, sizeof(fit));
  memset (&fitAve, 0, sizeof(fitAve));
  memset (&fitPM,  0, sizeof(fitPM));
  memset (&fitPAR, 0, sizeof(fitPAR));
  
  initObjectData (catalog, Ncatalog);

  /* project coordinates to a plane centered on the object with units of arcsec */
  coords.crval1 = 0;
  coords.crval2 = 0;
  coords.crpix1 = 0;
  coords.crpix2 = 0;
  coords.cdelt1 = coords.cdelt2 = 1.0 / 3600.0;
  coords.pc1_1  = coords.pc2_2 = 1.0;
  coords.pc1_2  = coords.pc2_1 = 0.0;
  coords.Npolyterms = 1;
  strcpy (coords.ctype, "RA---SIN");

  XVERB = FALSE;

  // use J2000 as a reference time
  T2000 = ohana_date_to_sec ("2000/01/01");

  // XXX in the future, use catalog[0].Nsecfilt only?  allow catalogs to have variable Nsecfilt?
  Nsecfilt = GetPhotcodeNsecfilt ();
  assert (catalog[0].Nsecfilt == Nsecfilt);

  NaveSum = NparSum = NpmSum = NskipSum = 0;
  for (i = 0; i < Ncatalog; i++) {

    if (VERBOSE) fprintf (stderr, "astrometrize catalog %d : %lld ave, %lld meas\n", i, (long long) catalog[i].Naverage, (long long) catalog[i].Nmeasure);

    Nave = Npar = Npm = Nskip = 0;
    for (j = 0; j < catalog[i].Naverage; j++) {
      /* calculate the average value of R,D for a single star */

      // skip objects which are known to be problematic
      // XXX include this code or not?
# if (0)
      if (catalog[i].average[j].code & STAR_BAD) {
	Nskip ++;
	continue;  
      }
# endif

      N = 0;
      m = catalog[i].average[j].measureOffset;

      Tmin = Tmax = (catalog[i].measure[m].t - T2000) / (86400*365.25);
      mode = FIT_MODE;

      // find the basic properties of the detections for this object (Tmin, Tmax, Tmean)
      Tmean = 0;
      for (k = 0; k < catalog[i].average[j].Nmeasure; k++, m++) {

	//does the measurement pass the supplied filtering constraints?
	if (!MeasFilterTest(&catalog[i].measure[m])) {
	  catalog[i].measure[m].dbFlags &= ~ID_MEAS_USED_OBJ;
	  continue;
	}

	//outlier rejection
	if (FlagOutlier && (catalog[i].measure[m].dbFlags & ID_MEAS_POOR_ASTROM)) {
	  catalog[i].measure[m].dbFlags &= ~ID_MEAS_USED_OBJ;
	  continue;
	}

	// exclude measurements by previous outlier detection
	// XXX include this code or not?
# if (0)
	if (catalog[i].measure[m].dbFlags & MEAS_BAD) { 
	  catalog[i].measure[m].dbFlags |= ID_MEAS_SKIP_ASTROM;
	  continue;
	}
# endif

	catalog[i].measure[m].dbFlags |= ID_MEAS_USED_OBJ;

	R[N] = getMeanR (&catalog[i].measure[m], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	D[N] = getMeanD (&catalog[i].measure[m], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	T[N] = (catalog[i].measure[m].t - T2000) / (86400*365.25) ; // time relative to J2000 in years

	Tmin = MIN(Tmin, T[N]);
	Tmax = MAX(Tmax, T[N]);
	Tmean += T[N];

	// dX, dY : error in arcsec -- 
	dX[N] = GetAstromError (&catalog[i].measure[m], ERROR_MODE_RA);
	dY[N] = GetAstromError (&catalog[i].measure[m], ERROR_MODE_DEC);
	dT[N] = catalog[i].measure[m].dt;

	// XXX this is (slightly) inconsistent: dX,dY are the X and Y direction errors in
	// arcseconds.  dR, dD are the errors in those directions in degrees.  IF we have
	// non-circular errors (different values for X and Y), then dR and dD will be
	// incorrect: they would need to be rotated to take out the position angle
	dR[k] = dX[k] / 3600.0;
	dD[k] = dY[k] / 3600.0;

	N++;
      }

      // if we have too few good detections for the desired fit, or too limited a baseline, use a
      // fit with fewer parameters.  XXX if we have too few parameters for even the average
      // position, consider including the lower-quality detections

      catalog[i].average[j].flags &= ~ID_STAR_FEW;

      // XXX add the parallax factor range as a criterion as well
      Trange = Tmax - Tmin;
      if (Trange < PM_DT_MIN) mode = FIT_AVERAGE;
      if ((mode == FIT_PM_ONLY) && (N < PM_TOOFEW)) mode = FIT_AVERAGE;

      // too few measurements for average position (require 2 values)
      if (N < SRC_MEAS_TOOFEW) {
	// XXX need to define PHOTOM and ASTROM object flags
	// XXX reset the average value fields?
	catalog[i].average[j].flags |= ID_STAR_FEW;
	catalog[i].average[j].ChiSqAve  = NAN;
	catalog[i].average[j].ChiSqPM   = NAN;
	catalog[i].average[j].ChiSqPar  = NAN;
	if (N < 2) continue;
      }

      /* we need to do the fit in a locally linear space; choose a ref coordinate */
      coords.crval1 = R[0];
      coords.crval2 = D[0];
      Tmean /= (float) N;
      
      XVERB = FALSE && (catalog[i].measure[m].dM < 0.01) && (N == 6) && (mode == FIT_PM_ONLY);

      // to judge the quality of the PM and PAR fits, we need to fit all three models and compare Chisq

      if ((mode == FIT_PM_ONLY) || (mode == FIT_PM_AND_PAR)) {
	// project all of the R,D coordinates to a plane centered on this coordinate set
	// the times to be relative to Tmean (this is required for parallax as well)
	for (k = 0; k < N; k++) {
	  RD_to_XY (&X[k], &Y[k], R[k], D[k], &coords);
	  T[k] -= Tmean;
	  if (XVERB) {
	    fprintf (stderr, "%lld %f %f %f  %f %f +/- %f %f\n", (long long) k, T[k], R[k], D[k], X[k], Y[k], dX[k], dY[k]);
	  }
	}	  

	FitPM (&fitPM, X, dX, Y, dY, T, N);
	if (XVERB) fprintf (stderr, "fitted:  %f - %f : %f %f : %f %f : %f vs %f\n", Tmin, Tmax, fitPM.Ro, fitPM.Do, fitPM.uR, fitPM.uD, fitPM.chisq, fitAve.chisq);

	// project Ro, Do back to RA,DEC
	XY_to_RD (&fitPM.Ro, &fitPM.Do, fitPM.Ro, fitPM.Do, &coords);
	if (XVERB) fprintf (stderr, "project: %f %f : %f %f : %f\n", fitPM.Ro, fitPM.Do, fitPM.uR, fitPM.uD, fitPM.p);

	fitPM.p  = fitPM.dp  = 0.0;
	catalog[i].average[j].flags |= ID_STAR_FIT_PM;
	Npm ++;
      }

      if (mode == FIT_PM_AND_PAR) {
	fprintf (stderr, "parallax fitting is still untested (%s, %d)", __FILE__, __LINE__);
	exit (2);

	for (k = 0; k < N; k++) {
	  ParFactor (&pX[k], &pY[k], R[k], D[k], T[k], Tmean);
	}
	FitPMandPar (&fitPAR, X, dX, Y, dY, T, pX, pY, N);
	XY_to_RD (&fitPAR.Ro, &fitPAR.Do, fitPAR.Ro, fitPAR.Do, &coords);
	catalog[i].average[j].flags |= ID_STAR_FIT_PAR;
	Npar ++;
      }	  

      // fit the average model
      if ((mode == FIT_AVERAGE) || (mode == FIT_PM_ONLY) || (mode == FIT_PM_AND_PAR)) {
	liststats (R, dR, N, &statsR); // WARNING: this function modifies R (do not use after here)
	liststats (D, dD, N, &statsD); // WARNING: this function modifies D (do not use after here)

	fitAve.Ro = statsR.mean;
	fitAve.dRo = 3600.0*statsR.sigma;

	fitAve.Do = statsD.mean;
	fitAve.dDo = 3600.0*statsD.sigma;

	fitAve.chisq = 0.5*(statsR.chisq + statsD.chisq);
	fitAve.Nfit = N;

	fitAve.uR = fitAve.duR = 0.0;
	fitAve.uD = fitAve.duD = 0.0;
	fitAve.p  = fitAve.dp  = 0.0;
	catalog[i].average[j].flags |= ID_STAR_FIT_AVE;
	Nave ++;
      }

      /* choose the result based on the chisq values */
      // XXXX for now, just use the mode as the result:
      result = mode;

      switch (result) {
	case FIT_AVERAGE:
	  catalog[i].average[j].flags |= ID_STAR_USE_AVE;
	  fit = fitAve;
	  break;
	case FIT_PM_ONLY:
	  catalog[i].average[j].flags |= ID_STAR_USE_PM;
	  fit = fitPM;
	  break;
	case FIT_PM_AND_PAR:
	  catalog[i].average[j].flags |= ID_STAR_USE_PAR;
	  fit = fitPAR;
	  break;
      }

      if (XVERB) fprintf (stderr, "%f %f -> %f %f (%f,%f)\n",
			  catalog[i].average[j].R, 
			  catalog[i].average[j].D, 
			  fit.Ro, fit.Do, 
			  3600*(catalog[i].average[j].R - fit.Ro), 
			  3600*(catalog[i].average[j].D - fit.Do));

      //make sure that the fit succeeded
      status  = finite(fit.Ro);
      status &= finite(fit.Do);
      status &= finite(fit.dRo);
      status &= finite(fit.dDo);
      status &= finite(fit.uR);
      status &= finite(fit.uD);
      status &= finite(fit.duR);
      status &= finite(fit.duD);
      status &= finite(fit.p);
      status &= finite(fit.dp);
      if (!status) {
	Nskip ++;
	continue;
      }

      // the measure fields must be updated before the average fields
      m = catalog[i].average[j].measureOffset;
      for (k = 0; k < catalog[i].average[j].Nmeasure; k++, m++) {
	setMeanR (fit.Ro, &catalog[i].measure[m], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	setMeanD (fit.Do, &catalog[i].measure[m], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
      }      

      catalog[i].average[j].R  	= fit.Ro; // RA in degrees
      catalog[i].average[j].D  	= fit.Do; // DEC in degrees
      catalog[i].average[j].dR 	= fit.dRo; // RA scatter in arcsec
      catalog[i].average[j].dD 	= fit.dDo; // DEC scatter in arcsec

      catalog[i].average[j].uR  = fit.uR; // RA proper motion in arcsec/year
      catalog[i].average[j].uD  = fit.uD; // DEC proper motion in arcsec/year
      catalog[i].average[j].duR = fit.duR; // RA proper motion error in arcsec/year
      catalog[i].average[j].duD = fit.duD; // DEC proper motion error in arcsec/year

      catalog[i].average[j].P   = fit.p; // parallax in arcsec
      catalog[i].average[j].dP  = fit.dp; // parallax error in arcsec

      // Xp is supposed to be the position scatter, not the chisq : fix this:
      // catalog[i].average[j].Xp  = (fit.Nfit > 1) ? 100.0*log10(fit.chisq) : NAN_S_SHORT;
      catalog[i].average[j].ChiSqAve  = fitAve.chisq;
      catalog[i].average[j].ChiSqPM   = fitPM.chisq;
      catalog[i].average[j].ChiSqPar  = fitPAR.chisq;
      catalog[i].average[j].Xp        = 0.0;
      catalog[i].average[j].Tmean = (Tmean * 86400 * 365.26) + T2000;
      catalog[i].average[j].Trange = (Trange * 86400 * 365.26);
      catalog[i].average[j].Npos = fit.Nfit;
    }

    NaveSum += Nave;
    NpmSum += Npm;
    NparSum += Npar;
    NskipSum += Nskip;
    if (VERBOSE) fprintf (stderr, "catalog %lld : %lld ave, %lld pm, %lld par : Nskip %lld\n", (long long) i, (long long) Nave, (long long) Npm, (long long) Npar, (long long) Nskip);
  }

  fprintf (stderr, "fitted %lld objects (%lld ave, %lld pm, %lld par), skipped %lld\n", (long long) (NaveSum + NpmSum + NparSum), (long long) NaveSum, (long long) NpmSum, (long long) NparSum, (long long) NskipSum);
  return (TRUE);
}

/* fitting proper-motion and parallax:

   given a source at position r,d, at a time t, we need to calculate a vector (pr,pd)

   let x,y be the coordinate in the linearized frame with y parallel to DEC lines

   L,B are the ecliptic longitude and latitude of the object, 
   dL and dB are the offsets in the L and B directions

   dL = sin(t - topp)
   dB = cos(t - topp)*sin(B)

   these need to be rotated to the R,D frame to yield pR,pD.  Then, the equation of motion
   for the source in the x,y frame is:

   x = Ro + uR * (t - to) + p * pR 
   y = Do + uD * (t - to) + p * pD

   the unknowns in these equations are Ro, uR, Do, uD, and p

   XXX think through the concepts for the pole a bit better.  all objects near the pole 
   move the same way with the same phase.  choose a projection center and define dL,dB relative 
   to that center point coordinate system?

*/
