#include "streaksremove.h"

static void freeAstrometry(strkAstrom *astrom)
{
    if (!astrom) {
        return;
    }
    psFree(astrom->pt);
}

// Initialize the astrometry object for current cell
strkAstrom *
streakSetAstrometry(strkAstrom *astrom, ippStage stage, pmFPA *fpa, pmChip *chip, bool fromCell, psMetadata *md, int numCols, int numRows)
{
    if (!astrom) {
        astrom = psAlloc(sizeof(strkAstrom));
        psMemSetDeallocator(astrom, (psFreeFunc) freeAstrometry);
        astrom->pt = pmAstromObjAlloc();
    }
    astrom->fpa = fpa;
    astrom->chip = chip;

    int cell_x0, cell_y0;
    int xParityCell, yParityCell;
    bool mdok;

    if (fromCell) {
        // XXX: I think that this is not needed
        // We are only asked to get the concepts from the cell when we're reading from a pmfile (skycell)
        // In that case we (x0,y0) = (0,0) and the parirties are positive
        // If the x0, y0 are not zero they are probably in the FITS unity based array format
        // We should probably assert that these are zero
        cell_x0 =  psMetadataLookupS32(&mdok, md, "CELL.X0");
        if (!mdok) {
            psError(PS_ERR_UNKNOWN, true, "CELL.X0 for cell is not set.\n");
            return NULL;
        }
        cell_y0 = psMetadataLookupS32(&mdok, md, "CELL.Y0");
        if (!mdok) {
            psError(PS_ERR_UNKNOWN, true, "CELL.Y0 for cell is not set.\n");
            return NULL;
        }
        xParityCell = psMetadataLookupS32(&mdok, md, "CELL.XPARITY");
        if (!mdok || (xParityCell != 1 && xParityCell != -1)) {
            psError(PS_ERR_UNKNOWN, true, "CELL.XPARITY for cell is not set.\n");
            return false;
        }
        yParityCell = psMetadataLookupS32(&mdok, md, "CELL.YPARITY");
        if (!mdok || (yParityCell != 1 && yParityCell != -1)) {
            psError(PS_ERR_UNKNOWN, true, "CELL.YPARITY for cell is not set.\n");
            return false;
        }
    } else if (md && (stage == IPP_STAGE_RAW)) {
        // The metadata is the raw header
        // Assumes GPC1
        //
        // Shouldn't these lookups be F32 ?
        cell_x0 =  psMetadataLookupS32(&mdok, md, "IMNPIX1");
        if (!mdok) {
            psError(PS_ERR_UNKNOWN, true, "IMNPIX1 for cell is not set.\n");
            // return NULL;
        }
        cell_y0 = psMetadataLookupS32(&mdok, md, "IMNPIX2");
        if (!mdok) {
            psError(PS_ERR_UNKNOWN, true, "IMNPIX2 for cell is not set.\n");
            return NULL;
        }

        xParityCell = psMetadataLookupS32(&mdok, md, "ATM1_1");
        if (!mdok || (xParityCell != 1 && xParityCell != -1)) {
            psError(PS_ERR_UNKNOWN, true, "CELL.XPARITY for cell is not set.\n");
            return false;
        }
        yParityCell = psMetadataLookupS32(&mdok, md, "ATM2_2");
        if (!mdok || (yParityCell != 1 && yParityCell != -1)) {
            psError(PS_ERR_UNKNOWN, true, "CELL.YPARITY for cell is not set.\n");
            return false;
        }
    } else {
        // no metadata regular cell
        // used by warpedpixels
        cell_x0 = 0;
        cell_y0 = 0;
        xParityCell = 1;
        yParityCell = 1;
    }

    astrom->cell_x0 = (double) cell_x0;
    astrom->cell_y0 = (double) cell_y0;
    // XXX: make sure we're applying cell parity correctly
    // the mosaic code uses (xParityCell * xParityChip == xParityTarget) ? -1 : 1
    //                            (yParityCell * yParityChip == yParityTarget) ? -1 : 1
    // since the transform already accounts for the chip parity we just use the cell parity
    astrom->xParity = (double) xParityCell;
    astrom->yParity = (double) yParityCell;

    astrom->numCols = numCols;
    astrom->numRows = numRows;

    return astrom;
}



void
chipToCell(double *xCell, double *yCell, strkAstrom *astrom, double xChip, double yChip)
{
    // convert from chip to cell
    if (astrom->xParity > 0) {
        *xCell = xChip - astrom->cell_x0;
    } else {
        *xCell = astrom->cell_x0 - 1 - xChip;
    }
    if (astrom->yParity > 0) {
        *yCell = yChip - astrom->cell_y0;
    } else {
        *yCell = astrom->cell_y0 - 1 - yChip;
    }
}

void
cellToChip(double *xChip, double *yChip, strkAstrom *astrom, double xCell, double yCell)
{
    if (astrom->xParity > 0) {
        *xChip = xCell + astrom->cell_x0;
    } else {
        *xChip = astrom->cell_x0 - 1 - xCell;
    }
    if (astrom->yParity > 0) {
        *yChip = yCell + astrom->cell_y0;
    } else {
        *yChip =  astrom->cell_y0 - 1 - yCell;
    }
}

void
cellToChipInt(unsigned int *xChip, unsigned int *yChip, strkAstrom *astrom, int xCell, int yCell)
{
    if (astrom->xParity > 0) {
        *xChip = xCell + astrom->cell_x0;
    } else {
        *xChip = astrom->cell_x0 - 1 - xCell;
    }
    if (astrom->yParity > 0) {
        *yChip = yCell + astrom->cell_y0;
    } else {
        *yChip =  astrom->cell_y0 - 1 - yCell;
    }
}

bool
SkyToLocal(strkPt *outPt, strkAstrom *astrom, double ra, double dec)
{
    // generate a local project using the RA, DEC of the 0,0 pixel of the chip as the
    // projection center with the same plate scale as the nominal TP->Sky astrometry.

    pmFPA *fpa   = (pmFPA *) astrom->fpa;

    // find the RA,DEC coords of the 0,0 pixel for this chip:

    psPlane ptTP;
    psSphere ptSky;

    ptSky.r = ra;
    ptSky.d = dec;
    ptSky.rErr = 0.0;
    ptSky.dErr = 0.0;

    psProject(&ptTP, &ptSky, fpa->toSky);

    outPt->x = ptTP.x;
    outPt->y = ptTP.y;

    return true;
}

bool
LocalToSky(strkPt *outPt, strkAstrom *astrom, strkPt *inPt)
{
    // generate a local project using the RA, DEC of the 0,0 pixel of the chip as the
    // projection center with the same plate scale as the nominal TP->Sky astrometry.

    pmFPA *fpa   = (pmFPA *) astrom->fpa;

    // find the RA,DEC coords of the 0,0 pixel for this chip:

    psPlane ptTP;
    psSphere ptSky;

    ptTP.x = inPt->x;
    ptTP.y = inPt->y;
    ptTP.xErr = 0.0;
    ptTP.yErr = 0.0;

    psDeproject(&ptSky, &ptTP, fpa->toSky);

    outPt->x = ptSky.r;
    outPt->y = ptSky.d;

    return true;
}

bool
componentBounds(int *minX, int *minY, int *maxX, int *maxY, strkAstrom *astrom, int numCols, int numRows)
{
    // find the bounds of the (padded) chip region in tangent-plane coordinates

    pmFPA *fpa   = (pmFPA *) astrom->fpa;
    pmChip *chip = (pmChip *) astrom->chip;

    psPlane ptCH, ptFP, TPo, TPx, TPy;

    // coordinate of the chip center:
    ptCH.x = 0.5*numCols;
    ptCH.y = 0.5*numRows;
    psPlaneTransformApply(&ptFP, chip->toFPA, &ptCH);
    psPlaneTransformApply(&TPo, fpa->toTPA,  &ptFP);

    // coordinate of the chip center + dX/2:
    ptCH.x = numCols;
    ptCH.y = 0.5*numRows;
    psPlaneTransformApply(&ptFP, chip->toFPA, &ptCH);
    psPlaneTransformApply(&TPx, fpa->toTPA,  &ptFP);

    // coordinate of the chip center + dY/2:
    ptCH.x = 0.5*numCols;
    ptCH.y = numRows;
    psPlaneTransformApply(&ptFP, chip->toFPA, &ptCH);
    psPlaneTransformApply(&TPy, fpa->toTPA,  &ptFP);

    // half-lengths of the two sides in tangent-plane coords:
    double xSize = hypot (TPx.x - TPo.x, TPx.y - TPo.y);
    double ySize = hypot (TPy.x - TPo.x, TPy.y - TPo.y);
    double radius = hypot (xSize, ySize);

    // define the region encompassed by the radius with some padding:
    *minX = TPo.x - 1.1*radius;
    *minY = TPo.y - 1.1*radius;
    *maxX = TPo.x + 1.1*radius;
    *maxY = TPo.y + 1.1*radius;

    return true;
}

bool
skyToCell(strkPt *outPt, strkAstrom *astrom, double ra, double dec)
{
    pmFPA *fpa   = (pmFPA *) astrom->fpa;
    pmChip *chip = (pmChip *) astrom->chip;
    pmAstromObj *pt = (pmAstromObj *) astrom->pt;

    pt->sky->r = ra;
    pt->sky->d = dec;
    pt->sky->rErr = 0;  // Is setting these errors to zero the right thing to do?
    pt->sky->dErr = 0;

    // convert from sky to TP to FP
    psProject(pt->TP, pt->sky, fpa->toSky);
    psPlaneTransformApply(pt->FP, fpa->fromTPA, pt->TP);
    // convert from FP to chip
    psPlaneTransformApply(pt->chip, chip->fromFPA, pt->FP);

    chipToCell(&outPt->x, &outPt->y, astrom, pt->chip->x, pt->chip->y);

//    printf("cell: %f %f chip: %f %f\n", outPt->x, outPt->y, pt->chip->x, pt->chip->y);

    return true;
}

bool
cellToSky(strkPt *outPt, strkAstrom *astrom, double x, double y)
{
    pmFPA *fpa   = (pmFPA *) astrom->fpa;
    pmChip *chip = (pmChip *) astrom->chip;
    pmAstromObj *pt = (pmAstromObj *) astrom->pt;

    cellToChip(&pt->chip->x, &pt->chip->y, astrom, x, y);

    pt->chip->xErr = 0;  // Is setting these errors to zero the right thing to do?
    pt->chip->yErr = 0;

        // chip to FP
    psPlaneTransformApply(pt->FP, chip->toFPA, pt->chip);
    // FP to TP to sky
    psPlaneTransformApply(pt->TP, fpa->toTPA, pt->FP);
    psDeproject(pt->sky, pt->TP, fpa->toSky);

    outPt->x = pt->sky->r;
    outPt->y = pt->sky->d;

    return true;
}


static bool
readAstrometry(streakFiles *sf)
{
    pmHDU *phu = pmFPAviewThisPHU(sf->view, sf->inAstrom->fpa);
    if (phu) {
        bool status;
        char *ctype = psMetadataLookupStr(&status, phu->header, "CTYPE1");
        if (ctype) {
            sf->bilevelAstrometry = !strcmp (&ctype[4], "-DIS");
        }
    }

    pmHDU *hdu = pmFPAviewThisHDU(sf->view, sf->inAstrom->fpa);
    PS_ASSERT_PTR_NON_NULL(hdu, 1)
    PS_ASSERT_PTR_NON_NULL(hdu->header, 1)
    if (sf->bilevelAstrometry) {
        if (!pmAstromReadBilevelMosaic(sf->inAstrom->fpa, phu->header)) {
            psError(PS_ERR_UNKNOWN, false, "Unable to read bilevel mosaic astrometry for input FPA.");
            return false;
        }
       if (!pmAstromReadBilevelChip (sf->chip, hdu->header)) {
            psError(PS_ERR_UNKNOWN, false, "Unable to read bilevel chip astrometry for input FPA.");
            return false;
        }
    } else {

        // we use a default FPA pixel scale of 1.0
        if (!pmAstromReadWCS (sf->inAstrom->fpa, sf->chip, hdu->header, 1.0)) {
            psError(PS_ERR_UNKNOWN, false, "Unable to read WCS astrometry for input FPA.");
            return false;
        }
    }

    return true;
}

void
setupAstrometry(streakFiles *sf)
{
    bool status;
    // load astrometry file
    if (USE_SUPPLIED_ASTROM(sf->stage)) {
        sf->inAstrom = pmFPAfileDefineFromArgs(&status, sf->config, "PSWARP.ASTROM", "ASTROM");
    } else {
        // otherwise get astrometry from pmfile
        if (!sf->inImage->pmfile) {
            streaksExit("unexpected null pmFPAfile", PS_EXIT_CONFIG_ERROR);
        }
        sf->inAstrom = sf->inImage->pmfile;
    }
    sf->view = pmFPAviewAlloc(0);

    if (!pmFPAfileIOChecks(sf->config, sf->view, PM_FPA_BEFORE)) {
        psError(PS_ERR_UNKNOWN, false, "Failed to load input.");
        streaksExit("", PS_EXIT_DATA_ERROR);
    }

    while ((sf->chip = pmFPAviewNextChip(sf->view, sf->inAstrom->fpa, 1)))  {
        if (sf->inAstrom->fpa->chips->n == 1) {
            // There's only one chip in this FPA and we've got it so break
            break;
        }
        bool status;
        psString chip_name = psMetadataLookupStr(&status, sf->chip->concepts, "CHIP.NAME");
        if (!strcmp(chip_name, sf->class_id)) {
            break;
        }
    }
    if (!sf->chip) {
        psError(PS_ERR_UNKNOWN, true, "Failed to find chip with data.");
        streaksExit("", PS_EXIT_DATA_ERROR);
    }
    if (!pmFPAfileIOChecks(sf->config, sf->view, PM_FPA_BEFORE)) {
        psError(PS_ERR_UNKNOWN, false, "failed to load chip");
        streaksExit("", PS_EXIT_DATA_ERROR);
    }
    if (!readAstrometry(sf)) {
        psError(PS_ERR_UNKNOWN, false, "failed to read astrometry");
        streaksExit("", PS_EXIT_DATA_ERROR);
    }
}

static void chipToSky(pmAstromObj *pt, pmFPA *fpa, pmChip *chip)
{
    // chip to FP
    psPlaneTransformApply(pt->FP, chip->toFPA, pt->chip);
    // FP to TP to sky
    psPlaneTransformApply(pt->TP, fpa->toTPA, pt->FP);
    psDeproject(pt->sky, pt->TP, fpa->toSky);
}


bool
linearizeTransforms(strkAstrom *astrom)
{
    pmFPA  *inFPA  = (pmFPA *) astrom->fpa;
    pmChip *inChip = (pmChip *) astrom->chip;

    // compute ra and dec of chip center
    pmAstromObj *center = pmAstromObjAlloc();
    center->chip->x = (double) astrom->numCols / 2.;
    center->chip->xErr = 0;
    center->chip->y = (double) astrom->numRows / 2.;
    center->chip->yErr = 0;

    chipToSky(center, inFPA, inChip);

    psRegion bounds = {0, astrom->numCols - 1, 0, astrom->numRows - 1 };

    // create skeleton containers for the linearized transforms;
    pmFPA  outFPA;
    pmChip outChip;

    memset(&outFPA, 0, sizeof(outFPA));
    memset(&outChip, 0, sizeof(outChip));

    outFPA.toSky = psProjectionAlloc (center->sky->r, center->sky->d, inFPA->toSky->Xs, inFPA->toSky->Ys, PS_PROJ_TAN);

    psFree(center);

    if (!pmAstromLinearizeToSky(inFPA, inChip, &outFPA, &outChip, &bounds)) {
        psError(PS_ERR_UNKNOWN, false, "Failed to linearize astrometry\n");
        return false;
    }

    psFree(inFPA->toSky);
    inFPA->toSky = outFPA.toSky;

    psFree(inFPA->toTPA);
    inFPA->toTPA = outFPA.toTPA;

    psFree(inFPA->fromTPA);
    inFPA->fromTPA = outFPA.fromTPA;

    psFree(inChip->toFPA)
    inChip->toFPA = outChip.toFPA;

    psFree(inChip->fromFPA);
    inChip->fromFPA = outChip.fromFPA;

    return true;
}
