# 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) {

  int i, j, k, m, N, Nsecfilt;
  StatType statsR, statsD;
  Coords coords;
  PMFit fit;
  time_t To;
  int mode, Nave, Npm, Npar, Nskip;
  double Tmin, Tmax;

  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");

  // use J2000 as a reference time
  To = ohana_date_to_sec ("2000/01/01");
  Nave = Npar = Npm = 0;

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

  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);

    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 - To) / (86400*365.25);
      mode = FIT_MODE;

      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 - To) / (86400*365.25) ; // time relative to J2000 in years

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

	dR[N] = GetAstromError (&catalog[i].measure[m], ERROR_MODE_RA);
	dD[N] = GetAstromError (&catalog[i].measure[m], ERROR_MODE_DEC);
	dT[N] = catalog[i].measure[m].dt;

	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
      if ((Tmax - Tmin) < 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
	catalog[i].average[j].flags |= ID_STAR_FEW;
	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];
      
      /* project all of the R,D coordinates to a plane centered on this coordinate */
      for (k = 0; k < N; k++) {
	RD_to_XY (&X[k], &Y[k], R[k], D[k], &coords);
	dX[k] =  dR[k];
	dY[k] =  dD[k];
	// fprintf (stderr, "%d %f %f %f  %f %f\n", k, T[k], R[k], D[k], X[k], Y[k]);
      }	  

      /* fit the model components as needed */
      switch (mode) {
	case FIT_AVERAGE:
	  liststats (R, dR, N, &statsR);
	  liststats (D, dD, N, &statsD);

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

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

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

	  fit.uR = fit.duR = 0.0;
	  fit.uD = fit.duD = 0.0;
	  fit.p  = fit.dp  = 0.0;

	  Nave ++;
	  break;

	case FIT_PM_ONLY:
	  FitPM (&fit, X, dX, Y, dY, T, N);
	  // fprintf (stderr, "fitted:  %f - %f : %f %f : %f %f : %f\n", Tmin, Tmax, fit.Ro, fit.Do, fit.uR, fit.uD, fit.p);
	  // project Ro, Do back to RA,DEC
	  XY_to_RD (&fit.Ro, &fit.Do, fit.Ro, fit.Do, &coords);
	  // fprintf (stderr, "project: %f %f : %f %f : %f\n", fit.Ro, fit.Do, fit.uR, fit.uD, fit.p);
	  // continue;

	  fit.p  = fit.dp  = 0.0;

	  Npm ++;
	  break;

	case FIT_PAR_ONLY:
	  fprintf (stderr, "programming error at %s, %d", __FILE__, __LINE__);
	  exit (2);

	  for (k = 0; k < N; k++) {
	    ParFactor (&pX[k], &pY[k], R[k], D[k], T[k]);
	  }
	  FitPar (&fit, X, dX, Y, dY, pX, pY, N);

	  // project Ro, Do back to RA,DEC
	  XY_to_RD (&fit.Ro, &fit.Do, fit.Ro, fit.Do, &coords);

	  fit.uR = fit.duR = 0.0;
	  fit.uD = fit.duD = 0.0;

	  Npar ++;
	  break;

	case FIT_PM_AND_PAR:
	  fprintf (stderr, "programming error at %s, %d", __FILE__, __LINE__);
	  exit (2);

	  for (k = 0; k < N; k++) {
	    ParFactor (&pX[k], &pY[k], R[k], D[k], T[k]);
	  }
	  FitPMandPar (&fit, X, dX, Y, dY, T, pX, pY, N);
	  XY_to_RD (&fit.Ro, &fit.Do, fit.Ro, fit.Do, &coords);
	  Npar ++;
	  break;

	default:
	  fprintf (stderr, "programming error at %s, %d", __FILE__, __LINE__);
	  exit (2);
      }	  

      if (0 && (j < 100)) {
	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
      assert(finite(fit.Ro) && finite(fit.Do) && 
	     finite(fit.dRo) && finite(fit.dDo) &&
	     finite(fit.uR) && finite(fit.uD) &&
	     finite(fit.duR) && finite(fit.duD) &&
	     finite(fit.p) && finite(fit.dp));

      // 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++) {
	// XXX why was this here?? if (catalog[i].measure[m].dbFlags & MEAS_BAD) continue;
	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

      catalog[i].average[j].Xp  = (fit.Nfit > 1) ? 100.0*log10(fit.chisq) : NAN_S_SHORT;
    }

    if (VERBOSE) fprintf (stderr, "catalog %d : %d ave, %d pm, %d par : Nskip % d\n", i, Nave, Npm, Npar, Nskip);
  }

  if (VERBOSE) fprintf (stderr, "fitted %d objects (%d ave, %d pm, %d par)\n", Nave + Npm + Npar, Nave, Npm, Npar);
  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?

*/
