# 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;
static double *C_blue;
static double *C_red;

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

  ALLOCATE (C_blue, double, MAX (1, Nmax));
  ALLOCATE (C_red,  double, MAX (1, Nmax));
}  

void freeObjectData () {

  free (R);
  free (D);
  free (T);
  free (X);
  free (Y);

  free (dR);
  free (dD);
  free (dT);
  free (dX);
  free (dY);

  free (pX);
  free (pY);

  free (C_blue);
  free (C_red);
}  

// This function operates on both Measure and MeasureTiny.  In the big stages, this should
// be called with just MeasureTiny set and Measure == NULL
int UpdateObjects (Catalog *catalog, int Ncatalog) {

  off_t j, k, m;
  int i, N, NcBlue, NcRed, Nsecfilt, mode, result, status, XVERB;
  StatType statsR, statsD;
  Coords coords;
  PMFit fitAve, fitPM, fitPAR, fit;
  time_t T2000;
  off_t Nave, Npm, Npar, Nskip, Noffset;
  off_t NaveSum, NpmSum, NparSum, NskipSum, NoffSum;
  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);

  int setRefColor = areImagesMatched();

  /* 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, "DEC--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 ();
  if (Ncatalog) {
    assert (catalog[0].Nsecfilt == Nsecfilt);
  }

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

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

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

      XVERB = FALSE;
      fitAve.chisq = NAN;
      fitPM.chisq = NAN;
      fitPAR.chisq = NAN;

      // if we fail to fit the astrometry for some reason, we need to set/reset these
      catalog[i].average[j].flags |= ID_STAR_NO_ASTROM;
      catalog[i].average[j].ChiSqAve  = NAN;
      catalog[i].average[j].ChiSqPM   = NAN;
      catalog[i].average[j].ChiSqPar  = NAN;
      catalog[i].average[j].Npos = 0;

      if (catalog[i].average[j].Nmeasure == 0) {
	continue;
      }

      NcBlue = 0;
      NcRed = 0;
      N = 0;
      m = catalog[i].average[j].measureOffset;
      MeasureTiny *measure = &catalog[i].measureT[m];
      Measure *measureBig = catalog[i].measure ? &catalog[i].measure[m] : NULL;
      // when we update the output measure values, we need to do it here

      mode = FIT_MODE;

      // XVERB |= (catalog[i].averge[j].objID == 0xc90) && (catalog[i].average[j].catID == 0x2a1e);
      XVERB |= (catalog[i].average[j].objID == OBJ_ID_SRC) && (catalog[i].average[j].catID == CAT_ID_SRC);
      XVERB |= (catalog[i].average[j].objID == OBJ_ID_DST) && (catalog[i].average[j].catID == CAT_ID_DST);

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

	if (XVERB) {
	  char *date = ohana_sec_to_date (measure[k].t);
	  int dbFlagsBig = measureBig ? measureBig[k].dbFlags : 0;
	  fprintf (stderr, OFF_T_FMT" %f %f %s : 0x%08x : 0x%08x\n",  k, measure[k].R, measure[k].D, date, measure[k].dbFlags, dbFlagsBig);
	  free (date);
	}

	// reset the bit to note that a detection was used (or not)
	measure[k].dbFlags &= ~ID_MEAS_USED_OBJ;
	if (measureBig) { measureBig[k].dbFlags &= ~ID_MEAS_USED_OBJ; }

	// does the measurement pass the supplied filtering constraints?
	// MeasFilterTestTiny does not test psfQF
	// exclude bad detections based on: photcodes, psfQF, time range, photflags & astromBadMask, mag_inst
	int keepMeasure = measureBig ? MeasFilterTest(&measureBig[k], FALSE) : MeasFilterTestTiny(&measure[k], FALSE);
	if (!keepMeasure) {
	  continue;
	}

	// mark (as POOR) any measurements which are deviant from the mean by > ExcludeBogusRadius
	if (ExcludeBogus) {
	  double Ri = getMeanR (&measure[k], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	  double Di = getMeanD (&measure[k], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	  coords.crval1 = catalog[i].average[j].R;
	  coords.crval2 = catalog[i].average[j].D;
	  double Xi, Yi;
	  RD_to_XY (&Xi, &Yi, Ri, Di, &coords);
	  double radius = hypot(Xi, Yi);
	  if (radius > ExcludeBogusRadius) {
	      measure[k].dbFlags |= ID_MEAS_POOR_ASTROM;
	      if (measureBig) { measureBig[k].dbFlags |= ID_MEAS_POOR_ASTROM; }
	      continue;
	  }
	  measure[k].dbFlags &= ~ID_MEAS_POOR_ASTROM;
	  if (measureBig) { measureBig[k].dbFlags &= ~ID_MEAS_POOR_ASTROM; }
	}

	// outlier rejection
	if (FALSE && FlagOutlier && (measure[k].dbFlags & ID_MEAS_POOR_ASTROM)) {
	  continue;
	}

	R[N] = getMeanR (&measure[k], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	D[N] = getMeanD (&measure[k], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);

	// XXX I think this is a problem: T[] is time in years relative to J2000, but ParFactor expects
	// to get Time in years relative to UNIX Tzero (1970/01/01)
	T[N] = (measure[k].t - T2000) / (86400*365.25) ; // time relative to J2000 in years

	// dX, dY : error in arcsec -- 
	dX[N] = GetAstromErrorTiny (&measure[k], ERROR_MODE_RA);
	dY[N] = GetAstromErrorTiny (&measure[k], ERROR_MODE_DEC);

	// allow a given photcode or measurement to be
	// ignored if the error is NAN (for photcode, set astromErrSys to NaN)
	if (isnan(dX[N])) continue;
	if (isnan(dY[N])) continue;

	// add systematic error in quadrature, if desired
	// only do this after the fit has converged (or you will never improve the poor images)
	// if (INCLUDE_SYS_ERR) {
	// float dRsys = FromShortPixels(measure[k].dRsys);
	// dX[N] = hypot(dX[N], dRsys);
	// dY[N] = hypot(dY[N], dRsys);
	// }

	// dX[N] = 0.1;
	// dY[N] = 0.1;

	dT[N] = measure[k].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[N] = dX[N] / 3600.0;
	dD[N] = dY[N] / 3600.0;

	if (setRefColor) {
	  float colorBlue = getColorBlue (m+k, i);
	  if (!isnan(colorBlue)) {
	    C_blue[NcBlue] = colorBlue;
	    NcBlue++;
	  }
	  float colorRed = getColorRed (m+k, i);
	  if (!isnan(colorRed)) {
	    C_red[NcRed] = colorRed;
	    NcRed++;
	  }
	}

	measure[k].dbFlags |= ID_MEAS_USED_OBJ;
	if (measureBig) { measureBig[k].dbFlags |= ID_MEAS_USED_OBJ; }

	N++;
      } // loop over measurements : catalog[i].average[j].Nmeasure 

      // 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 measurements
      // for even the average position, consider including the lower-quality detections?

      // find Tmin & Tmax from the list of accepted measurements
      Tmean = 0;
      Tmin = Tmax = T[0];
      for (k = 0; k < N; k++) {
	Tmin = MIN(Tmin, T[k]);
	Tmax = MAX(Tmax, T[k]);
	Tmean += T[k];
      }
      // 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) || (mode == FIT_PM_AND_PAR)) && (N <= PM_TOOFEW)) mode = FIT_AVERAGE;

      if (RELASTRO_OP == OP_HIGH_SPEED) {
	  Tmean = 0.5*(Tmax - Tmin);
      } else {
	  Tmean /= (float) N;
      }

      // too few measurements for average position (require 2 values)
      if (N < SRC_MEAS_TOOFEW) {
	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];

      // 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, OFF_T_FMT" %f %f %f  %f %f +/- %f %f\n",  k, T[k], R[k], D[k], X[k], Y[k], dX[k], dY[k]);
	  }
	}	  

	FitPM (&fitPM, X, dX, Y, dY, T, N, XVERB);

	if (XVERB) fprintf (stderr, "fitted PM:  %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 ++;

	// XXX a hard-wired hack...
	if ((fabs(fitPM.uR) > 2.0) || (fabs(fitPM.uD) > 2.0)) {
	  mode = FIT_AVERAGE;
	  catalog[i].average[j].flags |= ID_STAR_BAD_PM;
	}
      }

      if (mode == FIT_PM_AND_PAR) {
	float pXmin = +2.0;
	float pXmax = -2.0;
	float pYmin = +2.0;
	float pYmax = -2.0;
	for (k = 0; k < N; k++) {
	  ParFactor (&pX[k], &pY[k], R[k], D[k], T[k], Tmean);
	  pXmin = MIN (pXmin, pX[k]);
	  pXmax = MAX (pXmax, pX[k]);
	  pYmin = MIN (pYmin, pY[k]);
	  pYmax = MAX (pYmax, pY[k]);
	}
	float dXRange = pXmax - pXmin;
	float dYRange = pYmax - pYmin;
	float parRange = hypot (dXRange, dYRange);
	
# define PAR_TOOFEW 5
	if ((parRange >= PAR_FACTOR_MIN) && (N > PAR_TOOFEW)) {
	  FitPMandPar (&fitPAR, X, dX, Y, dY, T, pX, pY, N, XVERB);
	  if (XVERB) fprintf (stderr, "fitted PM+PAR:  %f - %f : %f %f : %f %f : %f %f : %f vs %f vs %f\n", Tmin, Tmax, fitPAR.Ro, fitPAR.Do, fitPAR.uR, fitPAR.uD, fitPAR.p, fitPAR.dp, fitPAR.chisq, fitPM.chisq, fitAve.chisq);

	  XY_to_RD (&fitPAR.Ro, &fitPAR.Do, fitPAR.Ro, fitPAR.Do, &coords);
	  catalog[i].average[j].flags |= ID_STAR_FIT_PAR;
	  Npar ++;

	  // XXX a hard-wired hack...
	  if ((fabs(fitPAR.uR) > 2.0) || (fabs(fitPAR.uD) > 2.0)) {
	    mode = FIT_AVERAGE;
	    catalog[i].average[j].flags |= ID_STAR_BAD_PM;
	  }
	} else {
	  // need to set mode = FIT_PM_ONLY if we do not fit for parallax
	  mode = FIT_PM_ONLY;
	}
      }	  

      // fit the average model
      if ((mode == FIT_AVERAGE) || (mode == FIT_PM_ONLY) || (mode == FIT_PM_AND_PAR)) {
	liststats_pos (R, dR, N, &statsR, XVERB); // WARNING: this function modifies R (do not use after here)
	liststats_pos (D, dD, N, &statsD, XVERB); // 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 ++;
      }

      if (setRefColor) {
	float colorMedian;
	dsort (C_blue, NcBlue);
	colorMedian = (NcBlue > 0) ? C_blue[(int)(0.5*NcBlue)] : NAN;
	catalog[i].average[j].refColorBlue = colorMedian;
	dsort (C_red, NcRed);
	colorMedian = (NcRed > 0) ? C_red[(int)(0.5*NcRed)] : NAN;
	catalog[i].average[j].refColorRed = colorMedian;
      }

      /* 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) pm=(%f %f) plx=(%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),
			  fit.uR, fit.uD, fit.p, fit.dp);

      // make sure that the fit succeeded
      status = TRUE;
      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;
      }

      // what is the offset relative to the mean fit position?
      coords.crval1 = catalog[i].average[j].R;
      coords.crval2 = catalog[i].average[j].D;

      double dXoff, dYoff;
      RD_to_XY (&dXoff, &dYoff, fit.Ro, fit.Do, &coords);
      float dPos = hypot (dXoff, dYoff);
      if (dPos > MaxMeanOffset) {
	if (Noffset < 100) {
	  fprintf (stderr, "(%f,%f) -> (%f,%f) (%f,%f)\n", coords.crval1, coords.crval2, fit.Ro, fit.Do, dXoff, dYoff);
	}
	Noffset ++;
	continue;
      }


      // the measure fields must be updated before the average fields
      for (k = 0; k < catalog[i].average[j].Nmeasure; k++) {
	setMeanR (fit.Ro, &measure[k], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	setMeanD (fit.Do, &measure[k], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	if (measureBig) {
	  setMeanR_Big (fit.Ro, &measureBig[k], &catalog[i].average[j], &catalog[i].secfilt[j*Nsecfilt]);
	  setMeanD_Big (fit.Do, &measureBig[k], &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].ChiSqAve  = fitAve.chisq;
      catalog[i].average[j].ChiSqPM   = fitPM.chisq;
      catalog[i].average[j].ChiSqPar  = fitPAR.chisq;
      catalog[i].average[j].Tmean = (Tmean * 86400 * 365.25) + T2000;
      catalog[i].average[j].Trange = (Trange * 86400 * 365.25);
      catalog[i].average[j].Npos = fit.Nfit;

      // unset the NO_ASTROM bit (not(NO_ASTROM) == HAVE_ASTROM)
      catalog[i].average[j].flags &= ~ID_STAR_NO_ASTROM;

      if (XVERB) fprintf (stderr, "%f %f -> %f %f (%f,%f) pm=(%f %f) chisq=(%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),
                          catalog[i].average[j].uR,
                          catalog[i].average[j].uD,
			  fitAve.chisq, fitPM.chisq, fitPAR.chisq);
    }

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

  freeObjectData ();

  if (VERBOSE) fprintf (stderr, "fitted "OFF_T_FMT" objects ("OFF_T_FMT" ave, "OFF_T_FMT" pm, "OFF_T_FMT" par), skipped "OFF_T_FMT", "OFF_T_FMT" have too large an offset\n",  (NaveSum + NpmSum + NparSum),  NaveSum,  NpmSum,  NparSum,  NskipSum, NoffSum);
  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?

*/
