/** @file  psEarthOrientation.c
 *
 *  @brief Function implementations for earth orientation calculations
 *
 *  @ingroup EarthOrientation
 *
 *  @author Dave Robbins, MHPCC
 *  @author Robert Daniel DeSonia, MHPCC
 *
 *  @version $Revision: 1.21 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-12-02 04:40:00 $
 *
 *  Copyright 2005 Maui High Performance Computing Center, University of Hawaii
 */

#include <math.h>
#include <string.h>

#include "psEarthOrientation.h"
//#include "psTime.h"
#include "psArray.h"
#include "psPolynomial.h"
#include "psVector.h"
#include "psMetadata.h"
#include "psMetadataConfig.h"
#include "psError.h"
#include "psErrorText.h"
#include "psMemory.h"
#include "psCoord.h"
#include "psConstants.h"

// Sun's Mass (src: Google's Calculator Service)
#define PS_M   1.98892e30 /* kilograms */
// Newton's Gravitational Constant (src:NIST)
#define PS_G   6.6742e-11 /* m^3/kg/s^2 */
// Speed of light in vacuum (src:NIST)
#define PS_C0  299792458.0 /* m/s */
// Average distance from earth to sun
#define PS_AU 149597890000.0 /* meters */
// Modified Julian Day 1/1/2000 00:00:00
#define MJD_2000  51544.0
// Days in Julian century
#define JULIAN_CENTURY 36525.0

static psArray* xTable = NULL;
static psArray* yTable = NULL;
static psArray* sTable = NULL;
static psPolynomial1D* xPoly = NULL;
static psPolynomial1D* yPoly = NULL;
static psPolynomial1D* sPoly = NULL;
static bool eocInitialized = false;

static psSphereRot *rotMatrix_To_Quat(double A[3][3]);

static void earthPoleFree(psEarthPole *pole)
{
    // There are non dynamic allocated items
}

psEarthPole *psEarthPoleAlloc(void)
{
    psEarthPole* pole = psAlloc(sizeof(psEarthPole));
    psMemSetDeallocator(pole, (psFreeFunc) earthPoleFree);
    pole->x = 0.0;
    pole->y = 0.0;
    pole->s = 0.0;
    return pole;
}

bool p_psEOCInit()
{
    unsigned int nFail = 0;

    // Read config file
    psMetadata* eocMetadata = psMetadataConfigParse(NULL,
                              &nFail,
                              p_psGetConfigFileName(),
                              true);

    if(eocMetadata == NULL) {
        return false;
    } else if(nFail != 0) {
        return false;
    }

    bool success = false;
    // Get table format
    char* tableFormat = psMetadataLookupStr(&success, eocMetadata, "psLib.eoc.precession.table.format");
    if(! success || tableFormat == NULL) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.table.format");
        return false;
    }

    char* xTableName = psMetadataLookupStr(&success, eocMetadata, "psLib.eoc.precession.table.file.x");
    if(! success || xTableName == NULL) {
        psFree(tableFormat);
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.x.file");
        return false;
    }

    char* yTableName = psMetadataLookupStr(&success, eocMetadata, "psLib.eoc.precession.table.file.y");
    if(! success || yTableName == NULL) {
        psFree(tableFormat);
        psFree(xTableName);
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.y.file");
        return false;
    }

    char* sTableName = psMetadataLookupStr(&success, eocMetadata, "psLib.eoc.precession.table.file.s");
    if(! success || sTableName == NULL) {
        psFree(tableFormat);
        psFree(xTableName);
        psFree(yTableName);
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.s.file");
        return false;
    }
    xTable = psVectorsReadFromFile(xTableName, tableFormat);
    yTable = psVectorsReadFromFile(yTableName, tableFormat);
    sTable = psVectorsReadFromFile(sTableName, tableFormat);
    psFree(tableFormat);
    psFree(xTableName);
    psFree(yTableName);
    psFree(sTableName);

    if(xTable == NULL || yTable == NULL || sTable == NULL) {
        // XXX: need to move the error message to the error messages file.
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Failed to read the precession-nutation model tables.");
        return false;
    }

    psVector* xCoeff = psMetadataLookupPtr(&success, eocMetadata, "psLib.eoc.precession.poly.x");
    if(! success || xCoeff == NULL || xCoeff->type.type != PS_TYPE_F64) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.poly.x");
        return false;
    }
    psVector* yCoeff = psMetadataLookupPtr(&success, eocMetadata, "psLib.eoc.precession.poly.y");
    if(! success || yCoeff == NULL || yCoeff->type.type != PS_TYPE_F64) {
        psFree(xCoeff);
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.poly.y");
        return false;
    }
    psVector* sCoeff = psMetadataLookupPtr(&success, eocMetadata, "psLib.eoc.precession.poly.s");
    if(! success || sCoeff == NULL || sCoeff->type.type != PS_TYPE_F64) {
        psFree(xCoeff);
        psFree(yCoeff);
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.poly.s");
        return false;
    }
    xPoly = psPolynomial1DAlloc(xCoeff->n, PS_POLYNOMIAL_ORD);
    memcpy(xPoly->coeff, xCoeff->data.F64,PSELEMTYPE_SIZEOF(PS_TYPE_F64)*xCoeff->n);
    yPoly = psPolynomial1DAlloc(yCoeff->n, PS_POLYNOMIAL_ORD);
    memcpy(yPoly->coeff, yCoeff->data.F64,PSELEMTYPE_SIZEOF(PS_TYPE_F64)*yCoeff->n);
    sPoly = psPolynomial1DAlloc(sCoeff->n, PS_POLYNOMIAL_ORD);
    memcpy(sPoly->coeff, sCoeff->data.F64,PSELEMTYPE_SIZEOF(PS_TYPE_F64)*sCoeff->n);

    psFree(xCoeff);
    psFree(yCoeff);
    psFree(sCoeff);
    psFree(eocMetadata);

    return true;
}

bool p_psEOCFinalize(void)
{
    psFree(xTable);
    psFree(yTable);
    psFree(sTable);    // There are non dynamic allocated items


    xTable = NULL;
    yTable = NULL;
    sTable = NULL;

    psFree(xPoly);
    psFree(yPoly);
    psFree(sPoly);

    xPoly = NULL;
    yPoly = NULL;
    sPoly = NULL;

    eocInitialized = false;

    return true;
}

psSphere *psAberration(psSphere *apparent,
                       const psSphere *actual,
                       const psSphere *direction,
                       double speed)
{
    PS_ASSERT_PTR_NON_NULL(actual, NULL);
    PS_ASSERT_PTR_NON_NULL(direction, NULL);
    if (fabs(speed) < DBL_EPSILON) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Aberration speed should not be equal to 0.\n");
        return NULL;
    }

    if (apparent == NULL) {
        apparent = psSphereAlloc();
    } else {
        apparent->r = 0.0;
        apparent->d = 0.0;
        apparent->rErr = 0.0;
        apparent->dErr = 0.0;
    }
    //    psSphere *rp = psSphereAlloc();
    psCube *rp = psCubeAlloc();
    //    psSphere *r_p = psSphereAlloc();
    psCube *r_p = psCubeAlloc();
    double mu = 0.0;
    double mu_p = 0.0;
    double a = 0.0;

    //mu = apparent * direction;  I believe this should be:
    //    mu = apparent->r * direction->r + apparent->d * direction->d;
    psCube* directionVector = psSphereToCube(direction);
    if (directionVector == NULL)
        printf("directionVector is null\n");
    psCube* actualVector = psSphereToCube(actual);
    if (directionVector == NULL)
        printf("actualVector is null\n");
    //    mu = acos(directionVector->x*actualVector->x +
    mu = (directionVector->x*actualVector->x +
          directionVector->y*actualVector->y +
          directionVector->z*actualVector->z);

    //    rp = apparent - mu * direction;
    //    rp->r = actual->r - mu * direction->r;
    //    rp->d = actual->d - mu * direction->d;
    rp->x = actualVector->x - mu * directionVector->x;
    rp->y = actualVector->y - mu * directionVector->y;
    rp->z = actualVector->z - mu * directionVector->z;

    mu_p = mu + speed * ((mu * mu - 1.0) / (1.0 - speed * mu));

    //Not sure if this is right.  ADD gets a scalar from division of rp (a vector)
    //Paul claims rp is the modulus or square of the vector components
    /*    psCube* rpVector = psSphereToCube(rp);
        a = sqrt( (1.0 - mu_p * mu_p) / ( rpVector->x*rpVector->x +
                rpVector->y*rpVector->y + rpVector->z*rpVector->z)  );
    */
    a = (1.0 - mu_p * mu_p) / (rp->x * rp->x + rp->y * rp->y + rp->z * rp->z);
    if (a < 0.0) {
        printf("a is negative\n");
        psFree(rp);
        psFree(r_p);
        psFree(directionVector);
        psFree(actualVector);
        psFree(apparent)
        return NULL;
    } else
        a = sqrt(a);
    //r_p = mu_p * direction + a * rp;
    //    r_p->r = mu_p * direction->r + a * rp->r;
    //    r_p->d = mu_p * direction->d + a * rp->d;
    r_p->x = mu_p * directionVector->x + a * rp->x;
    r_p->y = mu_p * directionVector->y + a * rp->y;
    r_p->z = mu_p * directionVector->z + a * rp->z;

    psSphere *r_pSphere = psCubeToSphere(r_p);
    if (r_pSphere == NULL)
        printf("r_pSphere is null\n");

    *apparent = *r_pSphere;
    /*
        psSphereRot *rot = NULL;
        double cosR = cos(direction->r);
        double cosD = cos(direction->d);
        double sinR = sin(direction->r);
        double sinD = sin(direction->d);
        rot = psSphereRotQuat(cosR*cosD, sinR*cosD, sinD, speed);

        actual = psSphereRotApply(actual, rot, apparent);
    */
    psFree(rp);
    psFree(r_p);
    psFree(r_pSphere)
    psFree(directionVector);
    psFree(actualVector);
    //    psFree(rpVector);
    return apparent;
}

psSphere *psGravityDeflection(psSphere *apparent,
                              psSphere *actual,
                              psSphere *sun)
{
    PS_ASSERT_PTR_NON_NULL(actual, NULL);
    PS_ASSERT_PTR_NON_NULL(sun, NULL);

    // calculating the apparent angle from the actual angle and the sun position

    psCube* sunVector = psSphereToCube(sun);
    psCube* actualVector = psSphereToCube(actual);

    // use dot product to calculate the angle of separation
    // N.B., assuming the psSphereToCube function returns a unit vector.
    //    double theta = acos(sunVector->x*actualVector->x +
    double theta = (sunVector->x*actualVector->x +
                    sunVector->y*actualVector->y +
                    sunVector->z*actualVector->z);
    double r0 = PS_AU * tan(theta);
    double deflection = 4.0*PS_G*PS_M/(PS_C0*PS_C0*r0);

    // make sure the deflection is not greater than 1.75 arcsec
    double limit = SEC_TO_RAD(1.75);
    if (deflection > limit) {
        //       deflection = limit;
        //if deflection is greater than limit, the light rays will hit the sun
        psWarning("Invalid positions.  Light ray will not be seen on earth.\n");
        psFree(actualVector);
        psFree(sunVector);
        return apparent;
    }

    if (apparent == NULL) {
        apparent = psSphereAlloc();
    } else {
        apparent->r = 0.0;
        apparent->d = 0.0;
        apparent->rErr = 0.0;
        apparent->dErr = 0.0;
    }

    // bend the actual vector away from the sun vector by deflection angle.
    // XXX: Not sure how to do this.  Dave thinks the formula should be:
    //      theta = atan(r0/d)*tan(deflection), phi = thete/tan(deflection)
    theta = 0.0;
    double phi = 0.0;
    deflection = SEC_TO_RAD(deflection);
    theta = atan(r0/PS_AU) * tan(deflection);
    //    phi = sqrt( deflection*deflection - theta*theta );
    phi = deflection * cos(asin(theta/deflection));
    apparent->r = theta;
    apparent->d = phi;
    psFree(actualVector);
    psFree(sunVector);
    return apparent;
}

psEarthPole *psEOC_PrecessionModel(const psTime *time)
{
    // Check for null parameter
    PS_ASSERT_PTR_NON_NULL(time, NULL);
    if (time->type == PS_TIME_UT1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Invalid time input.  Time cannot be of type UT1.\n");
        return NULL;
    }

    // Convert psTime to MJD
    double MJD = psTimeToMJD(time);

    // Calculate number of Julian centuries since 2000
    double t = ( MJD - MJD_2000 ) / JULIAN_CENTURY;
    double t2 = t*t;
    double t3 = t*t*t;
    double t4 = t*t*t*t;

    // N.B., the following formulae are from the ADD
    // Mean Anomaly of the Moon
    double F[14];
    F[0] = DEG_TO_RAD(134.96340251) +
           SEC_TO_RAD(1717915923.2178)*t +
           SEC_TO_RAD(31.8792)*t2 +
           SEC_TO_RAD(0.051635)*t3 -
           SEC_TO_RAD(0.00024470)*t4;

    // Mean Anomaly of the Sun
    F[1] = DEG_TO_RAD(357.52910918) +
           SEC_TO_RAD(129596581.0481)*t -
           SEC_TO_RAD(0.5532)*t2 +
           SEC_TO_RAD(0.000136)*t3 -
           SEC_TO_RAD(0.00001149)*t4;

    // L - Omega
    F[2] = DEG_TO_RAD(93.27209062) +
           SEC_TO_RAD(1739527262.8478)*t -
           SEC_TO_RAD(12.7512)*t2 -
           SEC_TO_RAD(0.001037)*t3 +
           SEC_TO_RAD(0.00000417)*t4;

    // Mean Elongation of the Moon from the Sun
    F[3] = DEG_TO_RAD(297.85019547) +
           SEC_TO_RAD(1602961601.2090)*t -
           SEC_TO_RAD(6.3706)*t2 +
           SEC_TO_RAD(0.006593)*t3 -
           SEC_TO_RAD(0.00003169)*t4;

    // Mean Longitude of the Ascending Node of the Moon
    F[4] = DEG_TO_RAD(125.04455501) -
           SEC_TO_RAD(6962890.5431)*t +
           SEC_TO_RAD(7.4722)*t2 +
           SEC_TO_RAD(0.007702)*t3 -
           SEC_TO_RAD(0.0000593)*t4;

    F[5] = 4.402608842 + 2608.7903141574*t;
    F[6] = 3.176146697 + 1021.3285546211*t;
    F[7] = 1.753470314 + 628.3075849991*t;
    F[8] = 6.203480913 + 334.0612426700*t;
    F[9] = 0.599546497 + 52.9690962641*t;
    F[10] = 0.874016757 + 21.3299104960*t;
    F[11] = 5.481293872 + 7.4781598567*t;
    F[12] = 5.311886287 + 3.8133035638*t;
    F[13] = 0.024381750*t + 0.00000538691*t2;

    // Check if EOC data loaded
    if(! eocInitialized) {
        eocInitialized = p_psEOCInit();
        if(!eocInitialized) {
            // XXX: Move error message.
            psError(PS_ERR_UNKNOWN, false,
                    "Could not initialize EOC tables -- check data files.");
            return NULL;
        }
    }

    // calculate the polynomial portion first
    double X = psPolynomial1DEval(xPoly,t);
    double Y = psPolynomial1DEval(yPoly,t);
    double S = psPolynomial1DEval(sPoly,t);

    // now calculate the non-poly portion from the tables

    psF64* cols[17];
    for (int lcv = 0; lcv < 17; lcv++) {
        cols[lcv] = ((psVector*)(xTable->data[lcv]))->data.F64;
    }
    int numRows = ((psVector*)(xTable->data[0]))->n;
    for (int lcv = 0; lcv < numRows; lcv++) {
        // arg = sum w_(i,j,k)*F_k
        double arg = 0.0;
        for (int k = 0; k < 14; k++) {
            arg += cols[k+3][lcv]*F[k];
        }
        double tj = pow(t,cols[0][lcv]);
        double as = cols[1][lcv];
        double ac = cols[2][lcv];

        X += as*tj*sin(arg) + ac*tj*cos(arg);
    }

    for (int lcv = 0; lcv < 17; lcv++) {
        cols[lcv] = ((psVector*)(yTable->data[lcv]))->data.F64;
    }
    numRows = ((psVector*)(yTable->data[0]))->n;
    for (int lcv = 0; lcv < numRows; lcv++) {
        // arg = sum w_(i,j,k)*F_k
        double arg = 0.0;
        for (int k = 0; k < 14; k++) {
            arg += cols[k+3][lcv]*F[k];
        }
        double tj = pow(t,cols[0][lcv]);
        double as = cols[1][lcv];
        double ac = cols[2][lcv];

        Y += as*tj*sin(arg) + ac*tj*cos(arg);
    }

    for (int lcv = 0; lcv < 17; lcv++) {
        cols[lcv] = ((psVector*)(sTable->data[lcv]))->data.F64;
    }
    numRows = ((psVector*)(sTable->data[0]))->n;
    for (int lcv = 0; lcv < numRows; lcv++) {
        // arg = sum w_(i,j,k)*F_k
        double arg = 0.0;
        for (int k = 0; k < 14; k++) {
            arg += cols[k+3][lcv]*F[k];
        }
        double tj = pow(t,cols[0][lcv]);
        double as = cols[1][lcv];
        double ac = cols[2][lcv];

        S += as*tj*sin(arg) + ac*tj*cos(arg);
    }

    // now, the tables for S actually gives S + XY/2, so let's get the real S now
    S -= X*Y/2.0;

    //    psEarthPole* pole = psAlloc(sizeof(psEarthPole));
    psEarthPole* pole = psEarthPoleAlloc();
    pole->x = X;
    pole->y = Y;
    pole->s = S;

    return pole;
}

psEarthPole *psEOC_PrecessionCorr(const psTime *time,
                                  psTimeBulletin bulletin)
{
    PS_ASSERT_PTR_NON_NULL(time,NULL);
    PS_ASSERT_INT_WITHIN_RANGE(bulletin, PS_IERS_A, PS_IERS_B, NULL);

    int tableColumn;
    double mjd;
    psLookupStatusType status;
    char* tableName[1] = {"dailyTable"};
    double x, y, s;
    //    psMetadataItem *tableMetadataItem = NULL;
    //   psVector *temp = NULL;
    //    double calc = 0.0;
    //    double t = 0.0;
    //    psMetadata *timeMetadata = p_psTimeGetMetadata();

    mjd = psTimeToMJD(time);

    if (bulletin == PS_IERS_A) {
        tableColumn = 1;
    } else {
        tableColumn = 4;
    }

    x = p_psTimeSearchTables(mjd, tableColumn, tableName, 1, &status);
    if ( status == PS_LOOKUP_ERROR ) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Time table search returned unsuccessful status.\n");
        return NULL;
    } else if (status != PS_LOOKUP_SUCCESS) {
        psSphere *sphere = p_psTimeGetPoleCoords(time);
        x = sphere->r;
        psFree(sphere);
    } else {
        x = SEC_TO_RAD(x);
    }


    /*    }else if(status == PS_LOOKUP_PAST_TOP) {
            // Date too early for tables. Get default time delta value from metadata, and issue warning.
            psLogMsg(__func__,PS_LOG_WARN,PS_ERRORTEXT_psTime_TIME_PREDATES_TABLES,mjd,"xp");

            // Lookup value from time metadata loaded from pslib.config
            tableMetadataItem = psMetadataLookup(timeMetadata, "psLib.time.before.xp");
            if(tableMetadataItem == NULL) {
                psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                        "psLib.time.before.xp");
                return NULL;
            }
            x = tableMetadataItem->data.F64;

        } else if(status == PS_LOOKUP_PAST_BOTTOM) {
            tableMetadataItem = psMetadataLookup(timeMetadata, "psLib.time.predict.mjd");
            if(tableMetadataItem == NULL) {
                psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                        "psLib.time.predict.mjd");
                return NULL;
            }
            mjdPred = tableMetadataItem->data.F64;

            // Get xp
            tableMetadataItem = psMetadataLookup(timeMetadata, "psLib.time.predict.xp");
            if(tableMetadataItem == NULL) {
                psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED, "psLib.time.predict.xp");
                return NULL;
            }
            xp = (psVector*)tableMetadataItem->data.V;
            PS_ASSERT_PTR_NON_NULL(xp,NULL);

            // Get yp
            tableMetadataItem = psMetadataLookup(timeMetadata, "psLib.time.predict.yp");
            if(tableMetadataItem == NULL) {
                psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED, "psLib.time.predict.yp");
                return NULL;
            }
            yp = (psVector*)tableMetadataItem->data.V;
            PS_ASSERT_PTR_NON_NULL(yp,NULL);

            // Calculate "a" and "c" constants
            a = TWOPI*(mjd - mjdPred)/365.25;
            c = TWOPI*(mjd - mjdPred)/435.0;

            // Calculate x and y polar coordinates
            x = xp->data.F64[0] +
                xp->data.F64[1]*cos(a) +
                xp->data.F64[2]*sin(a) +
                xp->data.F64[3]*cos(c) +
                xp->data.F64[4]*sin(c);





        }
    */


    tableColumn++;
    y = p_psTimeSearchTables(mjd, tableColumn, tableName, 1, &status);
    if ( status == PS_LOOKUP_ERROR ) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Time table search returned unsuccessful status.\n");
        return NULL;
    } else if (status != PS_LOOKUP_SUCCESS) {
        psSphere *sphere = p_psTimeGetPoleCoords(time);
        y = sphere->d;
        psFree(sphere);
    } else {
        y = SEC_TO_RAD(y);
    }

    tableColumn++;
    s = p_psTimeSearchTables(mjd, tableColumn, tableName, 1, &status);
    if ( status == PS_LOOKUP_ERROR ) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Time table search returned unsuccessful status.\n");
        return NULL;
    } else if (status != PS_LOOKUP_SUCCESS) {
        s = psTimeGetUT1Delta(time, bulletin);
    } else {
        s = SEC_TO_RAD(s);
    }

    //XXX: Do I need to do something for nutation terms w/periods of less than 2 days???
    psEarthPole *out = psEarthPoleAlloc();
    out->x = x;
    out->y = y;
    out->s = s;

    //    psFree(tableMetadataItem);
    //    psFree(timeMetadata);
    return out;
}

static psSphereRot *rotMatrix_To_Quat(double A[3][3])
{
    int i;
    psSphereRot *new = (psSphereRot*)psAlloc(sizeof(psSphereRot));
    //    psMemSetDeallocator(new, (psFreeFunc)sphereRotFree);
    //Convert rotation matrix to quaternions
    double diag_sum[3];
    int maxi;
    double recip;
    diag_sum[0] = 1.0 + A[0][0] - A[1][1] - A[2][2];
    diag_sum[1] = 1.0 - A[0][0] + A[1][1] - A[2][2];
    diag_sum[2] = 1.0 - A[0][0] - A[1][1] + A[2][2];
    diag_sum[3] = 1.0 + A[0][0] + A[1][1] + A[2][2];

    maxi = 0;
    for (i = 1; i < 4; ++i) {
        if (diag_sum[i] > diag_sum[maxi]) {
            maxi = i;
        }
    }

    double p = 0.5 * sqrt(diag_sum[maxi]);
    recip = 1.0 / (4.0 * p);

    if (maxi == 0) {
        new->q0 = p;
        new->q1 = recip * (A[0][1] + A[1][0]);
        new->q2 = recip * (A[2][0] + A[0][2]);
        new->q3 = recip * (A[1][2] - A[2][1]);
    } else if (maxi == 1) {
        new->q0 = recip * (A[0][1] + A[1][0]);
        new->q1 = p;
        new->q2 = recip * (A[1][2] + A[2][1]);
        new->q3 = recip * (A[2][0] - A[0][2]);
    } else if (maxi == 2) {
        new->q0 = recip * (A[2][0] + A[0][2]);
        new->q1 = recip * (A[1][2] + A[2][1]);
        new->q2 = p;
        new->q3 = recip * (A[0][1] - A[1][0]);
    } else if (maxi == 3) {
        new->q0 = recip * (A[1][2] - A[2][1]);
        new->q1 = recip * (A[2][0] - A[0][2]);
        new->q2 = recip * (A[0][1] - A[1][0]);
        new->q3 = p;
    }
    return new;
}

psSphereRot* psSphereRot_CEOtoGCRS(const psEarthPole *pole)
{
    PS_ASSERT_PTR_NON_NULL(pole,NULL);
    double A[3][3];
    psSphereRot *out = NULL;

    //Setup the rotation matrix and scalar value, a, as outlined by the ADD
    //XXX: Used formula for rotation matrix D from mathworld for z-axis rotation
    double a =  1.0 / (1.0 + sqrt(1.0 - (pole->x*pole->x + pole->y*pole->y) ) );
    A[0][0] = (1.0 - a*pole->x*pole->x)*cos(pole->s) - a*pole->x*pole->y*sin(pole->s);
    A[0][1] = -a*pole->x*pole->y*cos(pole->s) + (1.0 - a*pole->y*pole->y)*sin(pole->s);
    A[0][2] = pole->x*cos(pole->s) + pole->y*sin(pole->s);
    A[1][0] = -(1.0 - a*pole->x*pole->x)*sin(pole->s) - a*pole->x*pole->y*cos(pole->s);
    A[1][1] = a*pole->x*pole->y*sin(pole->s) + (1.0 - a*pole->y*pole->y)*cos(pole->s);
    A[1][2] = -pole->x*sin(pole->s) + pole->y*cos(pole->s);
    A[2][0] = -pole->x;
    A[2][1] = -pole->y;
    A[2][2] = 1.0 - a*(pole->x*pole->x + pole->y*pole->y);

    out = rotMatrix_To_Quat(A);

    return out;
}

psSphereRot* psSphereRot_TEOtoCEO(const psTime *time)
{
    PS_ASSERT_PTR_NON_NULL(time,NULL);
    psTime *in = psTimeAlloc(time->type);
    *in = *time;
    if (in->type != PS_TIME_UT1) {
        in = psTimeConvert(in, PS_TIME_UT1);
    }
    //    double T = (double)(in->sec) + (double)(in->nsec / 1e9);
    double T = psTimeToJD(in);
    T += -2451545.0;
    double theta = 2.0 * M_PI * (0.7790572732640 + 1.00273781191135448 * T);
    psSphereRot *out = psSphereRotAlloc(theta, 0.0, 0.0);
    //    psSphereRot *out = psSphereRotInvert(theta, 0.0, 0.0);

    psFree(in);
    return out;
}

psEarthPole* psEOC_GetPolarMotion(const psTime *time,
                                  psTimeBulletin bulletin)
{
    PS_ASSERT_PTR_NON_NULL(time, NULL);

    psEarthPole *out = psEarthPoleAlloc();
    psSphere *in = NULL;
    double s;
    psTime *time2 = psTimeAlloc(time->type);
    time2->sec = time->sec;
    time2->nsec = time->nsec;
    time2->leapsecond = time->leapsecond;
    //XXX:  Time must be converted to TAI before use?
    if (time->type == PS_TIME_UT1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "psTime cannot be of type UT1 here.\n");
        return NULL;
    } else if (time->type != PS_TIME_TAI) {
        time2 = psTimeConvert(time2, PS_TIME_TAI);
    }

    //XXX: This may be the wrong idea... ADD says to use 3rd-order polys to interpolate
    //polar motion coordinates.
    in = p_psTimeGetPoleCoords(time2);
    if (in == NULL) {
        psError(PS_ERR_BAD_PARAMETER_NULL, true,
                "p_psTimeGetPoleCoords return NULL psSphere for non-NULL input time.\n");
        return NULL;
    }
    out->x = in->r;
    out->y = in->d;

    s = psTimeGetUT1Delta(time2, bulletin);
    out->s = s;

    //XXX: Apply polar tide correction here???
    psEarthPole *correction = psEOC_PolarTideCorr(time2);
    out->x += correction->x;
    out->y += correction->y;
    out->s += correction->s;

    psFree(time2);
    psFree(in);
    psFree(correction);
    return out;
}

static double DMOD(double x, double y)
{
    double value = x - y * trunc(x/y);
    return value;
}

psEarthPole* psEOC_PolarTideCorr(const psTime *time)
{
    // Check for null parameter
    PS_ASSERT_PTR_NON_NULL(time, NULL);
    psEarthPole *out = psEarthPoleAlloc();

    // Convert psTime to MJD
    double MJD = psTimeToMJD(time);

    // Calculate number of Julian centuries since 2000
    //XXX: NOT SURE IF THIS IS CORRECT FOR THIS SITUATION
    double RJD = ( MJD - MJD_2000 ) / JULIAN_CENTURY;

    //Formula comes from fortran reference
    //DMOD in fortran ref. = double remainder -> x - y * trunc(x/y)
    double T, L, LPRIME, CAPF, CAPD, OMEGA, THETA, CORX, CORY, CORZ;
    double ARG1, ARG2, ARG3, ARG4, ARG5, ARG6, ARG7, ARG8;
    double T2, T3, T4;
    T = (RJD - 51544.5) / 36525.0;
    T2 = T*T;
    T3 = T*T*T;
    T4 = T*T*T*T;
    L = -0.0002447 * T4 + 0.051635 * T3 + 31.8792 * T2 + 1717915923.2178 * T + 485868.249036;
    L = DMOD(L, 1296000.0);
    LPRIME = -0.00001149 * T4 - 0.000136 * T3 - 0.5532 * T2 + 129596581.0481 * T + 1287104.79305;
    LPRIME = DMOD(LPRIME, 1296000.0);
    CAPF = 0.00000417 * T4 - 0.001037 * T3 - 12.7512 * T2 + 1739527262.8478 * T + 335779.526232;
    CAPF = DMOD(CAPF, 1296000.0);
    CAPD = -0.00003169 * T4 + 0.006593 * T3 - 6.3706 * T2 + 1602961601.209 * T + 1072260.70369;
    CAPD = DMOD(CAPD, 1296000.0);
    OMEGA = -0.00005939 * T4 + 0.007702 * T3 + 7.4722 * T2 - 6962890.2665 * T + 450160.398036;
    OMEGA = DMOD(OMEGA, 1296000.0);
    THETA = (67310.54841 + (876600.0 * 3600.0 + 8640184.812866) * T + 0.093104 * T2 -
             6.2e-6 * T3) * 15.0 + 648000.0;
    ARG7 = DMOD((-L - 2.0 * CAPF - 2.0 * OMEGA + THETA) * M_PI / 648000.0, 2.0 * M_PI)
           - M_PI / 2.0;
    ARG1 = DMOD((-2.0 * CAPF - 2.0 * OMEGA + THETA) * M_PI / 648000.0, 2.0 * M_PI) - M_PI / 2.0;
    ARG2 = DMOD((-2.0 * CAPF + 2.0 * CAPD - 2.0 * OMEGA + THETA) * M_PI / 648000.0, 2.0 * M_PI)
           - M_PI / 2.0;
    ARG3 = DMOD(THETA * M_PI / 648000.0, 2.0 * M_PI) - M_PI / 2.0;
    ARG4 = DMOD((-L - 2.0 * CAPF - 2.0 * OMEGA + 2.0 * THETA) * M_PI / 648000.0, 2.0 * M_PI);
    ARG5 = DMOD((-2.0 * CAPF - 2.0 * OMEGA + 2.0 * THETA) * M_PI / 648000.0, 2.0 * M_PI);
    ARG6 = DMOD((-2.0 * CAPF + 2.0 * CAPD - 2.0 * OMEGA + 2.0 * THETA) * M_PI / 648000.0,
                2.0 * M_PI);
    ARG8 = DMOD((2.0 * THETA) * M_PI / 648000.0, 2.0 * M_PI);
    CORX = -0.026 * sin(ARG7) + 0.006 * cos(ARG7)
           -0.133 * sin(ARG1) + 0.049 * cos(ARG1)
           -0.050 * sin(ARG2) + 0.025 * cos(ARG2)
           -0.152 * sin(ARG3) + 0.078 * cos(ARG3)
           -0.057 * sin(ARG4) - 0.013 * cos(ARG4)
           -0.330 * sin(ARG5) - 0.028 * cos(ARG5)
           -0.145 * sin(ARG6) + 0.064 * cos(ARG6)
           -0.036 * sin(ARG8) + 0.017 * cos(ARG8);
    CORY = -0.006 * sin(ARG7) - 0.026 * cos(ARG7)
           -0.049 * sin(ARG1) - 0.133 * cos(ARG1)
           -0.025 * sin(ARG2) - 0.050 * cos(ARG2)
           -0.078 * sin(ARG3) - 0.152 * cos(ARG3)
           +0.011 * sin(ARG4) + 0.033 * cos(ARG4)
           +0.037 * sin(ARG5) + 0.196 * cos(ARG5)
           +0.059 * sin(ARG6) + 0.087 * cos(ARG6)
           +0.018 * sin(ARG8) + 0.022 * cos(ARG8);
    CORZ =  0.0245 * sin(ARG7) + 0.0503 * cos(ARG7)
            +0.1210 * sin(ARG1) + 0.1605 * cos(ARG1)
            +0.0286 * sin(ARG2) + 0.0516 * cos(ARG2)
            +0.0864 * sin(ARG3) + 0.1771 * cos(ARG3)
            -0.0380 * sin(ARG4) - 0.0154 * cos(ARG4)
            -0.1617 * sin(ARG5) - 0.0720 * cos(ARG5)
            -0.0759 * sin(ARG6) - 0.0004 * cos(ARG6)
            -0.0196 * sin(ARG8) - 0.0038 * cos(ARG8);
    CORX = CORX * 1.0e-3;
    CORY = CORY * 1.0e-3;
    CORZ = CORZ * 0.1e-3;

    out->x = CORX;
    out->y = CORY;
    out->s = CORZ;

    return out;
}

psEarthPole* psEOC_NutationCorr(psTime *time)
{
    // Check for null parameter
    PS_ASSERT_PTR_NON_NULL(time, NULL);
    if (time->type == PS_TIME_UT1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Invalid time input.  Time cannot be of type UT1.\n");
        return NULL;
    }

    // Convert psTime to MJD
    double MJD = psTimeToMJD(time);

    // Calculate number of Julian centuries since 2000
    double t = ( MJD - MJD_2000 ) / JULIAN_CENTURY;
    double t2 = t*t;
    double t3 = t*t*t;
    double t4 = t*t*t*t;

    //XXX: I think the t's should be inside of the SEC_TO_RAD conversion.
    //Check this and for Precession Model as well!
    double F[5];
    F[0] = DEG_TO_RAD(134.96340251) +
           SEC_TO_RAD(1717915923.2178)*t +
           SEC_TO_RAD(31.8792)*t2 +
           SEC_TO_RAD(0.051635)*t3 -
           SEC_TO_RAD(0.00024470)*t4;

    // Mean Anomaly of the Sun
    F[1] = DEG_TO_RAD(357.52910918) +
           SEC_TO_RAD(129596581.0481)*t -
           SEC_TO_RAD(0.5532)*t2 +
           SEC_TO_RAD(0.000136)*t3 -
           SEC_TO_RAD(0.00001149)*t4;

    // L − Omega
    F[2] = DEG_TO_RAD(93.27209062) +
           SEC_TO_RAD(1739527262.8478)*t -
           SEC_TO_RAD(12.7512)*t2 -
           SEC_TO_RAD(0.001037)*t3 +
           SEC_TO_RAD(0.00000417)*t4;

    // Mean Elongation of the Moon from the Sun
    F[3] = DEG_TO_RAD(297.85019547) +
           SEC_TO_RAD(1602961601.2090)*t -
           SEC_TO_RAD(6.3706)*t2 +
           SEC_TO_RAD(0.006593)*t3 -
           SEC_TO_RAD(0.00003169)*t4;

    // Mean Longitude of the Ascending Node of the Moon
    F[4] = DEG_TO_RAD(125.04455501) -
           SEC_TO_RAD(6962890.5431)*t +
           SEC_TO_RAD(7.4722)*t2 +
           SEC_TO_RAD(0.007702)*t3 -
           SEC_TO_RAD(0.0000593)*t4;

    //argument values taken from table 5.1 in IERS techical note No.32
    //http://maia.usno.navy.mil/conv2000/chapter5/tn32_c5.pdf, p38
    //Units are in micro-arcseconds here and must be converted to radians before using
    double w_l[10] = {SEC_TO_RAD(-1.0), SEC_TO_RAD(-1.0), SEC_TO_RAD(1.0), 0.0, 0.0,
                      SEC_TO_RAD(-1.0), 0.0, 0.0, 0.0, SEC_TO_RAD(1.0)};
    double w_l_p[10] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
    double w_F[10] = {SEC_TO_RAD(-2.0), SEC_TO_RAD(-2.0), SEC_TO_RAD(-2.0),
                      SEC_TO_RAD(-2.0), SEC_TO_RAD(-2.0), 0.0, SEC_TO_RAD(-2.0), 0.0,0.0,0.0};
    double w_D[10] = {0.0,0.0, SEC_TO_RAD(-2.0), 0.0,0.0,0.0, SEC_TO_RAD(2.0), 0.0,0.0,0.0};
    double w_Omega[10] = {SEC_TO_RAD(-1.0), SEC_TO_RAD(-2.0), SEC_TO_RAD(-2.0),
                          SEC_TO_RAD(-1.0), SEC_TO_RAD(-2.0), 0.0, SEC_TO_RAD(-2.0), 0.0, SEC_TO_RAD(-1.0), 0.0};
    double xp_sin[10] = {SEC_TO_RAD(-0.44), SEC_TO_RAD(-2.31), SEC_TO_RAD(-0.44),
                         SEC_TO_RAD(-2.14), SEC_TO_RAD(-11.36), SEC_TO_RAD(0.84), SEC_TO_RAD(-4.76),
                         SEC_TO_RAD(14.27), SEC_TO_RAD(1.93), SEC_TO_RAD(0.76)};
    double xp_cos[10] = {SEC_TO_RAD(0.25), SEC_TO_RAD(1.32), SEC_TO_RAD(0.25), SEC_TO_RAD(1.23),
                         SEC_TO_RAD(6.52), SEC_TO_RAD(-0.48), SEC_TO_RAD(2.73), SEC_TO_RAD(-8.19),
                         SEC_TO_RAD(-1.11), SEC_TO_RAD(-0.43)};
    double yp_sin[10] = {SEC_TO_RAD(-0.25), SEC_TO_RAD(-1.32), SEC_TO_RAD(-0.25),
                         SEC_TO_RAD(-1.23), SEC_TO_RAD(-6.52), SEC_TO_RAD(0.48), SEC_TO_RAD(-2.73),
                         SEC_TO_RAD(8.19), SEC_TO_RAD(1.11), SEC_TO_RAD(0.43)};
    double yp_cos[10] = {SEC_TO_RAD(-0.44), SEC_TO_RAD(-2.31), SEC_TO_RAD(-0.44),
                         SEC_TO_RAD(-2.14), SEC_TO_RAD(-11.36), SEC_TO_RAD(0.84), SEC_TO_RAD(-4.76),
                         SEC_TO_RAD(14.27), SEC_TO_RAD(1.93), SEC_TO_RAD(0.76)};

    double X = 0.0;
    double Y = 0.0;
    double arg = 0.0;
    //This is from eqn 131 in the ADD - Note: pj_tj isn't included the first time.
    //XXX: The xp_sin, yp_cos, etc. may need to be multiplied by pow(t,i) here? adding now...
    double tj = 0.0;

    // calculate the polynomial portion first - the pj * t^j (poly coeff's)
    // Check if EOC data loaded
    if(! eocInitialized) {
        eocInitialized = p_psEOCInit();
        if(!eocInitialized) {
            // XXX: Move error message.
            psError(PS_ERR_UNKNOWN, false,
                    "Could not initialize EOC tables -- check data files.");
            return NULL;
        }
    }
    X = psPolynomial1DEval(xPoly,t);
    Y = psPolynomial1DEval(yPoly,t);
    for (int i = 0; i < 10; i++) {
        tj = pow(t, i);
        arg = w_l[i]*F[0] + w_l_p[i]*F[1] + w_F[i]*F[2] + w_D[i]*F[3] + w_Omega[i]*F[4];
        X += xp_sin[i] * tj * sin(arg) + xp_cos[i] * tj * cos(arg);
        Y += yp_sin[i] * tj * sin(arg) + yp_cos[i] * tj * cos(arg);
    }

    psEarthPole *pole = psEarthPoleAlloc();
    pole->x = X;
    pole->y = Y;
    pole->s = SEC_TO_RAD(4.7e-5) * t;

    return pole;
}

psSphereRot* psSphereRot_ITRStoTEO(const psEarthPole* motion)
{
    PS_ASSERT_PTR_NON_NULL(motion,NULL);
    double A[3][3];
    psSphereRot *out = NULL;

    //Setup Rotation Matrix for transformation (x,y,z rotation)
    //XXX: May need to be (z,y,x as in Mathworld?)
    A[0][0] = cos(motion->x)*cos(-motion->s);
    A[0][1] = sin(motion->y)*sin(motion->x)*cos(-motion->s) + cos(motion->y)*sin(-motion->s);
    A[0][2] = -cos(motion->y)*sin(motion->x)*cos(-motion->s) + sin(motion->y)*sin(-motion->s);
    A[1][0] = -cos(motion->x)*sin(-motion->s);
    A[1][1] = -sin(motion->y)*sin(motion->x)*sin(-motion->s) + cos(motion->y)*cos(-motion->s);
    A[1][2] = cos(motion->y)*sin(motion->x)*sin(-motion->s) + sin(motion->y)*cos(-motion->s);
    A[2][0] = sin(motion->x);
    A[2][1] = -sin(motion->y)*cos(motion->x);
    A[2][2] = cos(motion->y)*cos(motion->x);

    //Convert rotation matrix to quaternions
    out = rotMatrix_To_Quat(A);
    /*    double diag_sum[3];
        int maxi;
        double recip;
        diag_sum[0] = 1.0 + A[0][0] - A[1][1] - A[2][2];
        diag_sum[1] = 1.0 - A[0][0] + A[1][1] - A[2][2];
        diag_sum[2] = 1.0 - A[0][0] - A[1][1] + A[2][2];
        diag_sum[3] = 1.0 + A[0][0] + A[1][1] + A[2][2];

        maxi = 0;
        for (int i = 1; i < 4; ++i) {
            if (diag_sum[i] > diag_sum[maxi]) {
                maxi = i;
            }
        }

        double p = 0.5 * sqrt(diag_sum[maxi]);
        recip = 1.0 / (4.0 * p);

        if (maxi == 0) {
            out->q0 = p;
            out->q1 = recip * (A[0][1] + A[1][0]);
            out->q2 = recip * (A[2][0] + A[0][2]);
            out->q3 = recip * (A[1][2] - A[2][1]);
        } else if (maxi == 1) {
            out->q0 = recip * (A[0][1] + A[1][0]);
            out->q1 = p;
            out->q2 = recip * (A[1][2] + A[2][1]);
            out->q3 = recip * (A[2][0] - A[0][2]);
        } else if (maxi == 2) {
            out->q0 = recip * (A[2][0] + A[0][2]);
            out->q1 = recip * (A[1][2] + A[2][1]);
            out->q2 = p;
            out->q3 = recip * (A[0][1] - A[1][0]);
        } else if (maxi == 3) {
            out->q0 = recip * (A[1][2] - A[2][1]);
            out->q1 = recip * (A[2][0] - A[0][2]);
            out->q2 = recip * (A[0][1] - A[1][0]);
            out->q3 = p;
        }
    */
    return out;
}
