#include "streaksremove.h"
#include "pmAstrometryWCS.h"

#define DEBUG_PRINT 1

static void addTouchedPixels(streakFiles *sf, psString fileName);
static void nameCorners(psPlane pt[4]);
static int xLeft(psPlane *pt, int y);
static int xRight(psPlane *pt, int y);

#define  BL 0
#define  BR 1
#define  TL 2
#define  TR 3

// Examine the set of diff skycells and compute the pixels that were
// included in difference processing
bool
computeDiffedPixels(streakFiles *sf)
{
    bool status;
    pmConfig    *config = sf->config;

    psArray *skycells = psMetadataLookupPtr(&status, config->arguments, "SKYCELLS");
    if (skycells == NULL) {
        return false;
    }

    int n = psArrayLength(skycells);

    psRegion     *bounds = pmChipPixels(sf->chip);

    int width  = bounds->x1 - bounds->x0;
    int height = bounds->y1 - bounds->y0;

    psFree(bounds);

    sf->diffedPixels = psImageAlloc(width, height, PS_TYPE_U8);
    psImageInit(sf->diffedPixels, 0);

    for (int i=0; i<n; i++) {
        psString filename = psArrayGet(skycells, i);
        psString resolved_name = pmConfigConvertFilename(filename, config, false, false);
        if (resolved_name == NULL) {
            psError(PS_ERR_IO, false, "failed to resolve skycell file: %s", filename);
            streaksExit("", PS_EXIT_DATA_ERROR);
        }
        addTouchedPixels(sf, resolved_name);
        psFree(resolved_name);
    }

    bool writeTouchedPixels = false; // XXX: make this an argument to the program
    if (writeTouchedPixels) {
        // write out the diffed pixels
        psMetadata * header = psMetadataAlloc();
        if (sf->inAstrom->fpa->toSky->type != PS_PROJ_DIS) {
            pmAstromWriteWCS(header, sf->inAstrom->fpa, sf->chip, 0.001);
        }
        psFits *fits = psFitsOpen("diffedpixels.fits", "w");

        psFitsWriteImage(fits, header, sf->diffedPixels, 0, NULL);
        psFitsClose(fits);
        psFree(header);
    }
    return true;
}

static void
addTouchedPixels(streakFiles *sf, psString fileName)
{
    // read fits header
    psFits *fits = psFitsOpen(fileName, "r");
    if (!fits) {
        psError(PS_ERR_IO, false, "failed to open skycell file: %s", fileName);
        streaksExit("", PS_EXIT_DATA_ERROR);
    }
    psMetadata *header = psFitsReadHeader(NULL, fits);
    psFitsClose(fits);
    if (!header) {
        psError(PS_ERR_IO, false, "failed to read fixts header from skycell file: %s", fileName);
        streaksExit("", PS_EXIT_DATA_ERROR);
    }
    // set up astrometry for conversion from skycell image to sky
    pmAstromWCS *wcs = pmAstromWCSfromHeader(header);
    if (!wcs) {
        psError(PS_ERR_IO, false, "failed to read astrometry  from skycell file: %s", fileName);
        streaksExit("", PS_EXIT_DATA_ERROR);
    }
    int naxis1 = psMetadataLookupS32(NULL, header, "NAXIS1");
    int naxis2 = psMetadataLookupS32(NULL, header, "NAXIS2");

    psFree(header);

    /* now set up our wrapper to the chip astrometry to apply to the whole chip */
    sf->astrom = streakSetAstrometry(sf->astrom, sf->stage, sf->inAstrom->fpa, sf->chip, false, NULL,
        sf->diffedPixels->numCols, sf->diffedPixels->numRows);

    // convert corners of skycell to chip coordinates
    // compute overlap with the chip using astrometry
    psPlane pt[4];
    pt[0].x = 0;
    pt[0].y = 0;

    pt[1].x = naxis1 - 1;
    pt[1].y = 0;

    pt[2].x = naxis1 - 1;
    pt[2].y = naxis2 - 1;

    pt[3].x = 0;
    pt[3].y = naxis2 - 1;

    for (int i = 0; i < 4 ; i++ ) {
        psSphere sky;

        // convert corner of skycell to sky coordinates
        if (!pmAstromWCStoSky(&sky, wcs, &pt[i])) {
            psErrorStackPrint(stderr, "failed to convert pt %d of %s to sky coords", i, fileName);
            streaksExit("", PS_EXIT_DATA_ERROR);
        }
        strkPt p;
        // convert to chip coordinates
        if (!skyToCell(&p, sf->astrom, sky.r, sky.d)) {
            psErrorStackPrint(stderr, "failed to convert pt %d of %s to sky coords", i, fileName);
            streaksExit("", PS_EXIT_DATA_ERROR);
        }
        pt[i].x = round(p.x);
        pt[i].y = round(p.y);
    }

    psFree(wcs);

    // Identify the bottom left, bottom right, top left, and top right corners
    nameCorners(pt);

#ifdef DEBUG_PRINT
    fprintf(stderr, "\nSKYCELL %s\n", fileName);
    fprintf(stderr, "BL %f %f\n", pt[BL].x, pt[BL].y);
    fprintf(stderr, "BR %f %f\n", pt[BR].x, pt[BR].y);
    fprintf(stderr, "TL %f %f\n", pt[TL].x, pt[TL].y);
    fprintf(stderr, "TR %f %f\n", pt[TR].x, pt[TR].y);
#endif

    int xmax = sf->diffedPixels->numCols - 1;
    int ymin;
    int ymax; 
    
    if (pt[BL].y <= pt[BR].y) {
        ymin = fmax(0, pt[BL].y);
    } else {
        ymin = fmax(0, pt[BR].y);
    }
    if (pt[TR].y >= pt[TL].y) {
        ymax = fmin(pt[TR].y, sf->diffedPixels->numRows - 1);
    } else {
        ymax = fmin(pt[TL].y, sf->diffedPixels->numRows - 1);
    }
        
#if (DEBUG_PRINT > 1)
    fprintf(stderr, "\nxmin: %d xmax: %d\n", 0, xmax);
    fprintf(stderr, "\nymin: %d ymax: %d\n", ymin, ymax);
#endif

    // Now set the touched pixels
    for (int y = ymin ; y <= ymax; y++) {

        int xleft  = xLeft(pt, y) - 1;
        int xright = xRight(pt, y) + 1;

        if (xright < 0) {
            continue;
        }
        if (xleft > xmax) {
            continue;
        }
        if (xleft < 0) {
            xleft = 0;
        }
        if (xright > xmax) {
            xright = xmax;
        }
#if (DEBUG_PRINT > 1)
        fprintf(stderr, "  y: %d xleft: %d xright: %d\n", y, xleft, xright);
#endif

        psU8 *rowPixels = sf->diffedPixels->data.U8[y];

        int n = xright - xleft + 1;
        if (n > 0) {
            memset(&rowPixels[xleft], 255, n);
        }
    }
}

// x as a function of y for the line between two points
// Note: the caller guarentees that the y's of the two points are different
static double xOfY(psPlane *pI, psPlane *pJ, int y)
{
    double m_inv = (double) (pJ->x - pI->x) / (double) (pJ->y - pI->y);

    double x = ((double) y - pI->y) * m_inv + (double) pI->x;

    return x;
}

static int xLeft(psPlane *pt, int y)
{
    double x_d;

    if ((y <= pt[BL].y ) && ((pt[BL].y >= pt[BR].y))) {
        // left boundary is the line from bottom left to bottom right
        x_d = xOfY(&pt[BL], &pt[BR], y);
    } else if ( y <= pt[TL].y ) {
        // left boundary is the line from Bottom left to top left
        x_d = xOfY(&pt[BL], &pt[TL], y);
    } else {
        // left boundary is the line from top left to top right
        x_d= xOfY(&pt[TL], &pt[TR], y);
    }

    return (int) floor(x_d);
}

static int xRight(psPlane *pt, int y)
{
    double x_d;
    if ((y <= pt[BR].y) && (pt[BR].y > pt[BL].y)) {
        // right boundary is the line from Bottom right to Top right
        x_d = xOfY(&pt[BL], &pt[BR], y);
    } else if ( y<= pt[TR].y) {
        // right boundary is the line from Top left to top right
        x_d = xOfY(&pt[BR], &pt[TR], y);
    } else {
        x_d = xOfY(&pt[TL], &pt[TR], y);
    }
    return (int) ceil(x_d);
}

static void
nameCorners(psPlane pt[4])
{
    // sort points top to bottom (in case of ties take favor rightmost)
    int i;
    int j;
    for (i=0; i<4; i++) {
        for (j = i + 1; j < 4; j++) {
            if ((pt[j].y > pt[i].y) || 
                ((pt[j].y == pt[i].y) && (pt[j].x > pt[i].x))) {
                psPlane tmpPt = pt[j];
                pt[j] = pt[i];
                pt[i] = tmpPt;
            }
        }
    }
    psPlane bl, br;
    // sort bottom two points in x
    if (pt[3].x < pt[2].x) {
        bl = pt[3];
        br = pt[2];
    } else {
        bl = pt[2];
        br = pt[3];
    }
    // sort the top two points in x
    psPlane tl, tr;
    if (pt[0].x < pt[1].x) {
        tl = pt[0];
        tr = pt[1];
    } else {
        tl = pt[1];
        tr = pt[0];
    }
    pt[BL] = bl;
    pt[BR] = br;
    pt[TL] = tl;
    pt[TR] = tr;
}
