#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);

bool
computeWarpedPixels(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->warpedPixels = psImageAlloc(width, height, PS_TYPE_U8);
    psImageInit(sf->warpedPixels, 0);

    for (int i=0; i<n; i++) {
        psString filename = psArrayGet(skycells, i);
        psString resolved_name = pmConfigConvertFilename(filename, config, false, false);

        addTouchedPixels(sf, resolved_name);
        psFree(resolved_name);
    }

    bool writeTouchedPixels = false; // XXX: make this an argument to the program
    if (writeTouchedPixels) {
        // write out the warped 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("warpedpixels.fits", "w");

        psFitsWriteImage(fits, header, sf->warpedPixels, 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->warpedPixels->numCols, sf->warpedPixels->numRows);

    // convert corners of skycell to sky 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])) {
            psError(PS_ERR_IO, false, "failed to convert pt %d of %s to sky coords: %s", fileName);
            streaksExit("", PS_EXIT_DATA_ERROR);
        }
        strkPt p;
        // convert to chip coordinates
        if (!skyToCell(&p, sf->astrom, sky.r, sky.d)) {
            psError(PS_ERR_IO, false, "failed to convert pt %d of %s to sky coords: %s", fileName);
            streaksExit("", PS_EXIT_DATA_ERROR);
        }
        pt[i].x = p.x;
        pt[i].y = p.y;
    }

    psFree(wcs);

    // put the corners in the desired order (see comments below)

    nameCorners(pt);
    psString type;
    if (pt[0].y < pt[2].y ) {
        type = "1";
    } else {
        type = "2";
    }
#ifdef DEBUG_PRINT
    printf("\nSKYCELL %s Type: %s\n", fileName, type);
    for (int i=0; i<4; i++) {
        printf("%f %f\n", pt[i].x, pt[i].y);
    }
#endif
    // Now set the touched pixels
    int ymin = fmax(0, pt[1].y );
    int ymax = fmin(pt[3].y + 0.5, sf->warpedPixels->numRows - 1);
#if (DEBUG_PRINT > 1)
    printf("\nymin: %d ymax: %d\n", ymin, ymax);
#endif
    for (int y = ymin ; y <= ymax; y++) {
        int xleft  = xLeft(pt, y);
        int xright = xRight(pt, y);

        if (xleft < 0) {
            xleft = 0;
        }
        if (xleft > sf->warpedPixels->numCols) {
            continue;
        }
        if (xright < 0) {
            continue;
        }
        if (xright >= sf->warpedPixels->numCols) {
            xright = sf->warpedPixels->numCols - 1;
        }
#if (DEBUG_PRINT > 1)
        printf("  y: %d xleft: %d xright: %d\n", y, xleft, xright);
#endif

        psU8 *rowPixels = sf->warpedPixels->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[0].y) {
        // left boundary is the line from pt 0 to pt 1
        x_d = xOfY(&pt[0], &pt[1], y);
    } else {
        // left boundary is the line from pt 0 to pt 3
        x_d = xOfY(&pt[0], &pt[3], y);
    }
    return (int) (x_d + 0.5);
}

static int xRight(psPlane *pt, int y)
{
    double x_d;
    if (y < pt[2].y) {
        // right boundary is the line from pt 1 to pt 2
        x_d = xOfY(&pt[1], &pt[2], y);

    } else {
        // right boundary is the line from pt 2 to pt 3
        x_d = xOfY(&pt[2], &pt[3], y);
    }
    return (int) x_d;
}

/*
 * To compute the overlap of a quadrilateral with a set of
 * points we transform the 4 corners to image coordinates
 * and name the 4 points of the quad 
            pt 0 is left most (bottom most corner)
            pt 1 is bottom most (right most corner)
            pt 2 is right most (top most corner)
            pt 3 is top most (left most corner)

* Then we can divide the quad into 3 regions A B and C based
* on the y coordinate

* (in the degenerate case of a rectangle aligned to the axes
* we only have 1 region)
* In each of the three regions A, B, and C the test for whether a
* point is contained in the quad on scanline y is simply

        x >= xleft(y) x < xright(y)

* where xleft(y) and xright(y) are the bounding lines at the given
* y coordinate.

The following diagrams show some examples. The 4 points are labeled 0 to 3
The three regions are labeld A B and C, the boundaries between the regions
ocur at pt0.y and pt2.y

Example 1

                3
                 C       
         ---------------2           left boundary:  line 0_1 y < pt0.y
                 B                                  line 0_3 y >= pt0.y
          0--------------
                 A                  right boundary: line 1_2 y < pt2.y
                   1                                line 2_3  y >= pt.y
**************************
Example 2
                3
              C
        0----------------
              B     
      ----------------2
              A
            1

**************************
Example 3

        3       2
            
            B

        0       1

(region A and C are empty in the case where the points form a rectangle)
*/
            
/*
    Name the corners
    The following algorithm works for points that form a quadrilateral 
    I think it also works for the situation where 3 points are co-linear
    and we have a triangle, but that isn't important for our purposes

*/

static void
nameCorners(psPlane pt[4])
{
    psPlane   pt0, pt1, pt2, pt3;

    /* find pt0 the left most (bottom most) corner */
    int imin = 0;
    int  min = (int) pt[0].x;
    int i;
    for (i=1; i<4; i++) {
        int x = pt[i].x;
        if ((x < min) ||
            ((x == min) && (pt[i].y < pt[imin].y))) {
            imin = i;
            min = x;
        }
    }
    // remve pt0 from the list
    pt0 = pt[imin];
    for (i=imin; i<3; i++) {
        pt[i] = pt[i+1];
    }

    // find the bottom most (right most) corner from the remaining 3
    imin = 0;
    min = pt[0].y;
    for (i=1; i<3; i++) {
        int y = pt[i].y;
        if ((y < min) ||
            ((y == min) && (pt[i].x > pt[imin].x)) ) {
            imin = i;
            min = y;
        }
    }
    // remve pt1 from the list
    pt1 = pt[imin];
    for (i=imin; i<3; i++) {
        pt[i] = pt[i+1];
    }
            
    // now find the right most (top most) of the remaining 2 points
    if ((pt[0].x > pt[1].x) ||
        ((pt[0].x == pt[1].x) && (pt[0].y > pt[1].y))) {
        pt2 = pt[0];
        pt3 = pt[1];
    } else {
        pt2 = pt[1];
        pt3 = pt[0];
    }
    /* write the outputs */
    pt[0] = pt0;
    pt[1] = pt1;
    pt[2] = pt2;
    pt[3] = pt3;
}
