#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

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

#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))

// Returns the change in x' and y' for a change in x and y of 1
bool stacPlaneTransformDeriv(psPlane *out, // Output derivative, assumed already allocated
			     psPlaneTransform *transform, // The transform for which to obtain the derivative
			     psPlane *in // The position at which to obtain the derivative
    )
{
    psPolynomial2D *xPoly = transform->x;
    psPolynomial2D *yPoly = transform->y;

    assert(xPoly->type == PS_POLYNOMIAL_ORD);
    assert(yPoly->type == PS_POLYNOMIAL_ORD);

    // Initialise derivatives
    out->x = 0.0;
    out->y = 0.0;

    // Do x component
    double xSum = 1.0 / in->x;		// 1/x in order to calculate x^(i-1)
    for (int xOrder = 0; xOrder < xPoly->nX; xOrder++) {
        double ySum = xSum / in->y;	// 1/y in order to calculate y^(j-1)
        for (int yOrder = 0; yOrder < xPoly->nY; yOrder++) {
            if (xPoly->mask[xOrder][yOrder] == 0) {
		out->x += xPoly->coeff[xOrder][yOrder] * (double)xOrder * ySum * in->y;	// a_ij i x^(i-1) y^j
		out->x += xPoly->coeff[xOrder][yOrder] * (double)yOrder * ySum * in->x; // a_ij j x^i y^(j-1)
            }
	    ySum *= in->y;
        }
	xSum *= in->x;
    }

    // Do y component
    xSum = 1.0 / in->x;			// 1/x in order to calculate x^(i-1)
    for (int xOrder = 0; xOrder < yPoly->nX; xOrder++) {
        double ySum = xSum / in->y;	// 1/y in order to calculate y^(j-1)
        for (int yOrder = 0; yOrder < yPoly->nY; yOrder++) {
            if (yPoly->mask[xOrder][yOrder] == 0) {
		out->y += yPoly->coeff[xOrder][yOrder] * (float)xOrder * ySum * in->y;	// a_ij i x^(i-1) y^j
		out->y += yPoly->coeff[xOrder][yOrder] * (float)yOrder * ySum * in->x; // a_ij j x^i y^(j-1)
            }
	    ySum *= in->y;
        }
	xSum *= in->x;
    }

    return true;
}



// Calculate 
psImage *stacAreaOfInterest(psImage *mask, // Mask that is to be used to determine the area of interest
			    psPlaneTransform *map, // Map from the mask to the area of interest
			    int nxOut, int nyOut // Size of the area of interest
    )
{
    // Input checks
    assert(mask);
    assert(map);
    assert(mask->type.type == PS_TYPE_U8);
    
    // Number of pixels in x and y
    int nxIn = mask->numCols;
    int nyIn = mask->numRows;

    // Create and zero output image
    psImage *output = psImageAlloc(nxOut, nyOut, PS_TYPE_U8);
    for (int y = 0; y < nyOut; y++) {
	for (int x = 0; x < nxOut; x++) {
	    output->data.U8[y][x] = 0;
	}
    }

    psTrace("stac.area", 3, "Finding area of interest....\n");


    // Coordinates for the transformations
    psPlane *inFrame = psAlloc(sizeof(psPlane));
    psPlane *outFrame = psAlloc(sizeof(psPlane));
    psPlane *deriv = psAlloc(sizeof(psPlane));

    // Find pixels on the mask that are of interest
    for (int y = 0; y < nyIn; y++) {
	for (int x = 0; x < nxIn; x++) {
	    if (mask->data.U8[y][x]) {
		inFrame->x = (double)x + 0.5;
		inFrame->y = (double)y + 0.5;
		(void)psPlaneTransformApply(outFrame, map, inFrame);
		(void)stacPlaneTransformDeriv(deriv, map, inFrame);

		// Calculate range
		int iMin = MAX((int)(outFrame->x - 0.5*deriv->x), 0);
		int iMax = MIN((int)(outFrame->x + 0.5*deriv->x + 0.5), nxOut - 1);
		int jMin = MAX((int)(outFrame->y - 0.5*deriv->y), 0);
		int jMax = MIN((int)(outFrame->y + 0.5*deriv->y + 0.5), nyOut - 1);

		for (int j = jMin; j <= jMax; j++) {
		    for (int i = iMin; i <= iMax; i++) {
			output->data.U8[j][i] = 1;
		    }
		}
	    } // Pixel of interest
	}
    } // Iterating over input mask

    psFree(inFrame);
    psFree(outFrame);
    psFree(deriv);

    return output;
}
