Index: /branches/eam_branches/20091201/psModules/src/imcombine/pmSubtractionEquation.c
===================================================================
--- /branches/eam_branches/20091201/psModules/src/imcombine/pmSubtractionEquation.c	(revision 26429)
+++ /branches/eam_branches/20091201/psModules/src/imcombine/pmSubtractionEquation.c	(revision 26430)
@@ -801,4 +801,11 @@
         }
 
+	if ((stamp->x <= 0.0) && (stamp->y <= 0.0)) {
+	    psAbort ("bad stamp");
+	}
+	if (!isfinite(stamp->x) && !isfinite(stamp->y)) {
+	    psAbort ("bad stamp");
+	}
+
         if (pmSubtractionThreaded()) {
             psThreadJob *job = psThreadJobAlloc("PSMODULES_SUBTRACTION_CALCULATE_EQUATION");
@@ -822,5 +829,5 @@
     }
 
-    pmSubtractionVisualPlotLeastSquares(stamps);
+    // pmSubtractionVisualPlotLeastSquares(stamps);
 
     psLogMsg("psModules.imcombine", PS_LOG_INFO, "Calculate equation: %f sec",
@@ -838,12 +845,16 @@
 psImage *p_pmSubSolve_VwUt (psImage *V, psImage *wUt);
 
-psVector *p_pmSubSolve_UtB (psImage *U, psVector *B);
-psVector *p_pmSubSolve_wUtB (psVector *w, psVector *UtB);
-psVector *p_pmSubSolve_VwUtB (psImage *V, psVector *wUtB);
-
-psVector *p_pmSubSolve_Ax (psImage *A, psVector *x);
-
-double p_pmSubSolve_y2 (pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps);
-double p_pmSubSolve_VdV (psVector *x, psVector *y);
+bool p_pmSubSolve_SetWeights (psVector *wApply, psVector *w, psVector *wMask);
+
+bool p_pmSubSolve_UtB (psVector **UtB, psImage *U, psVector *B);
+bool p_pmSubSolve_wUtB (psVector **wUtB, psVector *w, psVector *UtB);
+bool p_pmSubSolve_VwUtB (psVector **VwUtB, psImage *V, psVector *wUtB);
+
+bool p_pmSubSolve_Ax (psVector **B, psImage *A, psVector *x);
+bool p_pmSubSolve_VdV (double *value, psVector *x, psVector *y);
+bool p_pmSubSolve_y2 (double *y2, pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps);
+
+psImage *p_pmSubSolve_Xvar (psImage *V, psVector *w);
+
 double p_pmSubSolve_ChiSquare (pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps);
 
@@ -976,8 +987,13 @@
 # define SVD_ANALYSIS 1
 # define COEFF_SIG 0.0
-# define SVD_TOL 1e-9
+# define SVD_TOL 0.0
 
 	psVector *solution = NULL;
 	psVector *solutionErr = NULL;
+
+#ifdef TESTING
+	psFitsWriteImageSimple("A.fits", sumMatrix, NULL);
+	psVectorWriteFile ("B.dat", sumVector);
+#endif
 
 	// Use SVD to determine the kernel coeffs (and validate)
@@ -1003,29 +1019,9 @@
 	    // identify the elements of x that are insignificant (x / dx < 1.0? < 0.5?) -> set to 0.0
 
-	    // XXX TEST with known matrix:
-	    // psFree (sumMatrix);
-	    // sumMatrix = psImageAlloc (2, 2, PS_TYPE_F64);
-	    // sumMatrix->data.F64[0][0] = 1.0;
-	    // sumMatrix->data.F64[0][1] = 0.5;
-	    // 
-	    // sumMatrix->data.F64[1][0] = 0.5;
-	    // sumMatrix->data.F64[1][1] = 2.0;
-
-	    // sumMatrix->data.F64[2][0] = 0.0;
-	    // sumMatrix->data.F64[2][1] = 1.0;
-
-            // TEST psFitsWriteImageSimple("A.fits", sumMatrix, NULL);
-	    // TEST psVectorWriteFile("B.dat", sumVector);;
-
 	    // If I use the SVD trick to re-condition the matrix, I need to break out the
-	    // kernel terms from the normalization and background terms.
-
-	    // XX int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+	    // kernel and normalization terms from the background term.
+	    // XXX is this true?  or was this due to an error in the analysis?
+
 	    int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
-
-	    // solve for the normalization first (independent of all other terms)
-	    // XX double sumRR = sumMatrix->data.F64[normIndex][normIndex];
-	    // XX double sumIR = sumVector->data.F64[normIndex];
-	    // XX double norm = sumIR / sumRR;
 
 	    // now pull out the kernel elements into their own square matrix
@@ -1033,21 +1029,14 @@
 	    psVector *kernelVector = psVectorAlloc (sumMatrix->numCols - 1, PS_TYPE_F64);
 
-	    // note that we need to subtract norm*sumRC from the sumVector elements (since we
-	    // are fitting to the residual image of I - norm*Ref)
 	    for (int ix = 0, kx = 0; ix < sumMatrix->numCols; ix++) {
-		// if (ix == normIndex) continue;
 		if (ix == bgIndex) continue;
 		for (int iy = 0, ky = 0; iy < sumMatrix->numRows; iy++) {
-		    // if (iy == normIndex) continue;
 		    if (iy == bgIndex) continue;
 		    kernelMatrix->data.F64[ky][kx] = sumMatrix->data.F64[iy][ix];
 		    ky++;
 		}
-		// kernelVector->data.F64[kx] = sumVector->data.F64[ix] - norm*sumMatrix->data.F64[normIndex][ix];
 		kernelVector->data.F64[kx] = sumVector->data.F64[ix];
 		kx++;
 	    }
-
-	    psFitsWriteImageSimple("A.fits", kernelMatrix, NULL);
 
 	    psImage *U = NULL;
@@ -1059,53 +1048,111 @@
 	    }
 
-	    // TEST psFitsWriteImageSimple("U.fits", U, NULL);
-	    // TEST psFitsWriteImageSimple("V.fits", V, NULL);
-	    // TEST psVectorWriteFile("w.dat", w);
-
 	    // calculate A_inverse:
 	    // Ainv = V * w * U^T
 	    psImage *wUt  = p_pmSubSolve_wUt (w, U);
 	    psImage *Ainv = p_pmSubSolve_VwUt (V, wUt);
-
-	    // TEST psFitsWriteImageSimple("AInv.fits", Ainv, NULL);
-	    // TEST psImage *Io = p_pmSubSolve_VwUt (Ainv, sumMatrix);
-	    // TEST psFitsWriteImageSimple("I.fits", Io, NULL);
-
+	    psImage *Xvar = NULL;
+	    psFree (wUt);
+
+# ifdef TESTING
 	    // kernel terms:
 	    for (int i = 0; i < w->n; i++) {
 		fprintf (stderr, "w: %f\n", w->data.F64[i]);
 	    }
-
-	    // XXX loop over w adding in more and more of the values until chisquare is no
-	    // longer dropping significantly
-
-	    // zero-out ill-conditioned SVD components:
-	    // largest w value is in w->data.F64[0]:
-	    double Wmax = w->data.F64[0];
-	    for (int k = 1; k < w->n; k++) {
-		if (fabs(w->data.F64[k] / Wmax) < SVD_TOL) {
-		    fprintf (stderr, "masking basis function %d : %f\n", k, w->data.F64[k] / Wmax);
-		    w->data.F64[k] = NAN;
-		    psTrace("psModules.imcombine", 5, "masking basis function %d : %f", k, w->data.F64[k] / Wmax);
-		}
-	    }
-
-	    // solve for x:
-	    // x = V * w * (U^T * B)
-	    psVector *UtB  = p_pmSubSolve_UtB (U, kernelVector);
-	    psVector *wUtB = p_pmSubSolve_wUtB (w, UtB);
-	    psVector *Xsvd = p_pmSubSolve_VwUtB (V, wUtB);
-
-	    // chi-square for this system of equations:
-	    // chi-square = sum over terms of: (Ax - B)*x - b*x - y^2
-	    // y^2 = \sum_stamps \sum_pixels input->kernel[y][x]^2
-	    psVector *Ax = p_pmSubSolve_Ax (kernelMatrix, Xsvd);
-	    double Axx   = p_pmSubSolve_VdV (Ax, Xsvd);
-	    double Bx    = p_pmSubSolve_VdV (kernelVector, Xsvd);
-	    double y2    = p_pmSubSolve_y2 (kernels, stamps);
-
-	    // apparently, this works (compare with the brute force value below
-	    double chiSquare = Axx - 2.0*Bx + y2;
-	    fprintf (stderr, "chi square = %f\n", chiSquare);
+# endif
+	    // loop over w adding in more and more of the values until chisquare is no longer
+	    // dropping significantly.
+	    // XXX this does not seem to work very well: we seem to need all terms even for
+	    // simple cases...
+
+	    psVector *Xsvd = NULL;
+	    { 
+		psVector *Ax = NULL;
+		psVector *UtB = NULL;
+		psVector *wUtB = NULL;
+
+		psVector *wApply = psVectorAlloc(w->n, PS_TYPE_F64);
+		psVector *wMask = psVectorAlloc(w->n, PS_TYPE_U8);
+		psVectorInit (wMask, 1); // start by masking everything
+
+		double chiSquareLast = NAN;
+		int maxWeight = 0;
+
+		double Axx, Bx, y2;
+
+		// XXX this is an attempt to exclude insignificant modes.
+		// it was not successful with the ISIS kernel set: removing even 
+		// the least significant mode leaves additional ringing / noise
+		// because the terms are so coupled.
+		for (int k = 0; false && (k < w->n); k++) {
+
+		    // unmask the k-th weight
+		    wMask->data.U8[k] = 0;
+		    p_pmSubSolve_SetWeights(wApply, w, wMask);
+
+		    // solve for x:
+		    // x = V * w * (U^T * B)
+		    p_pmSubSolve_UtB (&UtB, U, kernelVector);
+		    p_pmSubSolve_wUtB (&wUtB, wApply, UtB);
+		    p_pmSubSolve_VwUtB (&Xsvd, V, wUtB);
+
+		    // chi-square for this system of equations:
+		    // chi-square = sum over terms of: (Ax - B)*x - b*x - y^2
+		    // y^2 = \sum_stamps \sum_pixels input->kernel[y][x]^2
+		    p_pmSubSolve_Ax (&Ax, kernelMatrix, Xsvd);
+		    p_pmSubSolve_VdV (&Axx, Ax, Xsvd);
+		    p_pmSubSolve_VdV (&Bx, kernelVector, Xsvd);
+		    p_pmSubSolve_y2 (&y2, kernels, stamps);
+
+		    // apparently, this works (compare with the brute force value below
+		    double chiSquare = Axx - 2.0*Bx + y2;
+		    double deltaChi = (k == 0) ? chiSquare : chiSquareLast - chiSquare;
+		    chiSquareLast = chiSquare;
+
+		    // fprintf (stderr, "chi square = %f, delta: %f\n", chiSquare, deltaChi);
+		    if (k && !maxWeight && (deltaChi < 1.0)) {
+			maxWeight = k;
+		    }
+		}
+
+		// keep all terms or we get extra ringing
+		maxWeight = w->n;
+		psVectorInit (wMask, 1);
+		for (int k = 0; k < maxWeight; k++) {
+		    wMask->data.U8[k] = 0;
+		}
+		p_pmSubSolve_SetWeights(wApply, w, wMask);
+
+		// solve for x:
+		// x = V * w * (U^T * B)
+		p_pmSubSolve_UtB (&UtB, U, kernelVector);
+		p_pmSubSolve_wUtB (&wUtB, wApply, UtB);
+		p_pmSubSolve_VwUtB (&Xsvd, V, wUtB);
+
+		// chi-square for this system of equations:
+		// chi-square = sum over terms of: (Ax - B)*x - b*x - y^2
+		// y^2 = \sum_stamps \sum_pixels input->kernel[y][x]^2
+		p_pmSubSolve_Ax (&Ax, kernelMatrix, Xsvd);
+		p_pmSubSolve_VdV (&Axx, Ax, Xsvd);
+		p_pmSubSolve_VdV (&Bx, kernelVector, Xsvd);
+		p_pmSubSolve_y2 (&y2, kernels, stamps);
+
+		// apparently, this works (compare with the brute force value below
+		double chiSquare = Axx - 2.0*Bx + y2;
+		psLogMsg ("psModules.imcombine", PS_LOG_INFO, "model kernel with %d terms; chi square = %f\n", maxWeight, chiSquare);
+
+		// re-calculate A^{-1} to get new variances:
+		// Ainv = V * w * U^T
+		// XXX since we keep all terms, this is identical to Ainv
+		psImage *wUt  = p_pmSubSolve_wUt (wApply, U);
+		Xvar = p_pmSubSolve_VwUt (V, wUt);
+		psFree (wUt);
+
+		psFree (Ax);
+		psFree (UtB);
+		psFree (wUtB);
+		psFree (wApply);
+		psFree (wMask);
+	    }
 
 	    // copy the kernel solutions to the full solution vector:
@@ -1114,10 +1161,4 @@
 
 	    for (int ix = 0, kx = 0; ix < sumVector->n; ix++) {
-		// XX if (ix == normIndex) {
-		// XX     // solution->data.F64[ix] = norm; // 
-		// XX     solution->data.F64[ix] = 1.0; // XXX a CHEAT
-		// XX     solutionErr->data.F64[ix] = 0.001;
-		// XX     continue;
-		// XX }
 		if (ix == bgIndex) {
 		    solution->data.F64[ix] = 0;
@@ -1130,8 +1171,4 @@
 	    }
 
-	    // TEST psVectorWriteFile("X.dat", Xsvd);
-	    // TEST psVector *Bo = p_pmSubSolve_Ax (sumMatrix, Xsvd);
-	    // TEST psVectorWriteFile("Bo.dat", Bo);
-		
 	    psFree (kernelMatrix);
 	    psFree (kernelVector);
@@ -1141,17 +1178,14 @@
 	    psFree (w);
 
-	    psFree (wUt);
 	    psFree (Ainv);
-
-	    psFree (UtB);
-	    psFree (wUtB);
 	    psFree (Xsvd);
 	} else {
 	    psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
 	    psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, sumMatrix);
-	    psFree(sumMatrix);
 	    if (!luMatrix) {
 		psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
+		psFree(solution);
 		psFree(sumVector);
+		psFree(sumMatrix);
 		psFree(luMatrix);
 		psFree(permutation);
@@ -1160,10 +1194,18 @@
 
 	    solution = psMatrixLUSolution(NULL, luMatrix, sumVector, permutation);
-	    psFree(sumVector);
 	    psFree(luMatrix);
 	    psFree(permutation);
 	    if (!solution) {
 		psError(PS_ERR_UNKNOWN, true, "Failed to solve the least-squares system.\n");
+		psFree(solution);
+		psFree(sumVector);
+		psFree(sumMatrix);
 		return NULL;
+	    }
+
+	    // XXX LUD does not provide A^{-1}?  fake the error for now 
+	    solutionErr = psVectorAlloc(sumVector->n, PS_TYPE_F64);
+	    for (int ix = 0; ix < sumVector->n; ix++) {
+		solutionErr->data.F64[ix] = 0.1*solution->data.F64[ix];
 	    }
 	}
@@ -1188,10 +1230,10 @@
 	    int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
 	    for (int i = 0; i < numKernels * numPoly; i++) {
-		fprintf (stderr, "%f +/- %f (%f) -> ", solution->data.F64[i], solutionErr->data.F64[i], fabs(solution->data.F64[i]/solutionErr->data.F64[i]));
+		// XXX fprintf (stderr, "%f +/- %f (%f) -> ", solution->data.F64[i], solutionErr->data.F64[i], fabs(solution->data.F64[i]/solutionErr->data.F64[i]));
 		if (fabs(solution->data.F64[i] / solutionErr->data.F64[i]) < COEFF_SIG) {
-		    fprintf (stderr, "drop\n");
+		    // XXX fprintf (stderr, "drop\n");
 		    kernels->solution1->data.F64[i] = 0.0;
 		} else {
-		    fprintf (stderr, "keep\n");
+		    // XXX fprintf (stderr, "keep\n");
 		    kernels->solution1->data.F64[i] = solution->data.F64[i];
 		}
@@ -1201,5 +1243,7 @@
 	// fprintf (stderr, "chi square Brute = %f\n", chiSquare);
 
-	psFree (solution);
+	psFree(solution);
+	psFree(sumVector);
+	psFree(sumMatrix);
 
 #ifdef TESTING
@@ -1662,9 +1706,9 @@
 
 // we are supplied U, not Ut
-psVector *p_pmSubSolve_UtB (psImage *U, psVector *B) {
+bool p_pmSubSolve_UtB (psVector **UtB, psImage *U, psVector *B) {
 
     psAssert (U->numRows == B->n, "U and B dimensions do not match");
 
-    psVector *UtB = psVectorAlloc (U->numCols, PS_TYPE_F64);
+    UtB[0] = psVectorRecycle (UtB[0], U->numCols, PS_TYPE_F64);
 
     for (int i = 0; i < U->numCols; i++) {
@@ -1673,32 +1717,32 @@
 	    sum += B->data.F64[j] * U->data.F64[j][i];
 	}
-	UtB->data.F64[i] = sum;
-    }
-    return UtB;
+	UtB[0]->data.F64[i] = sum;
+    }
+    return true;
 }
 
 // w is diagonal
-psVector *p_pmSubSolve_wUtB (psVector *w, psVector *UtB) {
+bool p_pmSubSolve_wUtB (psVector **wUtB, psVector *w, psVector *UtB) {
 
     psAssert (w->n == UtB->n, "w and UtB dimensions do not match");
 
     // wUt has dimensions transposed relative to Ut.
-    psVector *wUtB = psVectorAlloc (w->n, PS_TYPE_F64);
-    psVectorInit (wUtB, 0.0);
+    wUtB[0] = psVectorRecycle (wUtB[0], w->n, PS_TYPE_F64);
+    psVectorInit (wUtB[0], 0.0);
 
     for (int i = 0; i < w->n; i++) {
 	if (!isfinite(w->data.F64[i])) continue;
 	if (w->data.F64[i] == 0.0) continue;
-	wUtB->data.F64[i] = UtB->data.F64[i] / w->data.F64[i];
-    }
-    return wUtB;
+	wUtB[0]->data.F64[i] = UtB->data.F64[i] / w->data.F64[i];
+    }
+    return true;
 }
 
 // this is basically matrix * vector
-psVector *p_pmSubSolve_VwUtB (psImage *V, psVector *wUtB) {
+bool p_pmSubSolve_VwUtB (psVector **VwUtB, psImage *V, psVector *wUtB) {
 
     psAssert (V->numCols == wUtB->n, "V and wUtB dimensions do not match");
 
-    psVector *VwUtB = psVectorAlloc (V->numRows, PS_TYPE_F64);
+    VwUtB[0] = psVectorRecycle (*VwUtB, V->numRows, PS_TYPE_F64);
 
     for (int j = 0; j < V->numRows; j++) {
@@ -1707,15 +1751,15 @@
 	    sum += V->data.F64[j][i] * wUtB->data.F64[i];
 	}
-	VwUtB->data.F64[j] = sum;
-    }
-    return VwUtB;
+	VwUtB[0]->data.F64[j] = sum;
+    }
+    return true;
 }
 
 // this is basically matrix * vector
-psVector *p_pmSubSolve_Ax (psImage *A, psVector *x) {
+bool p_pmSubSolve_Ax (psVector **B, psImage *A, psVector *x) {
 
     psAssert (A->numCols == x->n, "A and x dimensions do not match");
 
-    psVector *B = psVectorAlloc (A->numRows, PS_TYPE_F64);
+    B[0] = psVectorRecycle (*B, A->numRows, PS_TYPE_F64);
 
     for (int j = 0; j < A->numRows; j++) {
@@ -1724,11 +1768,11 @@
 	    sum += A->data.F64[j][i] * x->data.F64[i];
 	}
-	B->data.F64[j] = sum;
-    }
-    return B;
+	B[0]->data.F64[j] = sum;
+    }
+    return true;
 }
 
 // this is basically Vector * vector
-double p_pmSubSolve_VdV (psVector *x, psVector *y) {
+bool p_pmSubSolve_VdV (double *value, psVector *x, psVector *y) {
 
     psAssert (x->n == y->n, "x and y dimensions do not match");
@@ -1738,8 +1782,9 @@
 	sum += x->data.F64[i] * y->data.F64[i];
     }
-    return sum;
-}
-
-double p_pmSubSolve_y2 (pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps) {
+    *value = sum;
+    return true;
+}
+
+bool p_pmSubSolve_y2 (double *y2, pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps) {
 
     int footprint = stamps->footprint; // Half-size of stamps
@@ -1790,5 +1835,6 @@
 	}
     }
-    return sum;
+    *y2 = sum;
+    return true;
 }
 
@@ -1878,4 +1924,45 @@
 }
 
+bool p_pmSubSolve_SetWeights (psVector *wApply, psVector *w, psVector *wMask) {
+
+    for (int i = 0; i < w->n; i++) {
+	wApply->data.F64[i] = wMask->data.U8[i] ? 0.0 : w->data.F64[i];
+    }
+    return true;
+}
+
+// we are supplied V and w; w represents a diagonal matrix (also, we apply 1/w instead of w)
+psImage *p_pmSubSolve_Xvar (psImage *V, psVector *w) {
+
+    psAssert (w->n == V->numCols, "w and U dimensions do not match");
+
+    psImage *Vn = psImageAlloc (V->numCols, V->numRows, PS_TYPE_F64);
+    psImageInit (Vn, 0.0);
+
+    // generate Vn = V * w^{-1}
+    for (int j = 0; j < Vn->numRows; j++) {
+	for (int i = 0; i < Vn->numCols; i++) {
+	    if (!isfinite(w->data.F64[i])) continue;
+	    if (w->data.F64[i] == 0.0) continue;
+	    Vn->data.F64[j][i] = V->data.F64[j][i] / w->data.F64[i];
+	}
+    }
+
+    psImage *Xvar = psImageAlloc (V->numCols, V->numRows, PS_TYPE_F64);
+    psImageInit (Xvar, 0.0);
+
+    // generate Xvar = Vn * Vn^T
+    for (int j = 0; j < Vn->numRows; j++) {
+	for (int i = 0; i < Vn->numCols; i++) {
+	    double sum = 0.0;
+	    for (int k = 0; k < Vn->numCols; k++) {
+		sum += Vn->data.F64[k][i]*Vn->data.F64[k][j];
+	    }
+	    Xvar->data.F64[j][i] = sum;
+	}
+    }
+    return Xvar;
+}
+
 // I get confused by the index values between the image vs matrix usage:  In terms
 // of the elements of an image A(x,y) = A->data.F64[y][x] = A_x,y, a matrix