# include "pmAstrom.h"

// input: st1, st2 are psArray of psPlane? pmAstromObj

static double maxOffset;  // maximum allowed offset between lists, in raw pixels
static double Scale;      // grid pixel scale
static double Offset;     // deltas to pixels

bool p_pmAstromGridBin (int *pOut, int *qOut, double dP, int dQ) {

    if (fabs(dP) > maxOffset) return false;
    if (fabs(dQ) > maxOffset) return false;

    *pOut = dP / Scale + Offset;
    *qOut = dQ / Scale + Offset;
    return true;
}

/* Illustration of the grid bins
 dP        bin
-35:-25 -> 0     bin = dP / Scale + Offset
-25:-15 -> 1     Scale = 10
-15:-05 -> 2     Offset = 3.5
-05:+05 -> 3     nPix = 3 (maxOffset = 35 = (nPix + 0.5)*dPix
+05:+15 -> 4     dPix = 10
+15:+25 -> 5
+25:+35 -> 6

maxOffsetRequest = 30
nPix = (int) (maxOffset / dPix + 0.5);
maxOffset = (nPix + 0.5)*Scale;
*/

psArray *pmAstromGridMatch (psArray *raw, psArray *ref, psMetadata *options) {

    pmAstromGridMatchStat minStat, newStat

    // find center of the st2 field (focal-plane coords)
    pMin = 1e10; pMax = -1e10;
    qMin = 1e10; qMax = -1e10;
    for (int i = 0; i < st2->n; i++) {
	ob2 = (pmAstromObj *)st2->data[i];
	pMin = PS_MIN (ob2->P, pMin);
	pMax = PS_MAX (ob2->P, pMax);
	qMin = PS_MIN (ob2->Q, qMin);
	qMax = PS_MAX (ob2->Q, qMax);
    }
    pCenter = 0.5*(pMin + pMax);
    qCenter = 0.5*(qMin + qMax);

    minStat.minMetric = 1e10;
    for (angle = opt->minAngle; angle <= opt->maxAngle; angle += opt->delAngle) {
	st2r = pmAstromRotateObj (st2, angle, pCenter, qCenter);
	newStat = pmAstromGridMatchAngle (st1, st2r, opt);
	newStat.angle = angle;
	if (newStat.minMetric < minStat.minMetric) {
	    minStat = newStat;
	}
    }
    return (minStat);
}

// match the two lists using the binned delta-delta max
pmAstromGridMatchStats pmAstromGridMatchAngle (psArray *st1, psArray *st2, pmAstromOpt *opt) {

    // input lists must be projected to common focal plane
    pmAstromGridMatchStats matchStats;
    
    // set the static scaling factors
    nPixHalf = (int)(opt->gridOffset / opt->gridScale + 0.5);
    nPix     = 2*nPixHalf + 1;

    maxOffpix = opt->gridScale * (nPix + 0.5);
    Offset    = maxOffpix / opt->gridScale; 
    Scale     = opt->gridScale;

    // ** can we assume the allocated image is init-ed?
    gridNP = psImageAlloc (nPix, nPix, PS_TYPE_S32);
    gridDP = psImageAlloc (nPix, nPix, PS_TYPE_F32);
    gridDQ = psImageAlloc (nPix, nPix, PS_TYPE_F32);
    gridD2 = psImageAlloc (nPix, nPix, PS_TYPE_F32);
    psImageInit (gridNP);
    psImageInit (gridDP);
    psImageInit (gridDQ);
    psImageInit (gridD2);

    // short-cut names for grid images
    NP = gridNP->data.F32;
    DP = gridDP->data.F32;
    DQ = gridDQ->data.F32;
    D2 = gridD2->data.F32;

    // accumulate grids for focal plane (L,M) matches
    for (int i = 0; i < st1->n; i++) {
	ob1 = (pmAstromObj *)st1->data[i];
	for (int j = 0; j < st2->n; j++) {
	    ob2 = (pmAstromObj *)st2->data[i];
	    dP = ob1->P - ob2->P;
	    if (fabs(dP) > maxOffset) continue;

	    dQ = ob1->Q - ob2->Q;
	    if (fabs(dQ) > maxOffset) continue;

	    // find bin coordinates for this delta-delta
	    p_pmAstromGridBin (&iP, &iQ, dP, dQ);
		
	    // accumulate dP2, dQ2? 
	    NP[iQ][iP] ++;
	    DP[iQ][iP] += dP;
	    DQ[iQ][iP] += dQ;
	    D2[iQ][iP] += PS_SQR(dP) + PS_SQR(dQ);
	}
    }

    // find the max pixel
    stats = psStatsAlloc (PS_STAT_MAX);
    stats = psImageStats (stats, gridNP);

    // only check bins with at least 1/2 of max bin
    minNpt = 0.5*stats->max;

    // find the 'best' bin
    minMetric = minVar = 1e10;
    minP = minQ = -1;
    for (int j = 0; j < gridNP->nY; j++) {
	for (int i = 0; i < gridNP->nX; i++) {

	    if (NP[j][i] < minNpts) continue;

	    // this metric emphasize a narrow peak with lots of sources over one with few.
	    var = fabs((D2[j][i]/NP[j][i]) - PS_SQR(DP[j][i]/NP[j][i]) - PS_SQR(DQ[j][i]/NP[j][i]));
	    metric = var / PS_SQR(PS_SQR(NP[j][i]));
	    
	    if (metric < minMetric) {
		minMetric = metric;
		minVar    = var;
		minP      = i;
		minQ      = j;
	    }
	}
    }

    // convert the bin to delta-delta
    matchStats.dP        = DP[minQ][minP] / NP[minQ][minP];
    matchStats.dQ        = DQ[minQ][minP] / NP[minQ][minP];
    matchStats.minMetric = minMetric;
    matchStats.minVar    = minVar;
    matchStats.nMatch    = NP[minQ][minP];

    return (matchStats);
}

pmAstromMatch *pmAstromRadiusMatch (psArray *st1, psArray *st2, pmAstromOpt *opt) {

    // assume the lists are sorted, or sort in place?

    // sort st1 by P
    // sort st2 by P

    int i = 0;
    int j = 0;

    double RADIUS = opt->matchRadius;
    double RADIUS_SQR = PS_SQR (RADIUS);

    while ((i < st1->n) && (j < st2->n)) {

	dP = ((pmAstromObj *)st1->data[i])->P - ((pmAstromObj *)st2->data[i])->P;
	if (dP < -RADIUS) {
	    i++;
	    continue;
	}
	if (dP > +RADIUS) {
	    j++;
	    continue;
	}

	jStart = j;
	while ((dX > -RADIUS) && (j < st2->n)) {
	    
	    dP = ((pmAstromObj *)st1->data[i])->P - ((pmAstromObj *)st2->data[i])->P;
	    dQ = ((pmAstromObj *)st1->data[i])->Q - ((pmAstromObj *)st2->data[i])->Q;
	    dR = dP*dP + dQ*dQ;

	    if (dR > RADIUS_SQR) {
		j++;
		continue;
	    }
	    
	    // XXX got a match, add it to list
	    j++;
	}
	j = jStart;
	i++;
    }
    return (match);
}

pmAstromMatchedListFit (psArray *st1, psArray *st2, psArray *match, pmAstromOpt *opt) {

  psVector *X = psVectorAlloc (match->n);
  psVector *Y = psVectorAlloc (match->n);
  psVector *x = psVectorAlloc (match->n);
  psVector *y = psVectorAlloc (match->n);
  
  // take the matched stars, first fit 
  for (i = 0; i < match->n; i++) {

    pair = match->data[i];
    ob1 = st1->data[pair->i1];
    ob2 = st2->data[pair->i2];

    X->data.F64[i] = ob1->P;
    Y->data.F64[i] = ob1->Q;

    x->data.F64[i] = ob2->P;
    y->data.F64[i] = ob2->Q;

    // use one or the other...
    psPolynomial2D *xt = psVectorFitPolynomial2D (NULL, stats, NULL, 0, X, NULL, x, y)
    psPolynomial2D *xt = psVectorClipFitPolynomial2D (NULL, stats, NULL, 0, X, NULL, x, y)

    psPolynomial2D *yt = psVectorFitPolynomial2D (NULL, stats, NULL, 0, Y, NULL, x, y)
    psPolynomial2D *yt = psVectorClipFitPolynomial2D (NULL, stats, NULL, 0, Y, NULL, x, y)

      }

}

// rotate the focal-plane coordinates about the center coordinate
// angle specified in radians
psArray *pmAstromRotateObj (psArray *old, psPlane *center, double angle) {

    pmAstromObj *newObj;

    psArray *new = psArrayAlloc (old->n);

    cs = cos(angle);
    sn = sin(angle);
    xCenter = center.x;
    yCenter = center.y;
    
    for (int i = 0; i < old->n; i++) {

	oldObj = (pmAstromObj *)old->data[i];
	newObj = pmAstromObjCopy (oldObj);

	P = oldObj->FP.x - xCenter;
	Q = oldObj->FP.y - yCenter;
	
	newObj->FP.x = P*cs + Q*sn;
	newObj->FP.y = Q*cs - P*sn;
	
	new->data[i] = newObj;
    }
    return (new);
}

pmAstromObj *pmAstromObjCopy (pmAstromObj *old) {

  pmAstromObj *obj = pmAstromObjAlloc ();

  obj[0] = old[0];

  return (obj);
}

static void pmAstromObjFree (pmAstromObj *obj) {

  if (obj == NULL) return;
  return;
}

pmAstromObj *pmAstromObjAlloc (void) {

  pmAstromObj *obj = psAlloc (sizeof(pmAstromObj));
  psMemSetDeallocator(line, (psFreeFunc) pmAstromObjFree);

  obj->pix.x = 0;
  obj->pix.y = 0;

  obj->FP.x = 0;
  obj->FP.y = 0;

  obj->TP.x = 0;
  obj->TP.y = 0;

  obj->sky.x = 0;
  obj->sky.y = 0;

  obj->mag = 0;

  return (obj);
}

