/** @file  tst_psAstrometry01.c
*
*  @brief The code in this file will test the code in psAstrometry.[ch]
*
*  @author GLG, MHPCC
*
*  @version $Revision: 1.10 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-01-14 01:13:15 $
*
*  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
*/

#include "psTest.h"
#include "pslib.h"

static psS32 test1( void );
static psS32 test2( void );
static psS32 test3( void );

testDescription tests[] = {
                              {test1, -1, "Tests psFixedPatternAlloc()", 0, false},
                              {test2, -2, "Tests psGrommitAlloc()", 0, false},
                              {test3, -3, "MISC", 0, false},
                              {NULL}
                          };
#define RA  1.0
#define DEC  2.0
#define HA  3.0
#define ZD  4.0
#define AZ  5.0
#define ROT_ANGLE 6.0
#define TEMPERATURE 7.0
#define PRESSURE 8.0
#define HUMIDITY 20.0
#define EXP_TIME 22.0
#define WAVELENGTH 11.0
#define NAME  "The Kaiser Royal Observatory"
#define LATITUDE 20.7
#define LONGITUDE 156.3
#define HEIGHT  3055.0
#define LAPSERATE 20.0

#define NUM_READOUTS 2
#define READOUT_NUM_ROWS 8
#define READOUT_NUM_COLS 8

#define CELL_WIDTH READOUT_NUM_COLS
#define CELL_HEIGHT READOUT_NUM_ROWS

#define CELL_GAP 2

#define NUM_CELLS 4
#define CELL_MIN_X 0
#define CELL_MAX_X CELL_WIDTH
#define CELL_MIN_Y 0
#define CELL_MAX_Y CELL_HEIGHT

#define NUM_CHIPS 2
#define CHIP_MIN_X 0
#define CHIP_MAX_X ((NUM_CELLS * CELL_WIDTH) + ((NUM_CELLS) * CELL_GAP))
#define CHIP_MIN_Y 0
#define CHIP_MAX_Y CELL_HEIGHT

#define CHIP_WIDTH CHIP_MAX_X
#define CHIP_HEIGHT CHIP_MAX_Y
#define CHIP_GAP 2

#define FPA_MIN_X 0
#define FPA_MAX_X ((NUM_CHIPS * CHIP_WIDTH) + ((NUM_CHIPS) * CHIP_GAP))
#define FPA_MIN_Y 0
#define FPA_MAX_Y CHIP_HEIGHT

#define ROW0  0
#define COL0  0

static void psPlaneDistortFree(psPlaneDistort *pd)
{
    psFree(pd->x);
    psFree(pd->y);
}

static void psPlaneTransformFree(psPlaneTransform *pd)
{
    psFree(pd->x);
    psFree(pd->y);
}

#define PS_CREATE_2D_IDENTITY_PLANE_TRANSFORM(NAME) \
{\
    NAME = (psPlaneTransform *) psAlloc(sizeof(psPlaneTransform)); \
    p_psMemSetDeallocator(NAME, (psFreeFcn) psPlaneTransformFree); \
    NAME->x = psDPolynomial2DAlloc(2, 2, PS_POLYNOMIAL_ORD); \
    NAME->y = psDPolynomial2DAlloc(2, 2, PS_POLYNOMIAL_ORD); \
    NAME->x->coeff[1][0] = 1.0; \
    NAME->y->coeff[0][1] = 1.0; \
}

#define PS_CREATE_4D_IDENTITY_PLANE_DISTORT(NAME) \
{\
    NAME = (psPlaneDistort *) psAlloc(sizeof(psPlaneDistort)); \
    p_psMemSetDeallocator(NAME, (psFreeFcn) psPlaneDistortFree); \
    NAME->x = psDPolynomial4DAlloc(2, 2, 2, 2, PS_POLYNOMIAL_ORD); \
    NAME->y = psDPolynomial4DAlloc(2, 2, 2, 2, PS_POLYNOMIAL_ORD); \
    NAME->x->coeff[1][0][0][0] = 1.0; \
    NAME->y->coeff[0][1][0][0] = 1.0; \
}

/*
    This routine will create a system of FPAs/Chips/Cells/Readouts.  For
    simplicity, an FPA is defined as a linear array of chips, and a chip is
    defined as a linear array of cells, both in the x direction.  The transforms
    between the various layers take into account the cell/chip and the boundaries
    between each.
*/
psFPA *genSystem()
{
    psS32 i;
    psS32 j;
    psS32 k;
    psImage *tmpImage;
    //    psTime* now = psTimeGetTime(PS_TIME_UTC);
    //    psObservatory* obs = psObservatoryAlloc(NAME, LATITUDE, LONGITUDE, HEIGHT, LAPSERATE);
    //    psExposure* exp = psExposureAlloc(RA, DEC, HA, ZD, AZ,
    //                                      now, ROT_ANGLE, TEMPERATURE, PRESSURE, HUMIDITY,
    //                                      EXP_TIME, WAVELENGTH, obs);
    //    psGrommit *grom = psGrommitAlloc(exp);
    psFPA *myFPA = psFPAAlloc(NUM_CHIPS, NULL);
    psChip **chips;
    psCell **cells;
    psReadout **readouts;

    // We create the transforms between the FPA and tangent plane.  The
    // transform is a simple identity transform.
    PS_CREATE_4D_IDENTITY_PLANE_DISTORT(myFPA->fromTangentPlane);
    PS_CREATE_4D_IDENTITY_PLANE_DISTORT(myFPA->toTangentPlane);

    chips = (psChip **) myFPA->chips->data;
    myFPA->chips->n = NUM_CHIPS;

    for (i=0;i<NUM_CHIPS;i++) {
        chips[i] = psChipAlloc(NUM_CELLS, myFPA);
        // We create the transforms between the chip and FPA.  The
        // transform is a simple identity transform.
        PS_CREATE_2D_IDENTITY_PLANE_TRANSFORM(chips[i]->toFPA);
        chips[i]->toFPA->x->coeff[0][0] = (double) (i * (CHIP_WIDTH + CHIP_GAP));
        chips[i]->toFPA->x->coeff[1][1] = 0.0;
        PS_CREATE_2D_IDENTITY_PLANE_TRANSFORM(chips[i]->fromFPA);
        chips[i]->fromFPA->x->coeff[0][0] = (double) (-i * (CHIP_WIDTH + CHIP_GAP));
        chips[i]->fromFPA->x->coeff[1][1] = 0.0;

        cells = (psCell **) chips[i]->cells->data;
        chips[i]->cells->n = NUM_CELLS;

        for (j=0;j<NUM_CELLS;j++) {
            cells[j] = psCellAlloc(NUM_READOUTS, chips[i]);
            // We create the transforms between the cell and the chip.  The
            // transform is a simple identity transform.
            PS_CREATE_2D_IDENTITY_PLANE_TRANSFORM(cells[j]->toChip);
            cells[j]->toChip->x->coeff[0][0] = (double) (j * (CELL_WIDTH + CELL_GAP));
            cells[j]->toChip->x->coeff[1][1] = 0.0;
            PS_CREATE_2D_IDENTITY_PLANE_TRANSFORM(cells[j]->fromChip);
            cells[j]->fromChip->x->coeff[0][0] = (double) (-j * (CELL_WIDTH + CELL_GAP));
            cells[j]->fromChip->x->coeff[1][1] = 0.0;
            PS_CREATE_2D_IDENTITY_PLANE_TRANSFORM(cells[j]->toFPA);
            cells[j]->toFPA->x->coeff[0][0] = (double) ((i * (CHIP_WIDTH + CHIP_GAP)) +
                                              (j * (CELL_WIDTH + CELL_GAP)));
            cells[j]->toFPA->x->coeff[1][1] = 0.0;
            PS_CREATE_2D_IDENTITY_PLANE_TRANSFORM(cells[j]->toTP);
            cells[j]->toTP->x->coeff[0][0] = cells[j]->toFPA->x->coeff[0][0];
            cells[j]->toTP->x->coeff[1][1] = 0.0;

            readouts = (psReadout **) cells[j]->readouts->data;
            cells[j]->readouts->n = NUM_READOUTS;

            for (k=0;k<NUM_READOUTS;k++) {
                tmpImage = psImageAlloc(READOUT_NUM_COLS, READOUT_NUM_ROWS, PS_TYPE_F32);
                for (psS32 row=0;row<READOUT_NUM_ROWS;row++) {
                    for(psS32 col=0;col<READOUT_NUM_COLS;col++) {
                        tmpImage->data.F32[row][col] = (float) ((i * (CHIP_WIDTH + CHIP_GAP)) +
                                                                (j * (CELL_WIDTH + CELL_GAP)));
                    }
                }
                readouts[k] = psReadoutAlloc();
                readouts[k]->image = tmpImage;
                *(psS32*)&readouts[k]->col0 = (j * (CELL_WIDTH + CELL_GAP));
                *(psS32*)&readouts[k]->row0 = 0;


            }
        }
    }

    return(myFPA);
}

psS32 main( psS32 argc, char* argv[] )
{
    psLogSetLevel( PS_LOG_INFO );

    return ( ! runTestSuite( stderr, "psAstrometry", tests, argc, argv ) );
}

#define X_NUM_ROWS 4
#define X_NUM_COLS 5
#define Y_NUM_ROWS 6
#define Y_NUM_COLS 7
#define X_SCALE  1.0
#define Y_SCALE  2.0
#define X0  50.0
#define Y0  60.0

psS32 test1( void )
{
    psS32 i;
    psS32 j;
    psFixedPattern *tmp = NULL;
    float x0 = X0;
    float y0 = Y0;
    float xScale = X_SCALE;
    float yScale = Y_SCALE;
    psImage *x = psImageAlloc(X_NUM_COLS, X_NUM_ROWS, PS_TYPE_F64);
    psImage *y = psImageAlloc(Y_NUM_COLS, Y_NUM_ROWS, PS_TYPE_F64);
    psS32 testStatus = 0;
    psS32 numPixels = 0;

    for (i=0;i<X_NUM_ROWS;i++) {
        for (j=0;j<X_NUM_COLS;j++) {
            x->data.F64[i][j] = (double) (i + j);
        }
    }
    for (i=0;i<Y_NUM_ROWS;i++) {
        for (j=0;j<Y_NUM_COLS;j++) {
            y->data.F64[i][j] = (double) (i - j);
        }
    }

    tmp = psFixedPatternAlloc(x0, y0, xScale, yScale, x, y);

    // Test the x pattern
    for (i=0;i<X_NUM_ROWS;i++) {
        for (j=0;j<X_NUM_COLS;j++) {
            if ((tmp->x)[i][j] != x->data.F64[i][j]) {
                printf("ERROR: (tmp->x)[%d][%d] is %f, should be %f\n", i, j,
                       (tmp->x)[i][j], x->data.F64[i][j]);
                testStatus = 1;
                numPixels++;
            }
        }
    }
    if (numPixels != 0) {
        printf("ERROR: x: %d incorrect pixels\n", numPixels);
    }

    // Test the y pattern
    numPixels = 0;
    for (i=0;i<Y_NUM_ROWS;i++) {
        for (j=0;j<Y_NUM_COLS;j++) {
            if ((tmp->y)[i][j] != y->data.F64[i][j]) {
                printf("ERROR: (tmp->y)[%d][%d] is %f, should be %f\n", i, j,
                       (tmp->y)[i][j], y->data.F64[i][j]);

                testStatus = 1;
                numPixels++;
            }
        }
    }
    if (numPixels != 0) {
        printf("ERROR: y: %d incorrect pixels\n", numPixels);
    }

    // Test the x variables
    if (tmp->nX != (X_NUM_ROWS * X_NUM_COLS)) {
        printf("ERROR: tmp->nX is %d, should be %d\n", tmp->nX, (X_NUM_ROWS * X_NUM_COLS));
        testStatus = 1;
    }
    if (tmp->x0 != X0) {
        printf("ERROR: tmp->x0 is %f, should be %f\n", tmp->x0, X0);
        testStatus = 1;
    }
    if (tmp->xScale != X_SCALE) {
        printf("ERROR: tmp->xScale is %f, should be %f\n", tmp->xScale, X_SCALE);
        testStatus = 1;
    }

    // Test the y variables
    if (tmp->nY != (Y_NUM_ROWS * Y_NUM_COLS)) {
        printf("ERROR: tmp->nY is %d, should be %d\n", tmp->nY, (Y_NUM_ROWS * Y_NUM_COLS));
        testStatus = 1;
    }
    if (tmp->y0 != Y0) {
        printf("ERROR: tmp->y0 is %f, should be %f\n", tmp->y0, Y0);
        testStatus = 1;
    }
    if (tmp->yScale != Y_SCALE) {
        printf("ERROR: tmp->yScale is %f, should be %f\n", tmp->yScale, Y_SCALE);
        testStatus = 1;
    }

    psFree(tmp);
    psFree(x);
    psFree(y);

    return 0;
}

psS32 test2()
{
    psS32 testStatus = 0;

    psTime* now = psTimeGetTime(PS_TIME_TAI);

    psObservatory* obs = psObservatoryAlloc(NAME, LATITUDE, LONGITUDE, HEIGHT, LAPSERATE);

    psExposure* exp = psExposureAlloc(RA, DEC, HA, ZD, AZ,
                                      now, ROT_ANGLE, TEMPERATURE, PRESSURE, HUMIDITY,
                                      EXP_TIME, WAVELENGTH, obs);

    psGrommit *grom = psGrommitAlloc(exp);

    if (grom->height != HEIGHT) {
        printf("ERROR: grom->height is %f, should be %f\n", grom->height, HEIGHT);
        testStatus = 1;
    }
    if (grom->temperature != TEMPERATURE) {
        printf("ERROR: grom->temperature is %f, should be %f\n", grom->temperature, TEMPERATURE);
        testStatus = 2;
    }

    if (grom->pressure != PRESSURE) {
        printf("ERROR: grom->pressure is %f, should be %f\n", grom->pressure, PRESSURE);
        testStatus = 3;
    }

    if (grom->humidity != HUMIDITY) {
        printf("ERROR: grom->humidity is %f, should be %f\n", grom->humidity, HUMIDITY);
        testStatus = 4;
    }

    if (grom->wavelength != WAVELENGTH) {
        printf("ERROR: grom->wavelength is %f, should be %f\n", grom->wavelength, WAVELENGTH);
        testStatus = 5;
    }

    if (grom->lapseRate != LAPSERATE) {
        printf("ERROR: grom->lapseRate is %f, should be %f\n", grom->lapseRate, LAPSERATE);
        testStatus = 6;
    }

    /*
        XXX: Don't know how to verify: latitude, sinLat, cosLat, abberationMag,
             refractA, refractB, longitudeOffset, siderealTime

       if (grom-> != ) {
            printf("ERROR: grom-> is %f, should be %f\n", grom->, );
            testStatus = 1;
       }
    */

    printf("grom->latitude is %f\n", grom->latitude);
    printf("grom->sinLat is %f\n", grom->sinLat);
    printf("grom->cosLat is %f\n", grom->cosLat);
    printf("grom->abberationMag is %f\n", grom->abberationMag);
    printf("grom->height is %f\n", grom->height);
    printf("grom->temperature is %f\n", grom->temperature);
    printf("grom->pressure is %f\n", grom->pressure);
    printf("grom->humidity is %f\n", grom->humidity);
    printf("grom->wavelength is %f\n", grom->wavelength);
    printf("grom->lapseRate is %f\n", grom->lapseRate);
    printf("grom->refractA is %f\n", grom->refractA);
    printf("grom->refractB is %f\n", grom->refractB);
    printf("grom->longitudeOffset is %f\n", grom->longitudeOffset);
    printf("grom->siderealTime is %f\n", grom->siderealTime);

    psFree(now);
    psFree(obs);
    psFree(exp);
    psFree(grom);
    psLibFinalize();

    return(testStatus);
}

psS32 printCell(psCell *cell)
{
    psReadout **readouts = (psReadout **) cell->readouts->data;
    psReadout *readout = readouts[0];

    printf("-------------------------------\n");
    for (psS32 i = 0; i < readout->image->numRows ; i++) {
        for (psS32 j = 0; j < readout->image->numCols ; j++) {
            printf("(%.1f) ", readout->image->data.F32[i][j]);
        }
        printf("\n");
    }
    return(0);
}


/******************************************************************************
This routine tests many Astrometry functions:
    psChipInFPA()
    psCellInFPA()
    psCoordFPAToChip()
    psCoordChipToCell()
This routine loops through all valid cell coordinates and maintains a running
set of (x,y) coordinates in the FPA plane.  It calls the first two functions
with each FPA coordinate and determines the chip/cell that that coordinate
corresponds to.  It then calls the last two functions to determine the actual
chip and cell coordinates.  The cell coordinate is verified for correctness.
 
It then calls
    psCoordCellToChip()
to determine if the chip coordinates that were generated agree with the chip
coords returned above.
 
Then, it calls
    psCellInChip()
and flags an ERROR if the cell it returns is different from that of
psCellInFPA().
 
Then, it calls
    psCoordChipToFPA
to determine if the FPA coordinates are correct.
 
 
 *****************************************************************************/
psS32 test3( void )
{
    psS32 x;
    psS32 y;
    psPlane fpaCoord;
    psFPA *myFPA = genSystem();
    psCell *myCell = NULL;
    psPlane chipCoord;
    psPlane cellCoord;
    psPlane testCoord;

    psS32 xReadout = 0;
    psS32 xFPA = 0;
    for (psS32 chip=0;chip<NUM_CHIPS;chip++) {
        for (psS32 cell=0;cell<NUM_CELLS;cell++) {
            for(x=0;x<CELL_WIDTH;x++) {
                for (y=0;y<CELL_HEIGHT;y++) {
                    fpaCoord.x = (double) xFPA;
                    fpaCoord.y = (double) y;
                    //printf("------------------ (%f, %f) ------------------\n", fpaCoord.x, fpaCoord.y);
                    psChip* tmpChip = psChipInFPA(&fpaCoord, myFPA);
                    myCell = psCellInFPA(&fpaCoord, myFPA);

                    if ((myCell == NULL) || (tmpChip == NULL)) {
                        if (myCell == NULL) {
                            printf("ERROR: myCell is NULL\n");
                        }
                        if (tmpChip == NULL) {
                            printf("ERROR: tmpChip is NULL\n");
                        }
                    } else {
                        psCoordFPAToChip(&chipCoord, &fpaCoord, tmpChip);
                        psCoordChipToCell(&cellCoord, &chipCoord, myCell);

                        if (x != (psS32) cellCoord.x) {
                            printf("ERROR: x coord was %f, should be %d\n", cellCoord.x, x);
                        }
                        if (y != (psS32) cellCoord.y) {
                            printf("ERROR: y coord was %f, should be %d\n", cellCoord.y, y);
                        }

                        psCoordCellToChip(&testCoord, &cellCoord, myCell);
                        if (testCoord.x != chipCoord.x) {
                            printf("ERROR: psCoordCellToChip() x coord was %f, should be %d\n", cellCoord.x, x);
                        }
                        if (testCoord.y != chipCoord.y) {
                            printf("ERROR: psCoordCellToChip() y coord was %f, should be %d\n", cellCoord.y, y);
                        }

                        psCell *myCell2 = psCellInChip(&chipCoord, tmpChip);
                        if (myCell2 != myCell) {
                            printf("ERROR: psCellInFPA() != psCellInChip(psChipInFPA()) (%p %p)\n", myCell2, myCell);
                            printCell(myCell2);
                            printCell(myCell);

                        }

                        psCoordChipToFPA(&testCoord, &chipCoord, tmpChip);
                        if (testCoord.x != xFPA) {
                            printf("ERROR: psCoordChipToFPA() x coord was %f, should be %d\n", cellCoord.x, x);
                        }
                        if (testCoord.y != y) {
                            printf("ERROR: psCoordChipToFPA() y coord was %f, should be %d\n", cellCoord.y, y);
                        }

                        psCoordFPAToTP(&testCoord, &fpaCoord, 0.0, 0.0, myFPA);
                        if (testCoord.x != fpaCoord.x) {
                            printf("ERROR: psCoordFPAToTP() x coord was %f, should be %d\n", cellCoord.x, x);
                        }
                        if (testCoord.y != fpaCoord.y) {
                            printf("ERROR: psCoordFPAToTP() y coord was %f, should be %d\n", cellCoord.y, y);
                        }

                        psCoordCellToFPA(&testCoord, &cellCoord, myCell);
                        if (testCoord.x != fpaCoord.x) {
                            printf("ERROR: psCoordFPAToTP() x coord was %f, should be %d\n", cellCoord.x, x);
                        }
                        if (testCoord.y != fpaCoord.y) {
                            printf("ERROR: psCoordFPAToTP() y coord was %f, should be %d\n", cellCoord.y, y);
                        }

                        psCoordTPToFPA(&testCoord, &fpaCoord, 0.0, 0.0, myFPA);
                        if (testCoord.x != fpaCoord.x) {
                            printf("ERROR: psCoordFPAToTP() x coord was %f, should be %d\n", cellCoord.x, x);
                        }
                        if (testCoord.y != fpaCoord.y) {
                            printf("ERROR: psCoordFPAToTP() y coord was %f, should be %d\n", cellCoord.y, y);
                        }
                    }
                }
                xFPA++;
                xReadout++;
            }
            // We skip the gaps between cells.
            for(x=0;x<CELL_GAP;x++) {
                xFPA++;
                xReadout=0;
            }
        }
        // We skip the gaps between chips.
        for (x=0;x<CHIP_GAP;x++) {
            xFPA++;
            xReadout=0;
        }
    }

    printf("HERE 00\n");
    psFree(myFPA);
    printf("HERE 01\n");

    return(0);
}
//This code is
