Index: /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c
===================================================================
--- /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c	(revision 25894)
+++ /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c	(revision 25895)
@@ -15,5 +15,5 @@
 #include "pmSubtractionVisual.h"
 
-#define TESTING                         // TESTING output for debugging; may not work with threads!
+//#define TESTING                         // TESTING output for debugging; may not work with threads!
 
 #define USE_VARIANCE                    // Include variance in equation?
@@ -377,4 +377,67 @@
 }
 
+// Merge dual matrices and vectors into single matrix equation
+// Have: Aa = Ct.b + d
+// Have: Ca = Bb + e
+// Set: F = ( A -Ct ;  C -B )
+// Set: g = ( a ; b )
+// Set: h = ( d ; e )
+// So that we combine the above two equations: Fg = h
+static bool calculateEquationDual(psImage **outMatrix,
+                                  psVector **outVector,
+                                  const psImage *sumMatrix1,
+                                  const psImage *sumMatrix2,
+                                  const psImage *sumMatrixX,
+                                  const psVector *sumVector1,
+                                  const psVector *sumVector2
+                                  )
+{
+    psAssert(sumMatrix1 && sumMatrix2 && sumMatrixX, "Require input matrices");
+    psAssert(sumVector1 && sumVector2, "Require input vectors");
+    int num1 = sumVector1->n;   // Number of parameters in first set
+    int num2 = sumVector2->n;   // Number of parameters in second set
+    int num = num1 + num2;      // Number of parameters in new set
+
+    psAssert(sumMatrix1->type.type == PS_TYPE_F64 &&
+             sumMatrix2->type.type == PS_TYPE_F64 &&
+             sumMatrixX->type.type == PS_TYPE_F64 &&
+             sumVector1->type.type == PS_TYPE_F64 &&
+             sumVector2->type.type == PS_TYPE_F64,
+             "Require input type is F64");
+
+    psAssert(outMatrix, "Require output matrix");
+    psAssert(outVector, "Require output vector");
+    if (!*outMatrix) {
+        *outMatrix = psImageAlloc(num, num, PS_TYPE_F64);
+    }
+    if (!*outVector) {
+        *outVector = psVectorAlloc(num, PS_TYPE_F64);
+    }
+    psImage *matrix = *outMatrix;
+    psVector *vector = *outVector;
+
+    psAssert(sumMatrix1->numCols == num1 && sumMatrix1->numRows == num1, "Require size NxN");
+    psAssert(sumMatrix2->numCols == num2 && sumMatrix2->numRows == num2, "Require size MxM");
+    psAssert(sumMatrixX->numCols == num1 && sumMatrixX->numRows == num2, "Require size MxN");
+
+    memcpy(vector->data.F64, sumVector1->data.F64, num1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+    memcpy(&vector->data.F64[num1], sumVector2->data.F64, num2 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+
+    for (int i = 0; i < num1; i++) {
+        memcpy(matrix->data.F64[i], sumMatrix1->data.F64[i], num1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        for (int j = 0, k = num1; j < num2; j++, k++) {
+            matrix->data.F64[i][k] = - sumMatrixX->data.F64[j][i];
+        }
+    }
+    for (int i1 = 0, i2 = num1; i1 < num2; i1++, i2++) {
+        memcpy(matrix->data.F64[i2], sumMatrixX->data.F64[i1], num1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        for (int j = 0, k = num1; j < num2; j++, k++) {
+            matrix->data.F64[i2][k] = - sumMatrix2->data.F64[i1][j];
+        }
+    }
+
+    return true;
+}
+
 
 // Add in penalty term to least-squares vector
@@ -881,35 +944,140 @@
         calculatePenalty(sumMatrix2, sumVector2, kernels, -sumMatrix1->data.F64[bgIndex][bgIndex]);
 
-        // Pure matrix operations
-        // A * a = Ct * b + d
-        // C * a = B  * b + e
-        //
-        // Set F = ( A -Ct ;  C -B )
-        // Set g = ( a ; b )
-        // Set h = ( d ; e )
-        // So that we combine the above two equations: Fg = h
-
-        int num = numParams + numParams2; // Number of params for new set
-        psImage *F = psImageAlloc(num, num, PS_TYPE_F64);
-        psVector *h = psVectorAlloc(num, PS_TYPE_F64);
-
-        for (int i = 0; i < numParams; i++) {
-            h->data.F64[i] = sumVector1->data.F64[i];
-            for (int j = 0; j < numParams; j++) {
-                F->data.F64[i][j] = sumMatrix1->data.F64[i][j];
-            }
+        psImage *sumMatrix = NULL;      // Combined matrix
+        psVector *sumVector = NULL;     // Combined vector
+        calculateEquationDual(&sumMatrix, &sumVector, sumMatrix1, sumMatrix2,
+                              sumMatrixX, sumVector1, sumVector2);
+
+#ifdef TESTING
+        {
+            psFits *fits = psFitsOpen("sumMatrix.fits", "w");
+            psFitsWriteImage(fits, NULL, sumMatrix, 0, NULL);
+            psFitsClose(fits);
+        }
+        {
+            psImage *image = psImageAlloc(1, numParams + numParams2, PS_TYPE_F64);
+            psFits *fits = psFitsOpen("sumVector.fits", "w");
+            for (int i = 0; i < numParams + numParams2; i++) {
+                image->data.F64[0][i] = sumVector->data.F64[i];
+            }
+            psFitsWriteImage(fits, NULL, image, 0, NULL);
+            psFree(image);
+            psFitsClose(fits);
+        }
+#endif
+
+        psVector *solution = NULL;                       // Solution to equation!
+        psVector *mask = psVectorAlloc(numParams + numParams2, PS_TYPE_U8); // Mask of parameters
+        psVectorInit(mask, 0);
+        {
+            solution = psMatrixSolveSVD(solution, sumMatrix, sumVector);
+            if (!solution) {
+                psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
+                psFree(sumMatrix);
+                psFree(sumVector);
+                psFree(mask);
+                return NULL;
+            }
+
+            // Remove a kernel basis for image 1 from the equation
+#define MASK_BASIS_1(INDEX) \
+            { \
+                for (int k = 0; k < numParams2; k++) { \
+                    sumMatrix1->data.F64[k][INDEX] = 0.0; \
+                    sumMatrix1->data.F64[INDEX][k] = 0.0; \
+                    sumMatrixX->data.F64[k][INDEX] = 0.0; \
+                } \
+                sumMatrix1->data.F64[bgIndex][INDEX] = 0.0; \
+                sumMatrix1->data.F64[INDEX][bgIndex] = 0.0; \
+                sumMatrix1->data.F64[normIndex][INDEX] = 0.0; \
+                sumMatrix1->data.F64[INDEX][normIndex] = 0.0; \
+                sumMatrix1->data.F64[INDEX][INDEX] = 1.0; \
+                sumVector1->data.F64[INDEX] = 0.0; \
+                mask->data.U8[INDEX] = 0xFF; \
+            }
+
+            // Remove a kernel basis for image 2 from the equation
+#define MASK_BASIS_2(INDEX) \
+            { \
+                for (int k = 0; k < numParams2; k++) { \
+                    sumMatrix2->data.F64[k][j] = 0.0; \
+                    sumMatrix2->data.F64[j][k] = 0.0; \
+                    sumMatrixX->data.F64[j][k] = 0.0; \
+                } \
+                sumMatrix2->data.F64[INDEX][INDEX] = 1.0; \
+                sumMatrixX->data.F64[j][normIndex] = 0.0; \
+                sumMatrixX->data.F64[j][bgIndex] = 0.0; \
+                sumVector2->data.F64[j] = 0.0; \
+                mask->data.U8[numParams + j] = 0xFF; \
+            }
+
+            #define TOL 1.0e-5
+            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+            double norm = solution->data.F64[normIndex];        // Normalisation
+            double thresh = norm * TOL;                         // Threshold for low parameters
             for (int j = 0; j < numParams2; j++) {
-                F->data.F64[i][numParams + j] = - sumMatrixX->data.F64[j][i];
-            }
-        }
-        for (int i = 0; i < numParams2; i++) {
-            h->data.F64[numParams + i] = sumVector2->data.F64[i];
-            for (int j = 0; j < numParams; j++) {
-                F->data.F64[numParams + i][j] = sumMatrixX->data.F64[i][j];
-            }
-            for (int j = 0; j < numParams2; j++) {
-                F->data.F64[numParams + i][numParams + j] = - sumMatrix2->data.F64[i][j];
-            }
-        }
+                double param1 = solution->data.F64[j],
+                    param2 = solution->data.F64[numParams + j]; // Parameters of interest
+                fprintf(stderr, "%lf %lf   ", param1, param2);
+                if (fabs(param1) < thresh) {
+                    fprintf(stderr, "Parameter %d: 1 below threshold\n", j);
+                    MASK_BASIS_1(j);
+                }
+                if (fabs(param2) < thresh) {
+                    fprintf(stderr, "Parameter %d: 2 below threshold\n", j);
+                    MASK_BASIS_2(j);
+                }
+
+                if (!mask->data.U8[j] && !mask->data.U8[numParams + j]) {
+                    if (fabs(param1) < fabs(param2)) {
+                        fprintf(stderr, "Parameter %d: 1 < 2\n", j);
+                        MASK_BASIS_1(j);
+                    } else {
+                        fprintf(stderr, "Parameter %d: 2 < 1\n", j);
+                        MASK_BASIS_2(j);
+                    }
+                }
+            }
+        }
+
+        calculateEquationDual(&sumMatrix, &sumVector, sumMatrix1, sumMatrix2,
+                              sumMatrixX, sumVector1, sumVector2);
+
+#ifdef TESTING
+        {
+            psFits *fits = psFitsOpen("sumMatrixFix.fits", "w");
+            psFitsWriteImage(fits, NULL, sumMatrix, 0, NULL);
+            psFitsClose(fits);
+        }
+        {
+            psImage *image = psImageAlloc(1, numParams + numParams2, PS_TYPE_F64);
+            psFits *fits = psFitsOpen("sumVectorFix.fits", "w");
+            for (int i = 0; i < numParams + numParams2; i++) {
+                image->data.F64[0][i] = sumVector->data.F64[i];
+            }
+            psFitsWriteImage(fits, NULL, image, 0, NULL);
+            psFree(image);
+            psFitsClose(fits);
+        }
+#endif
+
+        solution = psMatrixSolveSVD(solution, sumMatrix, sumVector);
+        if (!solution) {
+            psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
+            psFree(sumMatrix);
+            psFree(sumVector);
+            psFree(mask);
+            return NULL;
+        }
+
+#if 0
+        for (int i = 0; i < num; i++) {
+            if (mask->data.U8[i]) {
+                solution->data.F64[i] = 0.0;
+            }
+        }
+#endif
+        psFree(mask);
+
         psFree(sumMatrix1);
         psFree(sumMatrix2);
@@ -918,15 +1086,13 @@
         psFree(sumVector2);
 
+        psFree(sumMatrix);
+        psFree(sumVector);
+
 #ifdef TESTING
         {
-            psFits *fits = psFitsOpen("sumMatrix.fits", "w");
-            psFitsWriteImage(fits, NULL, F, 0, NULL);
-            psFitsClose(fits);
-        }
-        {
-            psImage *image = psImageAlloc(1, num, PS_TYPE_F64);
-            psFits *fits = psFitsOpen("sumVector.fits", "w");
-            for (int i = 0; i < num; i++) {
-                image->data.F64[0][i] = h->data.F64[i];
+            psImage *image = psImageAlloc(1, numParams + numParams2, PS_TYPE_F64);
+            psFits *fits = psFitsOpen("solnVector.fits", "w");
+            for (int i = 0; i < numParams + numParams2; i++) {
+                image->data.F64[0][i] = solution->data.F64[i];
             }
             psFitsWriteImage(fits, NULL, image, 0, NULL);
@@ -936,148 +1102,4 @@
 #endif
 
-        psVector *g = NULL;             // Solution!
-        psVector *mask = psVectorAlloc(num, PS_TYPE_U8); // Mask of parameters
-        psVectorInit(mask, 0);
-        {
-            g = psMatrixSolveSVD(g, F, h);
-            if (!g) {
-                psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
-                psFree(F);
-                psFree(h);
-                psFree(mask);
-                return NULL;
-            }
-
-            #define TOL 1.0e-5
-            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
-            double norm = g->data.F64[normIndex];               // Normalisation
-            double thresh = norm * TOL;                         // Threshold for low parameters
-            for (int j = 0; j < numParams2; j++) {
-                double param1 = g->data.F64[j], param2 = g->data.F64[numParams + j]; // Parameters of interest
-                fprintf(stderr, "%lf %lf   ", param1, param2);
-                if (fabs(param1) < thresh) {
-                    fprintf(stderr, "Parameter %d 1 below threshold\n", j);
-                    for (int k = 0; k < numParams2; k++) {
-                        // Matrix 1
-                        F->data.F64[k][j] = 0.0;
-                        F->data.F64[j][k] = 0.0;
-#if 1
-                        // Cross matrices
-                        F->data.F64[numParams + j][k] = 0.0;
-                        F->data.F64[j][numParams + k] = 0.0;
-                        F->data.F64[k][numParams + j] = 0.0;
-                        F->data.F64[numParams + k][j] = 0.0;
-#endif
-                    }
-                    F->data.F64[bgIndex][j] = 0.0;
-                    F->data.F64[j][bgIndex] = 0.0;
-                    F->data.F64[normIndex][j] = 0.0;
-                    F->data.F64[j][normIndex] = 0.0;
-                    F->data.F64[j][j] = 1.0;
-                    h->data.F64[j] = 0.0;
-                    mask->data.U8[j] = 0xFF;
-                }
-                if (fabs(param2) < thresh) {
-                    fprintf(stderr, "Parameter %d 2 below threshold\n", j);
-                    for (int k = 0; k < numParams2; k++) {
-                        // Matrix 2
-                        F->data.F64[k][numParams + j] = 0.0;
-                        F->data.F64[numParams + j][k] = 0.0;
-#if 1
-                        // Cross matrices
-                        F->data.F64[numParams + j][k] = 0.0;
-                        F->data.F64[j][numParams + k] = 0.0;
-                        F->data.F64[k][numParams + j] = 0.0;
-                        F->data.F64[numParams + k][j] = 0.0;
-#endif
-                    }
-                    F->data.F64[bgIndex][numParams + j] = 0.0;
-                    F->data.F64[numParams + j][bgIndex] = 0.0;
-                    F->data.F64[normIndex][numParams + j] = 0.0;
-                    F->data.F64[numParams + j][normIndex] = 0.0;
-                    F->data.F64[numParams + j][numParams + j] = 1.0;
-                    h->data.F64[numParams + j] = 0.0;
-                    mask->data.U8[numParams + j] = 0xFF;
-                }
-
-                if (!mask->data.U8[j] && !mask->data.U8[numParams + j]) {
-                    if (fabs(param1) < fabs(param2)) {
-                        fprintf(stderr, "Parameter %d 1 < 2\n", j);
-                        for (int k = 0; k < numParams2; k++) {
-                            // Matrix 1
-                            F->data.F64[k][j] = 0.0;
-                            F->data.F64[j][k] = 0.0;
-#if 0
-                            // Cross matrices
-                            F->data.F64[numParams + j][k] = 0.0;
-                            F->data.F64[j][numParams + k] = 0.0;
-                            F->data.F64[k][numParams + j] = 0.0;
-                            F->data.F64[numParams + k][j] = 0.0;
-#endif
-                        }
-                        F->data.F64[bgIndex][j] = 0.0;
-                        F->data.F64[j][bgIndex] = 0.0;
-                        F->data.F64[normIndex][j] = 0.0;
-                        F->data.F64[j][normIndex] = 0.0;
-                        F->data.F64[j][j] = 1.0;
-                        h->data.F64[j] = 0.0;
-                        mask->data.U8[j] = 0xFF;
-                    } else {
-                        fprintf(stderr, "Parameter %d 2 < 1\n", j);
-                        for (int k = 0; k < numParams2; k++) {
-                            // Matrix 1
-                            F->data.F64[k][numParams + j] = 0.0;
-                            F->data.F64[numParams + j][k] = 0.0;
-#if 0
-                            // Cross matrices
-                            F->data.F64[numParams + j][k] = 0.0;
-                            F->data.F64[j][numParams + k] = 0.0;
-                            F->data.F64[k][numParams + j] = 0.0;
-                            F->data.F64[numParams + k][j] = 0.0;
-#endif
-                        }
-                        F->data.F64[bgIndex][numParams + j] = 0.0;
-                        F->data.F64[numParams + j][bgIndex] = 0.0;
-                        F->data.F64[normIndex][numParams + j] = 0.0;
-                        F->data.F64[numParams + j][normIndex] = 0.0;
-                        F->data.F64[numParams + j][numParams + j] = 1.0;
-                        h->data.F64[numParams + j] = 0.0;
-                        mask->data.U8[numParams + j] = 0xFF;
-                    }
-                }
-            }
-        }
-
-        g = psMatrixSolveSVD(g, F, h);
-        if (!g) {
-            psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
-            psFree(F);
-            psFree(h);
-            psFree(mask);
-            return NULL;
-        }
-        for (int i = 0; i < num; i++) {
-            if (mask->data.U8[i]) {
-                g->data.F64[i] = 0.0;
-            }
-        }
-        psFree(mask);
-
-        psFree(F);
-        psFree(h);
-
-#ifdef TESTING
-        {
-            psImage *image = psImageAlloc(1, num, PS_TYPE_F64);
-            psFits *fits = psFitsOpen("solnVector.fits", "w");
-            for (int i = 0; i < num; i++) {
-                image->data.F64[0][i] = g->data.F64[i];
-            }
-            psFitsWriteImage(fits, NULL, image, 0, NULL);
-            psFree(image);
-            psFitsClose(fits);
-        }
-#endif
-
         if (!kernels->solution1) {
             kernels->solution1 = psVectorAlloc(numParams, PS_TYPE_F64);
@@ -1087,11 +1109,9 @@
         }
 
-        for (int i = 0; i < numParams; i++) {
-            kernels->solution1->data.F64[i] = g->data.F64[i];
-        }
-        for (int i = 0; i < numParams2; i++) {
-            kernels->solution2->data.F64[i] = g->data.F64[numParams + i];
-        }
-        psFree(g);
+        memcpy(kernels->solution1->data.F64, solution->data.F64, numParams * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        memcpy(kernels->solution2->data.F64, &solution->data.F64[numParams],
+               numParams2 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+
+        psFree(solution);
 
     }