# include "pswarp.h"

// NOTE: in this function, the coordinates are transformed from the OUTPUT to the INPUT
bool pswarpTransformReadout_Opt(pmReadout *output, pmReadout *input, pmConfig *config)
{
    // XXX this implementation currently ignores the use of the region
    psImage *region = NULL;

    psTimerStart ("warp");

    // Get grid size
    int nGridX = psMetadataLookupS32(NULL, config->arguments, "GRID.NX");
    int nGridY = psMetadataLookupS32(NULL, config->arguments, "GRID.NY");
    psImageInterpolateMode interpolationMode = psMetadataLookupS32(NULL, config->arguments,
                                                                   "INTERPOLATION.MODE");
    psMaskType maskIn = psMetadataLookupU8(NULL, config->arguments, "MASK.IN"); // Mask for input data
    psMaskType maskPoor = psMetadataLookupU8(NULL, config->arguments, "MASK.POOR"); // Mask for "poor" data
    psMaskType maskBad = psMetadataLookupU8(NULL, config->arguments, "MASK.BAD"); // Mask for bad data
    float poorFrac = psMetadataLookupF32(NULL, config->arguments, "POOR.FRAC"); // Flux fraction for "poor"

    // we need to apply the offset to convert parent coordinates to child coordinates for
    // psImagePixelInterpolate below
    int inCol0 = input->image->col0;
    int inRow0 = input->image->row0;
    int outCol0 = output->image->col0;
    int outRow0 = output->image->row0;

    // we might want to do the rectangular regions outside of the selection independently
    // psImageInit (output->image, NAN);

    // find the output pixel range
    int minX, minY, maxX, maxY;
    pswarpMatchRange (&minX, &minY, &maxX, &maxY, input, output);

    // pswarpMapGridFromImage builds a set of locally-linear maps which convert the
    // output coordinates to input coordinates
    pswarpMapGrid *grid = pswarpMapGridFromImage (input, output, nGridX, nGridY);

    // XXX need to modify the grid based on this result and force the maxError < XXX
    double maxError = pswarpMapGridMaxError (grid);
    psLogMsg ("pswarp", 3, "maximum error using this grid sampling: %f\n", maxError);

    int gridX, gridY, nextGridX, nextGridY;
    pswarpMapGridSetGrid (grid, minX, minY, &gridX, &gridY);
    nextGridY = pswarpMapGridNextGrid_Y (grid, gridY);
    pswarpMap *map = grid->maps[gridX][gridY];

    assert ((int)(minX - outCol0) >= 0);
    assert ((int)(maxX - outCol0) <= output->image->numCols);
    assert ((int)(minY - outRow0) >= 0);
    assert ((int)(maxY - outRow0) <= output->image->numRows);

    int gridXo = gridX;
    int nextGridXo = pswarpMapGridNextGrid_X (grid, gridX);

    psImage *inImage = input->image;    // Input image
    psImage *inVar   = input->weight;   // Input weight map
    psImage *inMask  = input->mask;     // Input mask

    // Interpolation options
    psImageInterpolateOptions *interp = psImageInterpolateOptionsAlloc(interpolationMode, inImage,
                                                                       inVar, inMask, maskIn, NAN, NAN,
                                                                       maskBad, maskPoor, poorFrac);

    psPlane *inPix = psPlaneAlloc();    // Coordinates on the input detector
    psF32 **outImageData = output->image->data.F32; // Output image pixels
    psF32 **outVarData = NULL;          // Output variance pixels
    psMaskType **outMaskData = NULL;    // Output mask pixels
    if (inVar) {
        if (!output->weight) {
            output->weight = psImageAlloc(output->image->numCols, output->image->numRows, PS_TYPE_F32);
            psImageInit(output->weight, NAN);
        }
        outVarData = output->weight->data.F32;
    }
    if (inMask || maskPoor || maskBad) {
        if (!output->mask) {
            output->mask = psImageAlloc(output->image->numCols, output->image->numRows, PS_TYPE_MASK);
            psImageInit(output->mask, maskBad);
        }
        outMaskData = output->mask->data.PS_TYPE_MASK_DATA;
    }

    // Iterate over the output image pixels (parent frame)
    for (int y = minY; y < maxY; y++) {
        if (y >= nextGridY) {
            gridY ++;
            nextGridY = pswarpMapGridNextGrid_Y (grid, gridY);
            map = grid->maps[gridX][gridY];
        }

        gridX = gridXo;
        nextGridX = nextGridXo;
        map = grid->maps[gridX][gridY];
        for (int x = minX; x < maxX; x++) {
            if (x >= nextGridX) {
                gridX ++;
                nextGridX = pswarpMapGridNextGrid_X (grid, gridX);
                map = grid->maps[gridX][gridY];
            }

            // Only transform those pixels requested
            if (region && region->data.U8[y][x]) continue;

            // pswarpMapApply converts the output coordinate (x,y) to the input coordinate.
            // both are in the parent frames of the input and output images.
            pswarpMapApply (&inPix->x, &inPix->y, map, x + 0.5, y + 0.5);

            if (inPix->x - inCol0 < 0) continue;
            if (inPix->x - inCol0 >= inImage->numCols) continue;
            if (inPix->y - inRow0 < 0) continue;
            if (inPix->y - inRow0 >= inImage->numRows) continue;

            // XXX include mask
            // XXX apply scale and offset?
            // psImagePixelInterpolate determines the value at pixel coordinate (x,y) in child coordinates
            double imageValue, varValue;
            psMaskType maskValue;
            if (!psImageInterpolate(&imageValue, &varValue, &maskValue,
                                    inPix->x - inCol0, inPix->y - inRow0, interp)) {
                psError(PS_ERR_UNKNOWN, false, "Unable to interpolate image.");
                psFree(interp);
                psFree (inPix);
                psFree (grid);
                return false;
            }
            outImageData[y-outRow0][x-outCol0] = imageValue;
            if (inVar) {
                outVarData[y-outRow0][x-outCol0] = varValue;
            }
            if (outMaskData) {
                outMaskData[y-outRow0][x-outCol0] = maskValue;
            }
        }
    }

    output->data_exists = true;

    psFree(interp);
    psFree (inPix);
    psFree (grid);
    psLogMsg ("pswarp", 3, "warping analysis: %f sec\n", psTimerMark ("warp"));

    return true;
}
