Index: trunk/stac/src/stacInvertMaps.c
===================================================================
--- trunk/stac/src/stacInvertMaps.c	(revision 2500)
+++ trunk/stac/src/stacInvertMaps.c	(revision 2897)
@@ -5,6 +5,8 @@
 
 #define MAX(x,y) ((x) > (y) ? (x) : (y))
-
-psArray *stacInvertMaps(const psArray *maps // Array of maps to invert
+#define NUM_GRID 20
+
+psArray *stacInvertMaps(const psArray *maps, // Array of maps to invert
+			const stacConfig *config // Configuration
     )
 {
@@ -14,38 +16,130 @@
     psTrace("stac.invertMaps", 1, "Inverting maps....\n");
 
-    for (int i = 0; i < nMaps; i++) {
-	// Can't handle higher order than linear yet
-	if (((psPlaneTransform*)maps->data[i])->x->nX != 2 ||
-	    ((psPlaneTransform*)maps->data[i])->x->nY != 2 ||
-	    ((psPlaneTransform*)maps->data[i])->y->nX != 2 ||
-	    ((psPlaneTransform*)maps->data[i])->y->nY != 2) {
-	    psError("stac.invertMaps",
-		    "STAC cannot currently support orders other than linear.\n");
-	    psFree(inverted);
-	    return NULL;
-	}
-
-	psPlaneTransform *newMap = psPlaneTransformAlloc(2, 2); // Inverted map
-	psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[i]; // Uninverted map
-
-	// Now, simply do a 2x2 matrix inversion
-
-	double a = oldMap->x->coeff[1][0];
-	double b = oldMap->x->coeff[0][1];
-	double c = oldMap->y->coeff[1][0];
-	double d = oldMap->y->coeff[0][1];
-	double e = oldMap->x->coeff[0][0];
-	double f = oldMap->y->coeff[0][0];
-
-	double invDet = 1.0 / (a * d - b * c); // Inverse of the determinant
-
-	// Not entirely sure why this works, but it appears to do so.......................................
-	newMap->x->coeff[1][0] = invDet * a;
-	newMap->x->coeff[0][1] = - invDet * b;
-	newMap->y->coeff[1][0] = - invDet * c;
-	newMap->y->coeff[0][1] = invDet * d;
-
-	newMap->x->coeff[0][0] = - invDet * (d * e + c * f);
-	newMap->y->coeff[0][0] = - invDet * (b * e + a * f);
+    // Coordinates for the transformations
+    psPlane *inCoord = psAlloc(sizeof(psPlane));
+    psPlane *outCoord = psAlloc(sizeof(psPlane));
+
+    for (int mapNum = 0; mapNum < nMaps; mapNum++) {
+
+	psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[mapNum]; // Uninverted map
+	// Check input
+	if (oldMap->x->nX != oldMap->x->nY) {
+	    psError("stac.invertMaps", "Polynomial order in x and y don't match for map %d\n", mapNum);
+	    return NULL;
+	}
+	if (oldMap->y->nX != oldMap->y->nY) {
+	    psError("stac.invertMaps", "Polynomial order in x and y don't match for map %d\n", mapNum);
+	    return NULL;
+	}
+	if (oldMap->x->nX != oldMap->y->nX) {
+	    psError("stac.invertMaps", "Polynomial order in x and y don't match for map %d\n", mapNum);
+	    return NULL;
+	}
+	int order = oldMap->x->nX;	// Polynomial order
+	psTrace("stac.invertMaps", 4, "Generating order %d polynomial inverse transformation.\n", order);
+	psPlaneTransform *newMap = psPlaneTransformAlloc(order, order);	// Inverted map
+
+	// Create fake polynomial to use in evaluation
+	psDPolynomial2D *fakePoly = psDPolynomial2DAlloc(order, order, PS_POLYNOMIAL_ORD);
+	for (int i = 0; i < order; i++) {
+	    for (int j = 0; j < order; j++) {
+		fakePoly->coeff[i][j] = 1.0; // Set all coeffecients to 1
+		fakePoly->mask[i][j] = 1; // Mask all coefficients; unmask to evaluate
+	    }
+	}
+
+	// A grid of xin,yin --> xout,yout
+	psVector *xIn = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+	psVector *yIn = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+	psVector *xOut = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+	psVector *yOut = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32);
+
+	// Create grid of points
+	for (int yint = 0; yint < NUM_GRID; yint++) {
+	    inCoord->y = (float)(yint * config->outny) / (float)(NUM_GRID - 1);
+	    for (int xint = 0; xint < NUM_GRID; xint++) {
+		inCoord->x = (float)(xint * config->outnx) / (float)(NUM_GRID - 1);
+
+		(void)psPlaneTransformApply(outCoord, oldMap, inCoord);
+
+		xOut->data.F32[yint*NUM_GRID + xint] = inCoord->x;
+		yOut->data.F32[yint*NUM_GRID + xint] = inCoord->y;
+		xIn->data.F32[yint*NUM_GRID + xint] = outCoord->x;
+		yIn->data.F32[yint*NUM_GRID + xint] = outCoord->y;
+	    }
+	}
+
+	// Initialise the matrix and vectors
+	int nCoeff = order * (order + 1) / 2; // Number of polynomial coefficients
+	psImage *matrix = psImageAlloc(nCoeff, nCoeff, PS_TYPE_F64); // Matrix for solution
+	psVector *xVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in x
+	psVector *yVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in y
+	for (int i = 0; i < nCoeff; i++) {
+	    for (int j = 0; j < nCoeff; j++) {
+		matrix->data.F64[i][j] = 0.0;
+	    }
+	    xVector->data.F64[i] = 0.0;
+	    yVector->data.F64[i] = 0.0;
+	}
+
+	// Iterate over the grid points
+	for (int g = 0; g < NUM_GRID*NUM_GRID; g++) {
+
+	    // Iterate over the polynomial coefficients, accumulating the matrix and vectors
+	    for (int i = 0, ijIndex = 0; i < order; i++) {
+		for (int j = 0; j < order - i; j++, ijIndex++) {
+
+		    fakePoly->mask[i][j] = 0;
+		    double ijPoly = psDPolynomial2DEval(xIn->data.F32[g], yIn->data.F32[g], fakePoly);
+		    fakePoly->mask[i][j] = 1;
+
+		    for (int m = 0, mnIndex = 0; m < order; m++) {
+			for (int n = 0; n < order - m; n++, mnIndex++) {
+			    
+			    fakePoly->mask[m][n] = 0;
+			    double mnPoly = psDPolynomial2DEval(xIn->data.F32[g], yIn->data.F32[g], fakePoly);
+			    fakePoly->mask[m][n] = 1;
+
+			    matrix->data.F64[ijIndex][mnIndex] += ijPoly * mnPoly;
+			}
+		    }
+		    
+		    xVector->data.F64[ijIndex] += ijPoly * (double)xOut->data.F32[g];
+		    yVector->data.F64[ijIndex] += ijPoly * (double)yOut->data.F32[g];
+		}
+	    } // Iterating over coefficients
+	} // Iterating over grid points
+
+	// Solution via LU Decomposition
+	psVector *permutation = psVectorAlloc(nCoeff, PS_TYPE_F64); // Permutation vector for LU Decomposition
+	psImage *luMatrix = psMatrixLUD(NULL, permutation, matrix); // LU decomposed matrix
+	psVector *xSolution = psMatrixLUSolve(NULL, luMatrix, xVector, permutation); // Solution in x
+	psVector *ySolution = psMatrixLUSolve(NULL, luMatrix, yVector, permutation); // Solution in y
+
+	// Stuff coefficients into transformation
+	for (int i = 0, ijIndex = 0; i < order; i++) {
+	    for (int j = 0; j < order - i; j++, ijIndex++) {
+		newMap->x->coeff[i][j] = xSolution->data.F64[ijIndex];
+		newMap->y->coeff[i][j] = ySolution->data.F64[ijIndex];
+	    }
+	}
+	inverted->data[mapNum] = newMap;
+
+#ifdef TESTING
+	// Print x coefficients
+	psTrace("stac.invertMaps", 7, "x' = \n");
+	for (int i = 0; i < order; i++) {
+	    for (int j = 0; j < order - i; j++) {
+		psTrace("stac.invertMaps", 7, "      %f x^%d y^%d\n", newMap->x->coeff[i][j], i, j);
+	    }
+	}
+	// Print y coefficients
+	psTrace("stac.invertMaps", 7, "y' = \n");
+	for (int i = 0; i < order; i++) {
+	    for (int j = 0; j < order - i; j++) {
+		psTrace("stac.invertMaps", 7, "      %f x^%d y^%d\n", newMap->y->coeff[i][j], i, j);
+	    }
+	}
+#endif
 
 #ifdef TESTING
@@ -64,6 +158,58 @@
 #endif
 
-	inverted->data[i] = newMap;	// Stuff into the array
+	psFree(permutation);
+	psFree(luMatrix);
+	psFree(matrix);
+	psFree(xVector);
+	psFree(yVector);
+	psFree(xSolution);
+	psFree(ySolution);
+	psFree(fakePoly);
+	psFree(xIn);
+	psFree(yIn);
+	psFree(xOut);
+	psFree(yOut);
     }
+
+    psFree(inCoord);
+    psFree(outCoord);
+		
+
+#if 0
+	// Can't handle higher order than linear yet
+	if (((psPlaneTransform*)maps->data[i])->x->nX != 2 ||
+	    ((psPlaneTransform*)maps->data[i])->x->nY != 2 ||
+	    ((psPlaneTransform*)maps->data[i])->y->nX != 2 ||
+	    ((psPlaneTransform*)maps->data[i])->y->nY != 2) {
+	    psError("stac.invertMaps",
+		    "STAC cannot currently support orders other than linear.\n");
+	    psFree(inverted);
+	    return NULL;
+	}
+
+	psPlaneTransform *newMap = psPlaneTransformAlloc(2, 2); // Inverted map
+	psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[i]; // Uninverted map
+
+	// Now, simply do a 2x2 matrix inversion
+
+	double a = oldMap->x->coeff[1][0];
+	double b = oldMap->x->coeff[0][1];
+	double c = oldMap->y->coeff[1][0];
+	double d = oldMap->y->coeff[0][1];
+	double e = oldMap->x->coeff[0][0];
+	double f = oldMap->y->coeff[0][0];
+
+	double invDet = 1.0 / (a * d - b * c); // Inverse of the determinant
+
+	// Not entirely sure why this works, but it appears to do so.......................................
+	newMap->x->coeff[1][0] = invDet * a;
+	newMap->x->coeff[0][1] = - invDet * b;
+	newMap->y->coeff[1][0] = - invDet * c;
+	newMap->y->coeff[0][1] = invDet * d;
+
+	newMap->x->coeff[0][0] = - invDet * (d * e + c * f);
+	newMap->y->coeff[0][0] = - invDet * (b * e + a * f);
+#endif
+
 
     return inverted;