# include "pswarp.h"

bool pswarpTransformReadout (pmReadout *output, pmReadout *input, pmConfig *config) {

    // XXX this implementation currently ignores the use of the region
    psImage *region = NULL;
    pmCell *cell = NULL;

    // select the current recipe
    // psMetadata *recipe  = psMetadataLookupPtr (NULL, config->recipes, PSWARP_RECIPE);

    int outNx = output->image->numCols;
    int outNy = output->image->numRows;

    psPlane *inPix = psPlaneAlloc();    // Coordinates on the input detector
    psPlane *outPix = psPlaneAlloc();   // Coordinates on the output detector

    psPlane *FP = psPlaneAlloc();       // Coordinates on the focal plane
    psPlane *TP = psPlaneAlloc();       // Coordinates on the tangent plane
    psSphere *sky = psSphereAlloc();    // Coordinates on the sky

    cell = input->parent;
    pmChip *chipInput = cell->parent;
    pmFPA *fpaInput = chipInput->parent;

    cell = output->parent;
    pmChip *chipOutput = cell->parent;
    pmFPA *fpaOutput = chipOutput->parent;

    psF32 **outData = output->image->data.F32;
    psImage *inImage = input->image;

    psImageInterpolateOptions *interp = psImageInterpolateOptionsAlloc(PS_INTERPOLATE_BILINEAR, inImage,
                                                                       NULL, NULL, 0, NAN, NAN, 0, 0, 0.0);

    // Iterate over the output image pixels
    for (int y = 0; y < outNy; y++) {
        for (int x = 0; x < outNx; x++) {
            // Only transform those pixels requested
            if (region && region->data.U8[y][x]) continue;

            // XXX double check this 1/2 pixel offset
            outPix->x = (double)x + 0.5;
            outPix->y = (double)y + 0.5;

            psPlaneTransformApply(FP, chipOutput->toFPA, outPix);
            psPlaneTransformApply (TP, fpaOutput->toTPA, FP);
            psDeproject (sky, TP, fpaOutput->toSky);

            psProject (TP, sky, fpaInput->toSky);
            psPlaneTransformApply (FP, fpaInput->fromTPA, TP);
            psPlaneTransformApply (inPix, chipInput->fromFPA, FP);

            // XXX get interpolation method from the recipe
            double value;
            if (!psImageInterpolate(&value, NULL, NULL, inPix->x, inPix->y, interp)) {
                psError(PS_ERR_UNKNOWN, false, "Unable to interpolate image.");
                psFree(interp);
                psFree(inPix);
                psFree(outPix);
                psFree(FP);
                psFree(TP);
                psFree(sky);
                return false;
            }

            outData[y][x] = value;
            // modify zero and scale?
        }
    }
    psFree(interp);
    psFree(inPix);
    psFree(outPix);
    psFree(FP);
    psFree(TP);
    psFree(sky);

    return true;
}
