#include "pswarp.h"

#define SOURCE_ARRAY_BUFFER 100         // Size to grow the array of sources at a time

// NOTE: in this function, the coordinates are transformed from the OUTPUT to the INPUT
bool pswarpTransformSources(pmReadout *output, pmReadout *input, pmConfig *config)
{

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

    // Get warp parameters
    bool mdok;
    int nGridX = psMetadataLookupS32(NULL, config->arguments, "GRID.NX");
    int nGridY = psMetadataLookupS32(NULL, config->arguments, "GRID.NY");

    // Transform sources
    psArray *inSources = psMetadataLookupPtr(&mdok, input->analysis, "PSPHOT.SOURCES"); // Sources in source
    if (!inSources) return true;

    pswarpMapGrid *sourceGrid = pswarpMapGridFromImage(output, input, nGridX, nGridY); // Grid for sources

    psArray *outSources = psMemIncrRefCounter(psMetadataLookupPtr(&mdok, output->analysis, "PSPHOT.SOURCES")); // Target sources
    if (!outSources) {
        outSources = psArrayAllocEmpty(SOURCE_ARRAY_BUFFER);
        psMetadataAddPtr(output->analysis, PS_LIST_TAIL, "PSPHOT.SOURCES", PS_DATA_ARRAY,
                         "Warped sources", outSources);
    }

    // XXX it is probably not necessary to use the locally linear transformations we should
    // be using the full astrometry: since there are 100 - 1000 fewer sources than pixels,
    // this does not cost us so much time.
    for (int i = 0; i < inSources->n; i++) {
        pmSource *source = inSources->data[i]; // Source of interest
        pmModel *model = source->modelPSF; // Model for this source
        float xIn, yIn;             // Coordinates of source
        xIn = model->params->data.F32[PM_PAR_XPOS] - input->image->col0;
        yIn = model->params->data.F32[PM_PAR_YPOS] - input->image->row0;

        int xGrid, yGrid;           // Grid coordinates for local map
        if (!pswarpMapGridSetGrid(sourceGrid, xIn + 0.5, yIn + 0.5, &xGrid, &yGrid)) {
            psError(PS_ERR_UNKNOWN, false, "Unable to get grid coordinates for source at %f,%f\n",
                    xIn, yIn);
            psFree(outSources);
            psFree(sourceGrid);
            return false;
        }
        if (xGrid < 0 || xGrid >= sourceGrid->nXpts || yGrid < 0 || yGrid >= sourceGrid->nYpts) {
            // It's not even on the grid
            // XXX how can this happen?
            continue;
        }

        pswarpMap *map = sourceGrid->maps[xGrid][yGrid]; // Locally linear transformation
        double xOut, yOut;          // Output coordinates
        if (!pswarpMapApply(&xOut, &yOut, map, xIn + 0.5, yIn + 0.5)) {
            psError(PS_ERR_UNKNOWN, false, "Unable to transform coordinates for source at %f,%f\n",
                    xIn, yIn);
            psFree(outSources);
            psFree(sourceGrid);
            return false;
        }
        xOut += output->image->col0 - 0.5;
        yOut += output->image->row0 - 0.5;
        if (xOut < minX || xOut > maxX || yOut < minY || yOut > maxY) {
            // It's not in the output image
            continue;
        }

        // Generate the new source in the output frame
        //
        // Magnitudes will be off if there's any change in scale, but for our purposes (mainly x,y and
        // relative flux) that's OK.
        pmSource *new = pmSourceAlloc(); // New source
        new->peak = pmPeakAlloc(xOut, yOut, source->peak->flux, PM_PEAK_LONE);
        new->peak->flux = source->peak->flux;
        new->type = source->type;
        new->mode = source->mode;
        new->psfMag = source->psfMag;
        new->extMag = source->extMag;
        new->errMag = source->errMag;
        new->apMag = source->apMag;
        new->pixWeight = source->pixWeight;
        new->psfChisq = source->psfChisq;
        new->crNsigma = source->crNsigma;
        new->extNsigma = source->extNsigma;
        new->sky = source->sky;
        new->skyErr = source->skyErr;

        new->modelPSF = pmModelAlloc(source->modelPSF->type);
	new->modelPSF->params->data.F32[PM_PAR_I0]=
	  (isfinite(new->psfMag) ? pow(10.0, -0.4*new->psfMag) : NAN);
	new->modelPSF->dparams->data.F32[PM_PAR_I0]=
	  (isfinite(new->psfMag) ? new->errMag*pow(10.0, -0.4*new->psfMag) : NAN);

#if 0
        // XXX Note that this will not set the correct axes
        pmPSF_AxesToModel(new->modelPSF->params->data.F32,
                          pmPSF_ModelToAxes(source->modelPSF->params->data.F32, 20.0));
#endif

        // Propagate the position erorrs
        float dxIn, dyIn;           // Errors in input coordinates
        dxIn = model->dparams->data.F32[PM_PAR_XPOS];
        dyIn = model->dparams->data.F32[PM_PAR_YPOS];

        float dxOut, dyOut;         // Errors in output coordinates
        dxOut = sqrt(PS_SQR(map->Xx * dxIn) + PS_SQR(map->Xy * dyIn));
        dyOut = sqrt(PS_SQR(map->Yx * dxIn) + PS_SQR(map->Yy * dyIn));

        new->modelPSF->params->data.F32[PM_PAR_XPOS] = xOut;
        new->modelPSF->params->data.F32[PM_PAR_YPOS] = yOut;
        new->modelPSF->dparams->data.F32[PM_PAR_XPOS] = dxOut;
        new->modelPSF->dparams->data.F32[PM_PAR_YPOS] = dyOut;

        new->modelPSF->chisq = model->chisq;
        new->modelPSF->chisqNorm = model->chisqNorm;
        new->modelPSF->mag = model->mag;
        new->modelPSF->nDOF = model->nDOF;
        new->modelPSF->nIter = model->nIter;
        new->modelPSF->flags = model->flags;
        new->modelPSF->radiusFit = model->radiusFit;

        psArrayAdd(outSources, SOURCE_ARRAY_BUFFER, new);
        psFree(new);                // Drop reference
    }
    psFree(sourceGrid);
    psFree(outSources);             // Drop reference
    return true;
}

