/** @file  psEarthOrientation.c
 *
 *  @brief Function implementations for earth orientation calculations
 *
 *  @ingroup EarthOrientation
 *
 *  @author Dave Robbins, MHPCC
 *  @author Robert Daniel DeSonia, MHPCC
 *
 *  @version $Revision: 1.25 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-12-20 05:05:37 $
 *
 *  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 psArray* iersTable = NULL;
static psArray* finalsTable = 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;
    }

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

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

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

    char* finalsTableFormat = psMetadataLookupStr(&success, eocMetadata,
                              "psLib.eoc.precession.finals.table.format");
    if(! success || iersTableFormat == NULL) {
        psFree(tableFormat);
        psFree(xTableName);
        psFree(yTableName);
        psFree(sTableName);
        psFree(iersTableName);
        psFree(finalsTableName);
        psFree(iersTableFormat);
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, PS_ERRORTEXT_psTime_LOOKUP_METADATA_FAILED,
                "psLib.eoc.precession.iers.table.format");
        return false;
    }

    xTable = psVectorsReadFromFile(xTableName, tableFormat);
    yTable = psVectorsReadFromFile(yTableName, tableFormat);
    sTable = psVectorsReadFromFile(sTableName, tableFormat);
    iersTable = psVectorsReadFromFile(iersTableName, iersTableFormat);
    finalsTable = psVectorsReadFromFile(finalsTableName, finalsTableFormat);
    psFree(tableFormat);
    psFree(iersTableFormat);
    psFree(finalsTableFormat);
    psFree(xTableName);
    psFree(yTableName);
    psFree(sTableName);
    psFree(finalsTableName);
    psFree(iersTableName);

    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;
    }

    if(iersTable == NULL || finalsTable == NULL) {
        // XXX: need to move the error message to the error messages file.
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Failed to read the IERS/finals 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
    psFree(iersTable);
    psFree(finalsTable);

    xTable = NULL;
    yTable = NULL;
    sTable = NULL;
    iersTable = NULL;
    finalsTable = 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);

    X = SEC_TO_RAD(X * 1e-6);
    Y = SEC_TO_RAD(Y * 1e-6);
    S = SEC_TO_RAD(S * 1e-6);

    // 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];
        //        printf("as-x, ac-x, = %.13g, %.13g\n", as, ac);
        as = SEC_TO_RAD(as) * 1e-6;
        ac = SEC_TO_RAD(ac) * 1e-6;
        //        X += as*tj*sin(arg) + ac*tj*cos(arg);
        X += (as*tj*sin(arg) + ac*cos(arg)) * tj;
    }

    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];
        as = SEC_TO_RAD(as) * 1e-6;
        ac = SEC_TO_RAD(ac) * 1e-6;

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

    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];
        as = SEC_TO_RAD(as) * 1e-6;
        ac = SEC_TO_RAD(ac) * 1e-6;

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

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

    psEarthPole *out = psEarthPoleAlloc();
    out->x = 0.0;
    out->y = 0.0;
    out->s = 0.0;

    double MJD = psTimeToMJD(time);

    if (MJD < 41684.0 || MJD > 53334.0) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Invalid time input.  Date, %lf, is out of range (41684-53334)\n", MJD);
        return out;
    }

    // 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;
        }
    }

    psF64* cols[5];
    for (int colNum = 0; colNum < 5; colNum++) {
        cols[colNum] = ((psVector*)(iersTable->data[colNum]))->data.F64;
    }
    int numRows = ((psVector*)(iersTable->data[0]))->n;
    for (int rowNum = 0; rowNum < numRows; rowNum++) {
        if ( (MJD - cols[0][rowNum]) < 1.0 ) {
            if (bulletin == PS_IERS_A) {
                out->x = cols[1][rowNum];
                out->y = cols[2][rowNum];
                out->x = SEC_TO_RAD(out->x) * 1e-3;
                out->y = SEC_TO_RAD(out->y) * 1e-3;
                rowNum = numRows;
            } else {
                out->x = cols[3][rowNum];
                out->y = cols[4][rowNum];
                out->x = SEC_TO_RAD(out->x) * 1e-3;
                out->y = SEC_TO_RAD(out->y) * 1e-3;
                rowNum = numRows;
            }
        }
    }
    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);
    */

    A[0][0] = (1.0 - a*pole->x*pole->x)*cos(pole->s) - a*pole->x*pole->y*sin(pole->s);
    A[1][0] = -a*pole->x*pole->y*cos(pole->s) + (1.0 - a*pole->y*pole->y)*sin(pole->s);
    A[2][0] = pole->x*cos(pole->s) + pole->y*sin(pole->s);
    A[0][1] = -(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[2][1] = -pole->x*sin(pole->s) + pole->y*cos(pole->s);
    A[0][2] = -pole->x;
    A[1][2] = -pole->y;
    A[2][2] = 1.0 - a*(pole->x*pole->x + pole->y*pole->y);

    double x, y, s;
    x = pole->x;
    y = pole->y;
    s = pole->s;
    /*    A[0][0] = (1.0 - a*x*x)*cos(s) + a*x*y*sin(s);
        A[0][1] = (1.0 - a*x*x)*sin(s) - a*x*y*cos(s);
        A[0][2] = x;
        A[1][0] = -a*x*y*cos(s) - (1.0 - a*y*y)*sin(s);
        A[1][1] = -a*x*y*sin(s) + (1.0 - a*y*y)*cos(s);
        A[1][2] = y;
        A[2][0] = -x*cos(s) + y*sin(s);
        A[2][1] = -x*sin(s) - y*cos(s);
        A[2][2] = 1.0 - a * (x*x + y*y);
    */
    /*    A[0][0] = (1.0 - a*x*x)*cos(s) + a*x*y*sin(s);
        A[1][0] = (1.0 - a*x*x)*sin(s) - a*x*y*cos(s);
        A[2][0] = x;
        A[0][1] = -a*x*y*cos(s) - (1.0 - a*y*y)*sin(s);
        A[1][1] = -a*x*y*sin(s) + (1.0 - a*y*y)*cos(s);
        A[2][1] = y;
        A[0][2] = -x*cos(s) + y*sin(s);
        A[1][2] = -x*sin(s) - y*cos(s);
        A[2][2] = 1.0 - a * (x*x + y*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);
    PS_ASSERT_INT_WITHIN_RANGE(bulletin, PS_IERS_A, PS_IERS_B, NULL);

    psEarthPole *out = psEarthPoleAlloc();
    out->x = 0.0;
    out->y = 0.0;
    out->s = 0.0;

    double MJD = psTimeToMJD(time);
    /*
        if (MJD < 41684.0 || (MJD > 53434.0 && bulletin == PS_IERS_B) ||
                (MJD > 53858.0 && bulletin == PS_IERS_A) ) {
            psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                    "Invalid time input.  Date, %lf, is out of range\n", MJD);
            return out;
        }
    */
    // 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;
        }
    }

    psF64* cols[7];
    for (int colNum = 0; colNum < 7; colNum++) {
        cols[colNum] = ((psVector*)(finalsTable->data[colNum]))->data.F64;
    }
    int numRows = ((psVector*)(finalsTable->data[0]))->n;
    psVector *X = psVectorAlloc(numRows, PS_TYPE_F64);
    psVector *Y = psVectorAlloc(numRows, PS_TYPE_F64);
    psVector *S = psVectorAlloc(numRows, PS_TYPE_F64);
    psVector *T = psVectorAlloc(numRows, PS_TYPE_F64);
    if (bulletin == PS_IERS_A) {
        for (int rowNum = 0; rowNum < numRows; rowNum++) {
            T->data.F64[rowNum] = cols[0][rowNum];
            X->data.F64[rowNum] = cols[1][rowNum];
            Y->data.F64[rowNum] = cols[2][rowNum];
            S->data.F64[rowNum] = cols[3][rowNum];
        }
    } else {
        for (int rowNum = 0; rowNum < numRows; rowNum++) {
            T->data.F64[rowNum] = cols[0][rowNum];
            X->data.F64[rowNum] = cols[4][rowNum];
            Y->data.F64[rowNum] = cols[5][rowNum];
            S->data.F64[rowNum] = cols[6][rowNum];
        }
    }

    double xOut = 0.0;
    double yOut = 0.0;
    double sOut = 0.0;
    double xTerm = 0.0;
    double yTerm = 0.0;
    double sTerm = 0.0;
    int k = 0;
    for (int i = 0; i < (numRows-1); i++) {
        if (MJD >= T->data.F64[i] && MJD < T->data.F64[i+1]) {
            k = i;
            if (k < 2) {
                k = 2;
            }
            if (k > (numRows-2)) {
                k = numRows-2;
            }
            for (int m = k-1; m <= k+2; m++) {
                xTerm = X->data.F64[m];
                yTerm = Y->data.F64[m];
                sTerm = S->data.F64[m];
                for (int j = k-1; j <= k+2; j++) {
                    if ( m != j) {
                        double term = (MJD - T->data.F64[j])/(T->data.F64[m] - T->data.F64[j]);
                        xTerm *= term;
                        yTerm *= term;
                        sTerm *= term;
                    }
                }
                xOut += xTerm;
                yOut += yTerm;
                sOut += sTerm;
            }
            i = numRows-1;
        }
    }
    out->x = SEC_TO_RAD(xOut);
    out->y = SEC_TO_RAD(yOut);
    out->s = SEC_TO_RAD(sOut);


    /*
        for (int rowNum = 0; rowNum < numRows; rowNum++) {
            if ( (MJD - cols[0][rowNum]) < 1.0 ) {
                if (bulletin == PS_IERS_A) {
                    out->x = cols[1][rowNum];
                    out->y = cols[2][rowNum];
                    out->s = cols[3][rowNum];
                    out->x = SEC_TO_RAD(out->x);
                    out->y = SEC_TO_RAD(out->y);
                    out->s = SEC_TO_RAD(out->s);
                    rowNum = numRows;
                } else {
                    out->x = cols[4][rowNum];
                    out->y = cols[5][rowNum];
                    out->s = cols[6][rowNum];
                    out->x = SEC_TO_RAD(out->x);
                    out->y = SEC_TO_RAD(out->y);
                    out->s = SEC_TO_RAD(out->s);
                    rowNum = numRows;
                }
            }
        }
    */
    psFree(X);
    psFree(Y);
    psFree(S);
    psFree(T);
    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;
    double RJD = MJD;

    //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);
    X = SEC_TO_RAD(X * 1e-6);
    Y = SEC_TO_RAD(Y * 1e-6);

    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] * 1e-6 * tj * sin(arg) + xp_cos[i] * 1e-6 * cos(arg)) * tj;
        Y += (yp_sin[i] * 1e-6 * tj * sin(arg) + yp_cos[i] * 1e-6 * cos(arg)) * tj;
    }

    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;

    double x,y,s;
    x = motion->x;
    y = motion->y;
    s = motion->s;
    s = -s;


    //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);
    */
    /*
        A[0][0] = cos(motion->x)*cos(-motion->s);
        A[1][0] = sin(motion->y)*sin(motion->x)*cos(-motion->s) + cos(motion->y)*sin(-motion->s);
        A[2][0] = -cos(motion->y)*sin(motion->x)*cos(-motion->s) + sin(motion->y)*sin(-motion->s);
        A[0][1] = -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[2][1] = cos(motion->y)*sin(motion->x)*sin(-motion->s) + sin(motion->y)*cos(-motion->s);
        A[0][2] = sin(motion->x);
        A[1][2] = -sin(motion->y)*cos(motion->x);
        A[2][2] = cos(motion->y)*cos(motion->x);
    */
    /*
        A[0][0] = cos(s)*cos(x);
        A[0][1] = sin(s)*sin(y) + cos(s)*sin(x)*sin(y);
        A[0][2] = sin(s)*sin(y) - cos(s)*sin(x)*cos(y);
        A[1][0] = -sin(s)*cos(x);
        A[1][1] = cos(s)*cos(y) - sin(s)*sin(x)*sin(y);
        A[1][2] = cos(s)*sin(y) + sin(s)*sin(x)*cos(y);
        A[2][0] = sin(x);
        A[2][1] = -cos(x)*sin(y);
        A[2][2] = cos(x)*cos(y);
    */
    /*
        A[0][0] = cos(s)*cos(x);
        A[1][0] = sin(s)*sin(y) + cos(s)*sin(x)*sin(y);
        A[2][0] = sin(s)*sin(y) - cos(s)*sin(x)*cos(y);
        A[0][1] = -sin(s)*cos(x);
        A[1][1] = cos(s)*cos(y) - sin(s)*sin(x)*sin(y);
        A[2][1] = cos(s)*sin(y) + sin(s)*sin(x)*cos(y);
        A[0][2] = sin(x);
        A[1][2] = -cos(x)*sin(y);
        A[2][2] = cos(x)*cos(y);
    */

    /*
                psSphereRot r,p,t;
     
                r.q0=sin(y/2.0);
                r.q1=0;
                r.q2=0;
                r.q3=cos(y/2.0);
     
                p.q0=0;
                p.q1=sin(x/2.0);
                p.q2=0;
                p.q3=cos(x/2.0);
     
                t.q0=0;
                t.q1=0;
                t.q2=sin(s/2.0);
                t.q3=cos(s/2.0);
     
                // calculate t*s*r.
                psSphereRot* temp = psSphereRotCombine(NULL,&t,&p);
                out = psSphereRotCombine(NULL, temp, &r);
                psFree(temp);
     
                return out;
    */

    //Newest trial - mult. y matrix * x * z

    A[0][0] = cos(x)*cos(s);
    A[1][0] = cos(x)*sin(s);
    A[2][0] = -sin(x);
    A[0][1] = sin(x)*sin(y)*cos(s) - cos(y)*sin(s);
    A[1][1] = sin(x)*sin(y)*sin(s) + cos(y)*cos(s);
    A[2][1] = cos(x)*sin(y);
    A[0][2] = sin(x)*cos(y)*cos(s) + sin(y)*sin(s);
    A[1][2] = sin(x)*cos(y)*sin(s) - sin(y)*cos(s);
    A[2][2] = cos(x)*cos(y);

    /*
            A[0][0] = cos(x)*cos(s);
            A[0][1] = cos(x)*sin(s);
            A[0][2] = -sin(x);
            A[1][0] = sin(x)*sin(y)*cos(s) - cos(y)*sin(s);
            A[1][1] = sin(x)*sin(y)*sin(s) + cos(y)*cos(s);
            A[1][2] = cos(x)*sin(y);
            A[2][0] = sin(x)*cos(y)*cos(s) + sin(y)*sin(s);
            A[2][1] = sin(x)*cos(y)*sin(s) - sin(y)*cos(s);
            A[2][2] = cos(x)*cos(y);
    */
    //New trial - mult z * y * x
    /*    A[0][0] = cos(s)*cos(x);
        A[1][0] = sin(s)*cos(y) + cos(s)*sin(x)*sin(y);
        A[2][0] = sin(s)*sin(y) - cos(s)*sin(x)*cos(y);
        A[0][1] = -sin(s)*cos(x);
        A[1][1] = cos(s)*cos(y) - sin(s)*sin(x)*sin(y);
        A[2][1] = cos(s)*sin(y) + sin(s)*sin(x)*cos(y);
        A[0][2] = sin(x);
        A[1][2] = -cos(x)*sin(y);
        A[2][2] = cos(x)*cos(y);
    */
    /*
        A[0][0] = cos(s)*cos(x);
        A[0][1] = sin(s)*cos(y) + cos(s)*sin(x)*sin(y);
        A[0][2] = sin(s)*sin(y) - cos(s)*sin(x)*cos(y);
        A[1][0] = -sin(s)*cos(x);
        A[1][1] = cos(s)*cos(y) - sin(s)*sin(x)*sin(y);
        A[1][2] = cos(s)*sin(y) + sin(s)*sin(x)*cos(y);
        A[2][0] = sin(x);
        A[2][1] = -cos(x)*sin(y);
        A[2][2] = cos(x)*cos(y);
    */

    //Convert rotation matrix to quaternions
    out = rotMatrix_To_Quat(A);

    return out;
}
