Index: trunk/psLib/src/imageops/psImageCovariance.c
===================================================================
--- trunk/psLib/src/imageops/psImageCovariance.c	(revision 25994)
+++ trunk/psLib/src/imageops/psImageCovariance.c	(revision 26892)
@@ -14,6 +14,11 @@
 #include "psTrace.h"
 #include "psBinaryOp.h"
+#include "psScalar.h"
+#include "psThread.h"
 
 #include "psImageCovariance.h"
+
+static bool threaded = false;           // Run threaded?
+
 
 psKernel *psImageCovarianceNone(void)
@@ -24,54 +29,12 @@
 }
 
-
-psKernel *psImageCovarianceCalculate(const psKernel *kernel, const psKernel *covariance)
-{
-    PS_ASSERT_KERNEL_NON_NULL(kernel, NULL);
-
-    // See http://en.wikipedia.org/wiki/Error_propagation
-    //
-    // If
-    //     f_k = sum_i A_ik x_i
-    // is a set of functions, then the covariance matrix for f is given by:
-    //     M^f_ij = sum_k sum_l A_ik M^x_kl A_lj
-    // where M^x is the covariance matrix for x.
-    // Note that the errors in f are correlated (covariance) even if the errors in x are not.
-
-    psKernel *covar;                    // Covariance matrix to use
-    if (covariance) {
-        covar = psMemIncrRefCounter((psKernel*)covariance); // Casting away const
-    } else {
-        covar = psImageCovarianceNone();
-    }
-
-    // Check for non-finite elements
-    for (int y = kernel->yMin; y <= kernel->yMax; y++) {
-        for (int x = kernel->xMin; x <= kernel->xMax; x++) {
-            if (!isfinite(kernel->kernel[y][x])) {
-                psError(PS_ERR_BAD_PARAMETER_VALUE, true,
-                        "Non-finite covariance matrix element in kernel at %d,%d", x, y);
-                psFree(covar);
-                return NULL;
-            }
-        }
-    }
-    for (int y = covar->yMin; y <= covar->yMax; y++) {
-        for (int x = covar->xMin; x <= covar->xMax; x++) {
-            if (!isfinite(covar->kernel[y][x])) {
-                psError(PS_ERR_BAD_PARAMETER_VALUE, true,
-                        "Non-finite covariance matrix element in covariance matrix at %d,%d", x, y);
-                psFree(covar);
-                return NULL;
-            }
-        }
-    }
-
-    // The above (double) sum for the covariance matrix means that, for each point in the output covariance
-    // matrix, we need to work out all combinations of getting to the central point via a kernel, input
-    // covariance matrix and another kernel.  This means that the resultant covariance matrix has twice the
-    // size of the kernel plus the size of the input covariance matrix.
-    int xMin = kernel->xMin - kernel->xMax + covar->xMin, xMax = kernel->xMax - kernel->xMin + covar->xMax;
-    int yMin = kernel->yMin - kernel->yMax + covar->yMin, yMax = kernel->yMax - kernel->yMin + covar->yMax;
-    psKernel *out = psKernelAlloc(xMin, xMax, yMin, yMax); // Covariance matrix for output
+/// Calculation of covariance matrix element when convolving
+static float imageCovarianceCalculate(const psKernel *covar, // Original covariance matrix
+                                      const psKernel *kernel, // Convolution kernel
+                                      int x, int y            // Coordinates in output covariance matrix
+                                      )
+{
+    psAssert(covar, "Require covariance matrix");
+    psAssert(kernel, "Require kernel");
 
     // Need to go:
@@ -89,44 +52,234 @@
     // from the source coordinate), we take the smallest possible (because everything else is zero outside).
 
-    double total = 0.0;                 // Total covariance
+    // Range for v
+    int vMin = PS_MAX(kernel->yMin + covar->yMin, y + kernel->yMin);
+    int vMax = PS_MIN(kernel->yMax + covar->yMax, y + kernel->yMax);
+    // Range for u
+    int uMin = PS_MAX(kernel->xMin + covar->xMin, x + kernel->xMin);
+    int uMax = PS_MIN(kernel->xMax + covar->xMax, x + kernel->xMax);
+
+    double sum = 0.0;           // Sum for value of covariance matrix at (x,y)
+    for (int v = vMin; v <= vMax; v++) {
+        // Range for q
+        int qMin = PS_MAX(v + covar->yMin, kernel->yMin);
+        int qMax = PS_MIN(v + covar->yMax, kernel->yMax);
+        for (int u = uMin; u <= uMax; u++) {
+            // Range for p
+            int pMin = PS_MAX(u + covar->xMin, kernel->xMin);
+            int pMax = PS_MIN(u + covar->xMax, kernel->xMax);
+
+            double xyuvValue = kernel->kernel[v-y][u-x]; // Value for (x,y) --> (u,v)
+
+            double uvpqValue = 0.0; // Value for (u,v) --> (p,q) --> (0,0)
+            for (int q = qMin; q <= qMax; q++) {
+                for (int p = pMin; p <= pMax; p++) {
+                    uvpqValue += (double)covar->kernel[q-v][p-u] * (double)kernel->kernel[q][p];
+                }
+            }
+            sum += xyuvValue * uvpqValue;
+        }
+    }
+
+    return sum;
+}
+
+/// Thread entry point for calculation of covariance matrix element when convolving
+static bool imageCovarianceCalculateThread(psThreadJob *job)
+{
+    PS_ASSERT_THREAD_JOB_NON_NULL(job, false);
+    psAssert(job->args, "No job arguments");
+    psAssert(job->args->n == 5, "Wrong number of job arguments: %ld", job->args->n);
+
+    psKernel *out = job->args->data[0]; // Output covariance matrix
+    const psKernel *covar = job->args->data[1]; // Input covariance matrix
+    const psKernel *kernel = job->args->data[2]; // Convolution kernel
+    int x = PS_SCALAR_VALUE(job->args->data[3], S32); // x coordinate in output covariance matrix
+    int y = PS_SCALAR_VALUE(job->args->data[4], S32); // y coordinate in output covariance matrix
+
+    out->kernel[y][x] = imageCovarianceCalculate(covar, kernel, x, y);
+
+    return true;
+}
+
+
+
+psKernel *psImageCovarianceCalculate(const psKernel *kernel, const psKernel *covariance)
+{
+    PS_ASSERT_KERNEL_NON_NULL(kernel, NULL);
+
+    // See http://en.wikipedia.org/wiki/Error_propagation
+    //
+    // If
+    //     f_k = sum_i A_ik x_i
+    // is a set of functions, then the covariance matrix for f is given by:
+    //     M^f_ij = sum_k sum_l A_ik M^x_kl A_lj
+    // where M^x is the covariance matrix for x.
+    // Note that the errors in f are correlated (covariance) even if the errors in x are not.
+
+    psKernel *covar;                    // Covariance matrix to use
+    if (covariance) {
+        covar = psMemIncrRefCounter((psKernel*)covariance); // Casting away const
+    } else {
+        covar = psImageCovarianceNone();
+    }
+
+    // Check for non-finite elements
+    for (int y = kernel->yMin; y <= kernel->yMax; y++) {
+        for (int x = kernel->xMin; x <= kernel->xMax; x++) {
+            if (!isfinite(kernel->kernel[y][x])) {
+                psError(PS_ERR_BAD_PARAMETER_VALUE, true,
+                        "Non-finite covariance matrix element in kernel at %d,%d", x, y);
+                psFree(covar);
+                return NULL;
+            }
+        }
+    }
+    for (int y = covar->yMin; y <= covar->yMax; y++) {
+        for (int x = covar->xMin; x <= covar->xMax; x++) {
+            if (!isfinite(covar->kernel[y][x])) {
+                psError(PS_ERR_BAD_PARAMETER_VALUE, true,
+                        "Non-finite covariance matrix element in covariance matrix at %d,%d", x, y);
+                psFree(covar);
+                return NULL;
+            }
+        }
+    }
+
+    // The above (double) sum for the covariance matrix means that, for each point in the output covariance
+    // matrix, we need to work out all combinations of getting to the central point via a kernel, input
+    // covariance matrix and another kernel.  This means that the resultant covariance matrix has twice the
+    // size of the kernel plus the size of the input covariance matrix.
+    int xMin = kernel->xMin - kernel->xMax + covar->xMin, xMax = kernel->xMax - kernel->xMin + covar->xMax;
+    int yMin = kernel->yMin - kernel->yMax + covar->yMin, yMax = kernel->yMax - kernel->yMin + covar->yMax;
+    psKernel *out = psKernelAlloc(xMin, xMax, yMin, yMax); // Covariance matrix for output
+
     for (int y = yMin; y <= yMax; y++) {
-        // Range for v
-        int vMin = PS_MAX(kernel->yMin + covar->yMin, y + kernel->yMin);
-        int vMax = PS_MIN(kernel->yMax + covar->yMax, y + kernel->yMax);
         for (int x = xMin; x <= xMax; x++) {
-            // Range for u
-            int uMin = PS_MAX(kernel->xMin + covar->xMin, x + kernel->xMin);
-            int uMax = PS_MIN(kernel->xMax + covar->xMax, x + kernel->xMax);
-
-            double sum = 0.0;           // Sum for value of covariance matrix at (x,y)
-            for (int v = vMin; v <= vMax; v++) {
-                // Range for q
-                int qMin = PS_MAX(v + covar->yMin, kernel->yMin);
-                int qMax = PS_MIN(v + covar->yMax, kernel->yMax);
-                for (int u = uMin; u <= uMax; u++) {
-                    // Range for p
-                    int pMin = PS_MAX(u + covar->xMin, kernel->xMin);
-                    int pMax = PS_MIN(u + covar->xMax, kernel->xMax);
-
-                    double xyuvValue = kernel->kernel[v-y][u-x]; // Value for (x,y) --> (u,v)
-
-                    double uvpqValue = 0.0; // Value for (u,v) --> (p,q) --> (0,0)
-                    for (int q = qMin; q <= qMax; q++) {
-                        for (int p = pMin; p <= pMax; p++) {
-                            uvpqValue += (double)covar->kernel[q-v][p-u] * (double)kernel->kernel[q][p];
-                        }
-                    }
-                    sum += xyuvValue * uvpqValue;
+            if (threaded) {
+                psThreadJob *job = psThreadJobAlloc("PSLIB_IMAGE_COVARIANCE_CALCULATE");
+                psArrayAdd(job->args, 1, out);
+                psArrayAdd(job->args, 1, covar);
+                psArrayAdd(job->args, 1, (psKernel*)kernel); // Casting away const
+                PS_ARRAY_ADD_SCALAR(job->args, x, PS_TYPE_S32);
+                PS_ARRAY_ADD_SCALAR(job->args, y, PS_TYPE_S32);
+                if (!psThreadJobAddPending(job)) {
+                    psFree(covar);
+                    return NULL;
                 }
-            }
-            out->kernel[y][x] = sum;
-            total += sum;
-        }
-    }
-    psTrace("psLib.imageops", 3, "Total covariance: %lf ; Central variance: %f\n", total, out->kernel[0][0]);
-
+                psFree(job);
+            } else {
+                out->kernel[y][x] = imageCovarianceCalculate(covar, kernel, x, y);
+            }
+        }
+    }
     psFree(covar);
+
+    if (threaded && !psThreadPoolWait(true)) {
+        psError(PS_ERR_UNKNOWN, false, "Error waiting for threads.");
+        return false;
+    }
+
     return out;
 }
+
+float psImageCovarianceCalculateFactor(const psKernel *kernel, const psKernel *covariance)
+{
+    psKernel *covar;                    // Covariance matrix to use
+    if (covariance) {
+        covar = psMemIncrRefCounter((psKernel*)covariance); // Casting away const
+    } else {
+        covar = psImageCovarianceNone();
+    }
+
+    // Check for non-finite elements
+    for (int y = kernel->yMin; y <= kernel->yMax; y++) {
+        for (int x = kernel->xMin; x <= kernel->xMax; x++) {
+            if (!isfinite(kernel->kernel[y][x])) {
+                psError(PS_ERR_BAD_PARAMETER_VALUE, true,
+                        "Non-finite covariance matrix element in kernel at %d,%d", x, y);
+                psFree(covar);
+                return NAN;
+            }
+        }
+    }
+    for (int y = covar->yMin; y <= covar->yMax; y++) {
+        for (int x = covar->xMin; x <= covar->xMax; x++) {
+            if (!isfinite(covar->kernel[y][x])) {
+                psError(PS_ERR_BAD_PARAMETER_VALUE, true,
+                        "Non-finite covariance matrix element in covariance matrix at %d,%d", x, y);
+                psFree(covar);
+                return NAN;
+            }
+        }
+    }
+
+    float factor = imageCovarianceCalculate(covar, kernel, 0, 0); // Covariance factor
+    psFree(covar);
+    return factor;
+}
+
+// Calculation of covariance matrix element when binning
+static float imageCovarianceBin(const psKernel *covar, // Original covariance matrix
+                                int bin,               // Binning factor
+                                float binVal,          // Convolution kernel value for binning
+                                int x, int y           // Coordinates in output covariance matrix
+    )
+{
+    psAssert(covar, "Require covariance matrix");
+    psAssert(bin > 0 && binVal > 0, "Require binning: %d %f", bin, binVal);
+
+    int binMin = -(bin - 1) / 2, binMax = bin / 2; // Range of "kernel"
+
+    // Range for v
+    int vMin = PS_MAX(binMin + covar->yMin, bin * y + binMin);
+    int vMax = PS_MIN(binMax + covar->yMax, bin * y + binMax);
+    // Range for u
+    int uMin = PS_MAX(binMin + covar->xMin, bin * x + binMin);
+    int uMax = PS_MIN(binMax + covar->xMax, bin * x + binMax);
+
+    double sum = 0.0;           // Sum for value of covariance matrix at (x,y)
+    for (int v = vMin; v <= vMax; v++) {
+        // Range for q
+        int qMin = PS_MAX(v + covar->yMin, binMin);
+        int qMax = PS_MIN(v + covar->yMax, binMax);
+        for (int u = uMin; u <= uMax; u++) {
+            // Range for p
+            int pMin = PS_MAX(u + covar->xMin, binMin);
+            int pMax = PS_MIN(u + covar->xMax, binMax);
+
+            double xyuvValue = binVal; // Value for (x,y) --> (u,v)
+
+            double uvpqValue = 0.0; // Value for (u,v) --> (p,q) --> (0,0)
+            for (int q = qMin; q <= qMax; q++) {
+                for (int p = pMin; p <= pMax; p++) {
+                    uvpqValue += (double)covar->kernel[q-v][p-u] * (double)binVal;
+                }
+            }
+            sum += xyuvValue * uvpqValue;
+        }
+    }
+
+    return sum;
+}
+
+/// Thread entry point for calculation of covariance matrix element when binning
+static bool imageCovarianceBinThread(psThreadJob *job)
+{
+    PS_ASSERT_THREAD_JOB_NON_NULL(job, false);
+    psAssert(job->args, "No job arguments");
+    psAssert(job->args->n == 6, "Wrong number of job arguments: %ld", job->args->n);
+
+    psKernel *out = job->args->data[0]; // Output covariance matrix
+    const psKernel *covar = job->args->data[1]; // Input covariance matrix
+    int bin = PS_SCALAR_VALUE(job->args->data[2], S32); // Binning factor
+    float binVal = PS_SCALAR_VALUE(job->args->data[3], F32); // Convolution kernel value for binning
+    int x = PS_SCALAR_VALUE(job->args->data[4], S32); // x coordinate in output covariance matrix
+    int y = PS_SCALAR_VALUE(job->args->data[5], S32); // y coordinate in output covariance matrix
+
+    out->kernel[y][x] = imageCovarianceBin(covar, bin, binVal, x, y);
+
+    return true;
+}
+
 
 psKernel *psImageCovarianceBin(int bin, const psKernel *covariance, bool average)
@@ -165,43 +318,31 @@
     psKernel *out = psKernelAlloc(xMin, xMax, yMin, yMax); // Covariance matrix for output
 
-    double total = 0.0;                 // Total covariance
     for (int y = yMin; y <= yMax; y++) {
-        // Range for v
-        int vMin = PS_MAX(binMin + covar->yMin, bin * y + binMin);
-        int vMax = PS_MIN(binMax + covar->yMax, bin * y + binMax);
         for (int x = xMin; x <= xMax; x++) {
-            // Range for u
-            int uMin = PS_MAX(binMin + covar->xMin, bin * x + binMin);
-            int uMax = PS_MIN(binMax + covar->xMax, bin * x + binMax);
-
-            double sum = 0.0;           // Sum for value of covariance matrix at (x,y)
-            for (int v = vMin; v <= vMax; v++) {
-                // Range for q
-                int qMin = PS_MAX(v + covar->yMin, binMin);
-                int qMax = PS_MIN(v + covar->yMax, binMax);
-                for (int u = uMin; u <= uMax; u++) {
-                    // Range for p
-                    int pMin = PS_MAX(u + covar->xMin, binMin);
-                    int pMax = PS_MIN(u + covar->xMax, binMax);
-
-                    double xyuvValue = binVal; // Value for (x,y) --> (u,v)
-
-                    double uvpqValue = 0.0; // Value for (u,v) --> (p,q) --> (0,0)
-                    for (int q = qMin; q <= qMax; q++) {
-                        for (int p = pMin; p <= pMax; p++) {
-                            uvpqValue += (double)covar->kernel[q-v][p-u] * (double)binVal;
-                        }
-                    }
-                    sum += xyuvValue * uvpqValue;
+            if (threaded) {
+                psThreadJob *job = psThreadJobAlloc("PSLIB_IMAGE_COVARIANCE_BIN");
+                psArrayAdd(job->args, 1, out);
+                psArrayAdd(job->args, 1, covar);
+                PS_ARRAY_ADD_SCALAR(job->args, bin, PS_TYPE_S32);
+                PS_ARRAY_ADD_SCALAR(job->args, binVal, PS_TYPE_F32);
+                PS_ARRAY_ADD_SCALAR(job->args, x, PS_TYPE_S32);
+                PS_ARRAY_ADD_SCALAR(job->args, y, PS_TYPE_S32);
+                if (!psThreadJobAddPending(job)) {
+                    psFree(covar);
+                    return NULL;
                 }
-            }
-            out->kernel[y][x] = sum;
-            total += sum;
-        }
-    }
-    psTrace("psLib.imageops", 3, "Total covariance: %lf ; Central variance: %f\n", total, out->kernel[0][0]);
-
+                psFree(job);
+            } else {
+                out->kernel[y][x] = imageCovarianceBin(covar, bin, binVal, x, y);
+            }
+        }
+    }
     psFree(covar);
 
+    if (threaded && !psThreadPoolWait(true)) {
+        psError(PS_ERR_UNKNOWN, false, "Error waiting for threads.");
+        return false;
+    }
+
     return out;
 }
@@ -219,15 +360,15 @@
 
     for (int y = covar->yMin; y <= covar->yMax; y++) {
-	if (y < -radius) continue;
-	if (y > +radius) continue;
-	for (int x = covar->xMin; x <= covar->xMax; x++) {
-	    if (x < -radius) continue;
-	    if (x > +radius) continue;
-
-	    if (hypot(x, y) > radius) continue;
-
-	    psAssert (isfinite(covar->kernel[y][x]), "invalid NAN in covariance matrix");
-	    Sum += covar->kernel[y][x];
-	}
+        if (y < -radius) continue;
+        if (y > +radius) continue;
+        for (int x = covar->xMin; x <= covar->xMax; x++) {
+            if (x < -radius) continue;
+            if (x > +radius) continue;
+
+            if (hypot(x, y) > radius) continue;
+
+            psAssert (isfinite(covar->kernel[y][x]), "invalid NAN in covariance matrix");
+            Sum += covar->kernel[y][x];
+        }
     }
 
@@ -428,2 +569,32 @@
     return true;
 }
+
+
+bool psImageCovarianceSetThreads(bool set)
+{
+    bool old = threaded;                // Old value
+    if (set && !threaded) {
+        {
+            psThreadTask *task = psThreadTaskAlloc("PSLIB_IMAGE_COVARIANCE_CALCULATE", 5);
+            task->function = &imageCovarianceCalculateThread;
+            psThreadTaskAdd(task);
+            psFree(task);
+        }
+        {
+            psThreadTask *task = psThreadTaskAlloc("PSLIB_IMAGE_COVARIANCE_BIN", 6);
+            task->function = &imageCovarianceBinThread;
+            psThreadTaskAdd(task);
+            psFree(task);
+        }
+    } else if (!set && threaded) {
+        psThreadTaskRemove("PSLIB_IMAGE_COVARIANCE_CALCULATE");
+        psThreadTaskRemove("PSLIB_IMAGE_COVARIANCE_BIN");
+    }
+    threaded = set;
+    return old;
+}
+
+bool psImageCovarianceGetThreads(void)
+{
+    return threaded;
+}