Index: trunk/psLib/src/math/psSparse.c
===================================================================
--- trunk/psLib/src/math/psSparse.c	(revision 10001)
+++ trunk/psLib/src/math/psSparse.c	(revision 10009)
@@ -11,4 +11,7 @@
 #include "psAssert.h"
 #include "psConstants.h"
+#include "psImageStructManip.h"
+#include "psImage.h"
+#include "psMatrix.h"
 #include "psSparse.h"
 #include "psAbort.h"
@@ -210,20 +213,20 @@
 }
 
-# if (0)
-
-    /*** psSparseBorder Functions : these are used to solve a matrix equation of the form:
-      A x = f  where A is partitioned into:
-      A = |S B| where Q is a low-rank square matrix (N<20)
-          |B Q| and B is a rectangular band (technically this is B and B^T)
-      and S is a sparse matrix.
-    */
-
-    static void psSparseBorderFree(psSparse *sparse)
-{
-    if (!sparse) {
+/*** psSparseBorder Functions : these are used to solve a matrix equation of the form:
+     A x = f  where A is partitioned into:
+     A = |S B| where Q is a low-rank square matrix (N<20)
+     |B Q| and B is a rectangular band (technically this is B and B^T)
+     and S is a sparse matrix.
+*/
+
+static void psSparseBorderFree(psSparseBorder *border)
+{
+    if (!border) {
         return;
     }
-    psFree(sparse->Bij);
-    psFree(sparse->Qii);
+    psFree(border->sparse);
+    psFree(border->Bij);
+    psFree(border->Tjj);
+    psFree(border->Gj);
     return;
 }
@@ -236,4 +239,6 @@
     psMemSetDeallocator(border, (psFreeFunc) psSparseBorderFree);
 
+    border->sparse = psMemIncrRefCounter (sparse); // XXX increment ref counter or not?
+
     int Nrows = sparse->Nrows;
     border->Nrows = Nrows;
@@ -243,26 +248,23 @@
     psImageInit (border->Bij, 0.0);
 
-    border->Qii = psImageAlloc(Nborder, Nborder, PS_DATA_F32);
-    psImageInit (border->Qii, 0.0);
-
-    border->Yi = psVectorAlloc(Nborder, PS_DATA_F32);
-    psVectorInit (border->Yi, 0.0);
-
-    border->Gi = psVectorAlloc(Nborder, PS_DATA_F32);
-    psVectorInit (border->Gi, 0.0);
+    border->Tjj = psImageAlloc(Nborder, Nborder, PS_DATA_F32);
+    psImageInit (border->Tjj, 0.0);
+
+    border->Gj = psVectorAlloc(Nborder, PS_DATA_F32);
+    psVectorInit (border->Gj, 0.0);
 
     return border;
 }
 
-// add elements to border->Qii
-bool psSparseBorderElementQ(psSparseBorder *border, int i, int j, float value)
+// add elements to border->Tjj
+bool psSparseBorderElementT(psSparseBorder *border, int i, int j, float value)
 {
     PS_ASSERT_PTR_NON_NULL(border, false);
-    PS_ASSERT_PTR_NON_NULL(border->Qii, false);
+    PS_ASSERT_PTR_NON_NULL(border->Tjj, false);
     PS_ASSERT_INT_NONNEGATIVE(i, false);
     PS_ASSERT_INT_NONNEGATIVE(j, false);
 
-    // check i,j against border->Qii->nX,nY
-    border->Qii->data.F32[i][j] = value;
+    // check i,j against border->Tjj->nX,nY
+    border->Tjj->data.F32[i][j] = value;
     return true;
 }
@@ -280,34 +282,24 @@
 }
 
-// add elements to border->Yi
-bool psSparseBorderElementY(psSparseBorder *border, int i, float value)
+// add elements to border->Gj
+bool psSparseBorderElementG(psSparseBorder *border, int i, float value)
 {
     PS_ASSERT_PTR_NON_NULL(border, false);
-    PS_ASSERT_PTR_NON_NULL(border->Yi, false);
+    PS_ASSERT_PTR_NON_NULL(border->Gj, false);
     PS_ASSERT_INT_NONNEGATIVE(i, false);
 
-    border->Yi->data.F32[i] = value;
-    return true;
-}
-
-// add elements to border->Gi
-bool psSparseBorderElementY(psSparseBorder *border, int i, float value)
-{
-    PS_ASSERT_PTR_NON_NULL(border, false);
-    PS_ASSERT_PTR_NON_NULL(border->Gi, false);
-    PS_ASSERT_INT_NONNEGATIVE(i, false);
-
-    border->Gi->data.F32[i] = value;
+    border->Gj->data.F32[i] = value;
     return true;
 }
 
 // perform the operation dG = B*x
-psVector *psSparseBorderUpperProduct (psSparse *border)
-{
+psVector *psSparseBorderLowerProduct (psVector *dG, psSparseBorder *border, psVector *xVec)
+{
+    // XXX assert xVec->n == border->Nrows
 
     int Nborder = border->Nborder;
-    int Nrow = border->Nrows;
-
-    psVector *dG = psVectorAlloc (Nborder, PS_DATA_F32);
+    int Nrows = border->Nrows;
+
+    dG = psVectorRecycle (dG, Nborder, PS_TYPE_F32);
     psVectorInit (dG, 0.0);
 
@@ -315,5 +307,5 @@
         double value = 0;
         for (int i = 0; i < Nrows; i++) {
-            value += border->Bij->data.F32[i][j] * x->data.F32[i];
+            value += border->Bij->data.F32[i][j] * xVec->data.F32[i];
         }
         dG->data.F32[j] = value;
@@ -324,11 +316,12 @@
 
 // perform the operation dF = B^T*y
-psVector *psSparseBorderLowerProduct (psSparse *border)
-{
+psVector *psSparseBorderUpperProduct (psVector *dF, psSparseBorder *border, psVector *yVec)
+{
+    // XXX assert yVec->n == border->Nborder
 
     int Nborder = border->Nborder;
-    int Nrow = border->Nrows;
-
-    psVector *dF = psVectorAlloc (Nrows, PS_DATA_F32);
+    int Nrows = border->Nrows;
+
+    dF = psVectorRecycle (dF, Nrows, PS_TYPE_F32);
     psVectorInit (dF, 0.0);
 
@@ -336,5 +329,5 @@
         double value = 0;
         for (int j = 0; j < Nborder; j++) {
-            value += border->Bij->data.F32[i][j] * border->Yi->data.F32[j];
+            value += border->Bij->data.F32[i][j] * yVec->data.F32[j];
         }
         dF->data.F32[i] = value;
@@ -344,10 +337,30 @@
 }
 
+// perform the operation dG = T*y
+psVector *psSparseBorderSquareProduct (psVector *dG, psSparseBorder *border, psVector *yVec)
+{
+    // XXX assert yVec->n == border->Nborder
+
+    int Nborder = border->Nborder;
+
+    dG = psVectorRecycle (dG, Nborder, PS_TYPE_F32);
+    psVectorInit (dG, 0.0);
+
+    for (int i = 0; i < Nborder; i++) {
+        double value = 0;
+        for (int j = 0; j < Nborder; j++) {
+            value += border->Tjj->data.F32[j][i] * yVec->data.F32[j];
+        }
+        dG->data.F32[i] = value;
+    }
+
+    return dG;
+}
+
 // perform the operation dF = B^T*y
-psVector *psSparseBorderUpperDelta (psSparse *border, psVector *dF)
-{
-
-    int Nborder = border->Nborder;
-    int Nrow = border->Nrows;
+bool psSparseBorderUpperDelta (psSparseBorder *border, psVector *dF)
+{
+
+    int Nrows = border->Nrows;
 
     for (int i = 0; i < Nrows; i++) {
@@ -359,15 +372,72 @@
 
 // perform the operation dF = B^T*y
-psVector *psSparseBorderLowerDelta (psSparse *border, psVector *dG)
+bool psSparseBorderLowerDelta (psSparseBorder *border, psVector *dG)
 {
 
     int Nborder = border->Nborder;
-    int Nrow = border->Nrows;
 
     for (int i = 0; i < Nborder; i++) {
-        border->Gi->data.F32[i] -= dG->data.F32[i];
-    }
-
-    return true;
-}
-# endif /* gene's dev work */
+        border->Gj->data.F32[i] -= dG->data.F32[i];
+    }
+
+    return true;
+}
+
+// multiply A*x = b (where x is (x,y) and b is (f,g))
+bool psSparseBorderMultiply (psVector **fIn, psVector **gIn, psSparseBorder *border, psVector *xVec, psVector *yVec)
+{
+    psVector *fVec = *fIn;
+    fVec = psSparseMatrixTimesVector (fVec, border->sparse, xVec);
+    psVector *dF = psSparseBorderUpperProduct (NULL, border, yVec);
+    for (int i = 0; i < border->Nrows; i++) {
+        fVec->data.F32[i] += dF->data.F32[i];
+    }
+    psFree (dF);
+    *fIn = fVec;
+
+    psVector *gVec = *gIn;
+    gVec = psSparseBorderSquareProduct (gVec, border, yVec);
+    psVector *dG = psSparseBorderLowerProduct (NULL, border, xVec);
+    for (int i = 0; i < border->Nborder; i++) {
+        gVec->data.F32[i] += dG->data.F32[i];
+    }
+    psFree (dG);
+    *gIn = gVec;
+
+    return true;
+}
+
+bool psSparseBorderSolve(psVector **xFit, psVector **yFit, psSparseConstraint constraint, psSparseBorder *border, int Niter)
+{
+    // PS_ASSERT_PTR_NON_NULL(sparse, NULL);
+    // PS_ASSERT_INT_POSITIVE(Niter, NULL);
+    // PS_ASSERT_FLOAT_LARGER_THAN(constraint.paramDelta, 0.0, NULL);
+
+    psVector *xVec = *xFit;
+    psVector *yVec = *yFit;
+
+    for (int j = 0; j < Niter; j++) {
+
+        // solve Sx = f
+        xVec = psSparseSolve(xVec, constraint, border->sparse, Niter);
+
+        psVector *dG = psSparseBorderLowerProduct (NULL, border, xVec);
+        psSparseBorderLowerDelta (border, dG);
+
+        // XXX not defined
+        psImage *square = psImageCopy (NULL, border->Tjj, PS_TYPE_F32);
+        psVectorCopy (yVec, border->Gj, PS_TYPE_F32);
+        psMatrixGJSolve (square, yVec);
+        psFree (square);
+
+        psVector *dF = psSparseBorderUpperProduct (NULL, border, yVec);
+        psSparseBorderUpperDelta (border, dF);
+
+        psFree (dF);
+        psFree (dG);
+    }
+
+    *xFit = xVec;
+    *yFit = yVec;
+    return true;
+}