/** @file  tst_psSphereOps.c
*
*  @brief The code will perform sphere rotations and transformations.
*
*  @author d-Rob, MHPCC
*
*  @version $Revision: 1.14 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-11-30 02:17:17 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*/

#include "psTest.h"
#include "pslib_strict.h"

static psS32 testSphereRotAlloc(void);
static psS32 testSphereRotQuat(void);
static psS32 testSphereRotApply1(void);
static psS32 testSphereRotApplyCelestial(void);
static psS32 testSphereRotPrecess(void);
static psS32 testSphereOffset(void);

testDescription tests[] = {
                              {testSphereRotAlloc, 819, "psSphereRotAlloc()", 0, false},
                              {testSphereRotQuat, 820, "psSphereRotQuat()", 0, false},
                              {testSphereRotApply1, 821, "psSphereRotApply()", 0, false},
                              {testSphereRotApplyCelestial, 822, "psSphereRotApplyCel()", 0, false},
                              {testSphereRotPrecess, 823, "psSphereRotPrecess()", 0, false},
                              {testSphereOffset, 825, "testSphereOffset()", 0, false},
                              {NULL}
                          };

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

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

#define DEG_INC   30.0

#define MJD_1900  15021.0        // Modified Julian Day 1/1/1900 00:00:00
#define MJD_2000  51544.0        // Modified Julian Day 1/1/2000 00:00:00
#define MJD_2100  88069.0        // Modified Julian Day 1/1/2100 00:00:00

#define ERROR_TOL   0.0001

#define ALPHA_P 4*M_PI/3
#define DELTA_P M_PI/4
#define PHI_P M_PI/3

psS32 testSphereRotAlloc( void )
{
    // Allocate data structure
    psSphereRot* myST = psSphereRotAlloc(ALPHA_P, DELTA_P, PHI_P);

    // Verify null not returned
    if(myST == NULL) {
        psError(PS_ERR_UNKNOWN,true,"Returned NULL with valid parameters");
        return 1;
    }

    double a0 = (ALPHA_P - PHI_P)/2.0;
    double a1 = (ALPHA_P - PHI_P)/2.0;
    double a2 = (ALPHA_P + PHI_P)/2.0;
    double a3 = (ALPHA_P + PHI_P)/2.0;

    double q0 = sin(a0)*sin(DELTA_P/2);
    double q1 = cos(a1)*sin(DELTA_P/2);
    double q2 = sin(a2)*cos(DELTA_P/2);
    double q3 = cos(a3)*cos(DELTA_P/2);

    if (DBL_EPSILON < fabs(q0 - myST->q0)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q0 is %lf, should be %lf\n", myST->q0, q0);
        return 2;
    }
    if (DBL_EPSILON < fabs(q1 - myST->q1)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q1 is %f, should be %f\n", myST->q1, q1);
        return 3;
    }
    if (DBL_EPSILON < fabs(q2 - myST->q2)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q2 is %f, should be %f\n", myST->q2, q2);
        return 4;
    }
    if (DBL_EPSILON < fabs(q3 - myST->q3)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q0 is %f, should be %f\n", myST->q3, q3);
        return 5;
    }

    // Free data structure
    psFree(myST);

    return 0;
}

psS32 testSphereRotQuat(void)
{
    double a0 = (ALPHA_P - PHI_P)/2.0;
    double a1 = (ALPHA_P - PHI_P)/2.0;
    double a2 = (ALPHA_P + PHI_P)/2.0;
    double a3 = (ALPHA_P + PHI_P)/2.0;

    double q0 = sin(a0)*sin(DELTA_P/2);
    double q1 = cos(a1)*sin(DELTA_P/2);
    double q2 = sin(a2)*cos(DELTA_P/2);
    double q3 = cos(a3)*cos(DELTA_P/2);

    psSphereRot *myST = psSphereRotQuat(q0*2.0, q1*2.0, q2*2.0, q3*2.0);
    // Verify null not returned
    if(myST == NULL) {
        psError(PS_ERR_UNKNOWN,true,"Returned NULL with valid parameters");
        return 1;
    }

    if (FLT_EPSILON < fabs(q0 - myST->q0)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q0 is %lf, should be %lf\n", myST->q0, q0);
        return 2;
    }
    if (FLT_EPSILON < fabs(q1 - myST->q1)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q1 is %f, should be %f\n", myST->q1, q1);
        return 3;
    }
    if (FLT_EPSILON < fabs(q2 - myST->q2)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q2 is %f, should be %f\n", myST->q2, q2);
        return 4;
    }
    if (FLT_EPSILON < fabs(q3 - myST->q3)) {
        psError(PS_ERR_UNKNOWN,true,"myST->q0 is %f, should be %f\n", myST->q3, q3);
        return 5;
    }

    // Free data structure
    psFree(myST);

    return 0;
}


// We do a simple identity transformation on a few RA, DEC pairs.
psS32 testSphereRotApply1( void )
{

    psSphere *in = psSphereAlloc();
    psSphere *out = psSphereAlloc();
    psSphere *temp = NULL;
    psSphere *rc = NULL;
    psSphere *temp2 = psSphereAlloc();
    //        psSphereRot *myST = psSphereRotAlloc(0.0, 0.0, 0.0);
    psSphereRot *myST = psSphereRotAlloc(ALPHA_P, DELTA_P, PHI_P);
    psSphereRot *yourST =  psSphereRotInvert(ALPHA_P, DELTA_P, PHI_P);

    for (float r=0.0;r<180.0;r+=DEG_INC) {
        for (float d=0.0;d<90.0;d+=DEG_INC) {
            in->r = DEG_TO_RAD(r);
            in->d = DEG_TO_RAD(d);
            in->rErr = 0.0;
            in->dErr = 0.0;

            temp2 = psSphereRotApply(temp2, myST, in);
            out = psSphereRotApply(out, yourST, temp2);

            if (ERROR_TOL < fabs(out->r - in->r)) {
                psError(PS_ERR_UNKNOWN,true,"out->r is %f, should be %f\n", out->r, in->r);
                return 2;
            }
            if (ERROR_TOL < fabs(out->d - in->d)) {
                psError(PS_ERR_UNKNOWN,true,"out->d is %f, should be %f\n", out->d, in->d);
                return 3;
            }
        }
    }
    // Verify new sphere object is created if out parameter NULL
    temp = psSphereRotApply(NULL, myST, in);
    if ( temp == NULL) {
        psError(PS_ERR_UNKNOWN,true,"Returned NULL when out parameter was null");
        return 4;
    }
    psFree(temp);

    // Verify NULL returned if transform structure null
    psLogMsg(__func__,PS_LOG_INFO,"Following should generate an error");
    rc = psSphereRotApply(NULL, NULL, in);
    if (rc != NULL) {
        psError(PS_ERR_UNKNOWN,true,"psSphereRotApply() did not return NULL.");
        return 5;
    }

    // Verify NULL returned when input sphere is NULL
    psLogMsg(__func__,PS_LOG_INFO,"Following should generate an error");
    rc = psSphereRotApply(NULL, myST, NULL);
    if (rc != NULL) {
        psError(PS_ERR_UNKNOWN,true,"psSphereRotApply() did not return NULL");
        return 6;
    }

    psFree(myST);
    psFree(yourST);
    psFree(temp2);
    psFree(out);
    psFree(in);

    return 0;
}

#define ERROR_PERCENT 0.01

psS32 testSphereRotApplyCelestial( void)
{
    int numTestPoints = 3;
    // ICRS coordinates
    double alpha[] = {  0.0,        0.0,     180.0     };
    double delta[] = {  0.0,       90.0,      30.0     };
    //Ecliptic coordinates
    double lambda[] ={  0.0,       90.0,     167.072470};
    double beta[] =  {  0.0,       66.560719, 27.308813};
    // Galactic coordinates
    double l[] =     { 96.337272, 122.93192, 195.639488};
    double b[] =     {-60.188553,  27.12825,  78.353806};
    double t[] =     {  MJD_2000,  MJD_2000,   MJD_2100};
    double TOLERANCE = 0.001;

    for (int x = 0; x < numTestPoints; x++) {

        psTime* time = psTimeFromMJD(t[x]);
        psSphereRot* toEcliptic = psSphereRotICRSToEcliptic(time);
        psSphereRot* fromEcliptic = psSphereRotEclipticToICRS(time);
        psSphereRot* toGalactic = psSphereRotICRSToGalactic();
        psSphereRot* fromGalactic = psSphereRotGalacticToICRS();
        psFree(time);

        // set the ICRS coordinate
        psSphere* icrs = psSphereAlloc();
        icrs->r = DEG_TO_RAD(alpha[x]);
        icrs->d = DEG_TO_RAD(delta[x]);

        // apply/unapply Ecliptic
        psSphere* ecliptic = psSphereRotApply(NULL, toEcliptic, icrs);
        psSphere* icrsFromEcliptic = psSphereRotApply(NULL, fromEcliptic, ecliptic);

        // check ecliptic transforms for correctness
        if (fabs(RAD_TO_DEG(ecliptic->r) - lambda[x]) > TOLERANCE ||
                fabs(RAD_TO_DEG(ecliptic->d) - beta[x]) > TOLERANCE) {
            psError(PS_ERR_UNKNOWN, false,
                    "Ecliptic tranformation incorrect.  Result is (%g,%g), expected (%g,%g)",
                    RAD_TO_DEG(ecliptic->r),RAD_TO_DEG(ecliptic->d),
                    lambda[x], beta[x]);
            return 1;
        }
        if ( (fabs(RAD_TO_DEG(icrsFromEcliptic->r) - alpha[x]) > TOLERANCE &&
                fabs(RAD_TO_DEG(icrsFromEcliptic->d) - 90.0) > TOLERANCE ) ||
                fabs(RAD_TO_DEG(icrsFromEcliptic->d) - delta[x]) > TOLERANCE) {
            psError(PS_ERR_UNKNOWN, false,
                    "ICRS for Ecliptic tranformation incorrect.  Result is (%g,%g), expected (%g,%g)",
                    RAD_TO_DEG(icrsFromEcliptic->r),RAD_TO_DEG(icrsFromEcliptic->d),
                    alpha[x], delta[x]);
            return 2;
        }
        psFree(ecliptic);
        psFree(icrsFromEcliptic);

        // apply/unapply Galactic
        //        if (x == 1){
        //             psSphereRot *temp = psSphereRotICRSToGalactic();
        //             *fromGalactic = *temp;
        //             fromGalactic->q3 *= -1.0;
        //             psFree(temp);
        //        }
        psSphere* galactic = psSphereRotApply(NULL, toGalactic, icrs);
        psSphere* icrsFromGalactic = psSphereRotApply(NULL, fromGalactic, galactic);

        // check ecliptic transforms for correctness
        if (fabs(RAD_TO_DEG(galactic->r) - l[x]) > TOLERANCE ||
                fabs(RAD_TO_DEG(galactic->d) - b[x]) > TOLERANCE) {
            psError(PS_ERR_UNKNOWN, false,
                    "Galactic tranformation incorrect.  Result is (%g,%g), expected (%g,%g)",
                    RAD_TO_DEG(galactic->r),RAD_TO_DEG(galactic->d),
                    l[x], b[x]);
            return 3;
        }
        if ( (fabs(RAD_TO_DEG(icrsFromGalactic->r) - alpha[x]) > TOLERANCE &&
                fabs(RAD_TO_DEG(icrsFromGalactic->d) - 90.0) > TOLERANCE ) ||
                fabs(RAD_TO_DEG(icrsFromGalactic->d) - delta[x]) > TOLERANCE) {
            psError(PS_ERR_UNKNOWN, false,
                    "ICRS for Galactic tranformation incorrect.  Result is (%g,%g), expected (%g,%g)",
                    RAD_TO_DEG(icrsFromGalactic->r),RAD_TO_DEG(icrsFromGalactic->d),
                    alpha[x], delta[x]);
            return 4;
        }
        psFree(galactic);
        psFree(icrsFromGalactic);
        psFree(icrs);
        psFree(toEcliptic);
        psFree(fromEcliptic);
        psFree(toGalactic);
        psFree(fromGalactic);
    }
    return 0;
}

#define SPHERE_PRECESS_TP1_R            0.0               //    0.0       degrees
#define SPHERE_PRECESS_TP1_D            0.0               //    0.0       degrees
#define SPHERE_PRECESS_TP1_EXPECT_R     6.238453          //  357.437     degrees
#define SPHERE_PRECESS_TP1_EXPECT_D    -0.019426          //   -1.113     degrees
#define SPHERE_PRECESS_TP2_R            0.0               //    0.0       degrees
#define SPHERE_PRECESS_TP2_D            1.570796          //   90.0       degrees
#define SPHERE_PRECESS_TP2_EXPECT_R     6.260828          //  358.719     degrees
#define SPHERE_PRECESS_TP2_EXPECT_D     1.551353          //   88.886     degrees
#define SPHERE_PRECESS_TP3_R            3.141593          //  180.0       degrees
#define SPHERE_PRECESS_TP3_D            0.523599          //   30.0       degrees
#define SPHERE_PRECESS_TP3_EXPECT_R     3.096616          //  177.423     degrees
#define SPHERE_PRECESS_TP3_EXPECT_D     0.543024          //   31.113     degrees

psS32 testSphereRotPrecess( void )
{

    psSphere*     inputCoord  = psSphereAlloc();
    psSphere*     outputCoord = NULL;
    psTime*       fromTime    = psTimeFromMJD(MJD_2100);
    psTime*       toTime      = psTimeFromMJD(MJD_1900);

    // Set input coordinate
    inputCoord->r = SPHERE_PRECESS_TP1_R;
    inputCoord->d = SPHERE_PRECESS_TP1_D;
    inputCoord->rErr = 0.0;
    inputCoord->dErr = 0.0;

    // Calculate precess
    outputCoord = psSpherePrecess(inputCoord, fromTime, toTime);
    // Verify return is not NULL
    if(outputCoord == NULL) {
        psError(PS_ERR_UNKNOWN,true,"Returned NULL not expected");
        return 1;
    }
    // Verify return with expected values
    if( fabs(outputCoord->r - SPHERE_PRECESS_TP1_EXPECT_R) > ERROR_TOL) {
        psError(PS_ERR_UNKNOWN,true,"Precess r = %lg not equal to expected = %lg",
                outputCoord->r,SPHERE_PRECESS_TP1_EXPECT_R);
        return 2;
    }
    if( fabs(outputCoord->d - SPHERE_PRECESS_TP1_EXPECT_D) > ERROR_TOL) {
        psError(PS_ERR_UNKNOWN,true,"Precess d = %lg not equal to expected = %lg",
                outputCoord->d,SPHERE_PRECESS_TP1_EXPECT_D);
        return 3;
    }
    psFree(outputCoord);

    // Set input coordinate
    inputCoord->r = SPHERE_PRECESS_TP2_R;
    inputCoord->d = SPHERE_PRECESS_TP2_D;
    inputCoord->rErr = 0.0;
    inputCoord->dErr = 0.0;

    // Calculate precess
    outputCoord = psSpherePrecess(inputCoord, fromTime, toTime);
    // Verify return is not NULL
    if(outputCoord == NULL) {
        psError(PS_ERR_UNKNOWN,true,"Returned NULL not expected");
        return 4;
    }
    // Verify return with expected values
    if( fabs(outputCoord->r - SPHERE_PRECESS_TP2_EXPECT_R) > ERROR_TOL) {
        psError(PS_ERR_UNKNOWN,true,"Precess r = %lg not equal to expected = %lg",
                outputCoord->r,SPHERE_PRECESS_TP2_EXPECT_R);
        return 5;
    }
    if( fabs(outputCoord->d - SPHERE_PRECESS_TP2_EXPECT_D) > ERROR_TOL) {
        psError(PS_ERR_UNKNOWN,true,"Precess d = %lg not equal to expected = %lg",
                outputCoord->d,SPHERE_PRECESS_TP2_EXPECT_D);
        return 6;
    }
    psFree(outputCoord);

    // Set input coordinate
    inputCoord->r = SPHERE_PRECESS_TP3_R;
    inputCoord->d = SPHERE_PRECESS_TP3_D;
    inputCoord->rErr = 0.0;
    inputCoord->dErr = 0.0;

    // Calculate precess
    outputCoord = psSpherePrecess(inputCoord, fromTime, toTime);
    // Verify return is not NULL
    if(outputCoord == NULL) {
        psError(PS_ERR_UNKNOWN,true,"Returned NULL not expected");
        return 7;
    }
    // Verify return with expected values
    if( fabs(outputCoord->r - SPHERE_PRECESS_TP3_EXPECT_R) > ERROR_TOL) {
        psError(PS_ERR_UNKNOWN,true,"Precess r = %lg not equal to expected = %lg",
                outputCoord->r,SPHERE_PRECESS_TP3_EXPECT_R);
        return 8;
    }
    if( fabs(outputCoord->d - SPHERE_PRECESS_TP3_EXPECT_D) > ERROR_TOL) {
        psError(PS_ERR_UNKNOWN,true,"Precess d = %lg not equal to expected = %lg",
                outputCoord->d,SPHERE_PRECESS_TP3_EXPECT_D);
        return 9;
    }
    psFree(outputCoord);

    // Invoke precess with invalid parameter
    psLogMsg(__func__,PS_LOG_INFO,"Following should generate an error message");
    outputCoord = psSpherePrecess(inputCoord, fromTime, NULL);
    if(outputCoord != NULL) {
        psError(PS_ERR_UNKNOWN,true,"Did not return NULL with invalid input");
        return 10;
    }

    // Invoke precess with invalid parameter
    psLogMsg(__func__,PS_LOG_INFO,"Following should generate an error message");
    outputCoord = psSpherePrecess(inputCoord, NULL, toTime);
    if(outputCoord != NULL) {
        psError(PS_ERR_UNKNOWN,true,"Did not return NULL with invalid input");
        return 11;
    }

    // Invoke precess with invalid parameter
    psLogMsg(__func__,PS_LOG_INFO,"Following should generate an error message");
    outputCoord = psSpherePrecess(NULL, fromTime, toTime);
    if(outputCoord != NULL) {
        psError(PS_ERR_UNKNOWN,true,"Did not return NULL with invalid input");
        return 12;
    }

    // Free objects
    psFree(fromTime);
    psFree(toTime);
    psFree(inputCoord);

    return 0;
}

psS32 testSphereOffset(void)
{
    psSphere *origin = psSphereAlloc();
    psSphere *offset = psSphereAlloc();
    psSphere *dest = psSphereAlloc();
    psSphere *empty = NULL;
    psSphere *output = NULL;

    //Test Set for NULL position
    output = psSphereSetOffset(empty, offset, PS_SPHERICAL, PS_DEGREE);
    if (output != NULL) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereSetOffset Failed to return NULL for NULL input.\n");
        return 1;
    }
    //Test Set for NULL offset
    output = psSphereSetOffset(offset, empty, PS_SPHERICAL, PS_DEGREE);
    if (output != NULL) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereSetOffset Failed to return NULL for NULL input.\n");
        return 2;
    }
    //Test Get for NULL position1
    output = psSphereGetOffset(empty, origin, PS_LINEAR, PS_RADIAN);
    if (output != NULL) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereGetOffset Failed to return NULL for NULL input.\n");
        return 3;
    }
    //Test Get for NULL position2
    output = psSphereGetOffset(origin, empty, PS_LINEAR, PS_RADIAN);
    if (output != NULL) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereGetOffset Failed to return NULL for NULL input.\n");
        return 4;
    }

    //Test Set using Spherical mode, Degree units
    origin->r = 0.0;
    origin->d = 0.0;
    offset->r = 45.0;
    offset->d = 30.0;
    output = psSphereSetOffset(origin, offset, PS_SPHERICAL, PS_DEGREE);
    if ( fabs(output->r - M_PI/4.0) > 0.0001 ||
            fabs(output->d - M_PI/6.0) > 0.0001 ) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereSetOffset failed to return correct spherical offset values.\n");
        return 5;
    }
    psFree(output);

    //Test Set and Get using Linear mode, Radian units
    origin->r = 0.0;
    origin->d = 0.0;
    offset->r = 1.0;
    offset->d = 1.0;
    output = psSphereSetOffset(origin, offset, PS_LINEAR, PS_RADIAN);
    /*    if ( fabs(output->r - 1.0) > 0.0001 || fabs(output->d - 1.0) > 0.0001 ) {
            psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereSetOffset failed to return correct linear offset values.\n");
        printf("\n SphereSetOffset should be %lf, %lf and is %lf, %lf\n", offset->r, offset->d,
            output->r, output->d);
            return 6;
        }
    */
    //    output->r *= -1.0;
    //    output->d *= -1.0;
    empty = psSphereGetOffset(origin, output, PS_LINEAR, PS_RADIAN);
    if ( fabs(offset->r - empty->r) > 0.0001 || fabs(offset->d - empty->d) > 0.0001 ) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereGetOffset failed to return correct linear offset values.\n");
        return 7;
    }
    psFree(output);
    psFree(empty);

    //Test Set using Linear mode, Arcmin units
    origin->r = 0.0;
    origin->d = 0.0;
    offset->r = RAD_TO_MIN(M_PI / 4.0);     //45 deg
    offset->d = RAD_TO_MIN(M_PI / 6.0);     //30 deg
    output = psSphereSetOffset(origin, offset, PS_SPHERICAL, PS_ARCMIN);
    //Test Get using Spherical mode, Arcsec units
    empty = psSphereGetOffset(origin, output, PS_SPHERICAL, PS_ARCSEC);
    empty->r = SEC_TO_RAD(empty->r);
    empty->d = SEC_TO_RAD(empty->d);
    if (fabs(empty->r - (M_PI / 4.0)) > 0.001 || fabs(empty->d - (M_PI / 6.0)) > 0.001) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, false,
                "psSphereGetOffset failed to return correct offset unit values.\n");
        printf("\n SphereGetOffset should be %lf, %lf and is %lf, %lf\n", output->r, output->d,
               empty->r, empty->d);
        return 8;
    }
    psFree(output);

    psFree(origin);
    psFree(dest);
    psFree(offset);
    psFree(empty);

    return 0;
}

