#include <stdio.h>
#include "pslib.h"
#include "stac.h"


#define MAX(x,y) ((x) > (y) ? (x) : (y))

psArray *stacInvertMaps(const psArray *maps // Array of maps to invert
    )
{
    int nMaps = maps->n;		// Number of maps
    psArray *inverted = psArrayAlloc(nMaps); // Array of inverted maps for output

    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);

#ifdef TESTING
	// Go forward then backward
	double x = 123.4;
	double y = 432.1;
	psPlane *oldCoords = psAlloc(sizeof(psPlane));
	oldCoords->x = x;
	oldCoords->y = y;
	psPlane *newCoords = psPlaneTransformApply(NULL, oldMap, oldCoords);
	psTrace("stac.invertMaps.test", 5, "%f,%f --> %f,%f\n", x, y, newCoords->x, newCoords->y);
	(void)psPlaneTransformApply(oldCoords, newMap, newCoords);
	psTrace("stac.invertMaps.test", 5, "--------> %f,%f\n", oldCoords->x, oldCoords->y);
	psFree(newCoords);
	psFree(oldCoords);
#endif

	inverted->data[i] = newMap;	// Stuff into the array
    }

    return inverted;
}


