/** @file  psCoord.c
*
*  @brief Contains basic coordinate transformation definitions and operations
*
*  This file defines the basic types for astronomical coordinate
*  transformation
*
*  @ingroup CoordinateTransform
*
*  @author GLG, MHPCC
*
*  @version $Revision: 1.61 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-03-31 23:01:46 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*/
/******************************************************************************/
/*  INCLUDE FILES                                                             */
/******************************************************************************/
#include "psType.h"
#include "psCoord.h"
#include "psMemory.h"
#include "psTime.h"
#include "psConstants.h"
#include "psError.h"
#include "psLogMsg.h"
#include "psAstronomyErrors.h"
#include "psAstrometry.h"
#include "psMatrix.h"
#include <math.h>
#include <float.h>
/******************************************************************************/
/*  DEFINE STATEMENTS                                                         */
/******************************************************************************/

// Modified Julian Day 01/01/1900 00:00:00
#define MJD_1900 15021.0

// Days in Julian century
#define JULIAN_CENTURY 36525.0

/******************************************************************************/
/*  TYPE DEFINITIONS                                                          */
/******************************************************************************/

// None

/*****************************************************************************/
/*  GLOBAL VARIABLES                                                         */
/*****************************************************************************/

// None

/*****************************************************************************/
/*  FILE STATIC VARIABLES                                                    */
/*****************************************************************************/

// None

/*****************************************************************************/
/*  FUNCTION IMPLEMENTATION - LOCAL                                          */
/*****************************************************************************/
/*****************************************************************************
multiplyDPoly2D(trans1, trans2): Takes two 2-D polynomials as input and
multiplies them.  Basically, for each non-zero coeff in the trans1 coeff[][]
array, you must multiply by all non-zero coeffs in trans2.
 
XXX: Inefficient in that the out polynomial is allocated every time.
 *****************************************************************************/

psDPolynomial2D *multiplyDPoly2D(psDPolynomial2D *trans1,
                                 psDPolynomial2D *trans2)
{
    psS32 orderX = (trans1->nX + trans2->nX) - 1;
    psS32 orderY = (trans1->nX + trans2->nX) - 1;

    psDPolynomial2D *out = psDPolynomial2DAlloc(orderX, orderY, PS_POLYNOMIAL_ORD);
    for (psS32 i = 0 ; i < out->nX; i++) {
        for (psS32 j = 0 ; j < out->nY; j++) {
            out->coeff[i][j] = 0.0;
            out->mask[i][j] = 0;
        }
    }

    for (psS32 t1x = 0 ; t1x < trans1->nX ; t1x++) {
        for (psS32 t1y = 0 ; t1y < trans1->nY ; t1y++) {
            if (0.0 != trans1->coeff[t1x][t1y]) {
                for (psS32 t2x = 0 ; t2x < trans2->nX ; t2x++) {
                    for (psS32 t2y = 0 ; t2y < trans2->nY ; t2y++) {
                        out->coeff[t1x+t2x][t1y+t2y]+= (trans1->coeff[t1x][t1y] * trans2->coeff[t2x][t2y]);
                    }
                }
            }
        }
    }
    return(out);
}

/*****************************************************************************/
/* FUNCTION IMPLEMENTATION - PUBLIC                                          */
/*****************************************************************************/
static void planeFree(psPlane *p)
{
    // There are non dynamic allocated items
}

/*****************************************************************************
p_psPlaneTransformLinearInvert(transform): : this is a private function which
simply inverts the supplied psPlaneTransform transform.  It assumes that
"transform" is linear.
 
This program assumes that the inverse of the following linear equations:
        X2 = A + (B * X1) + (C * Y1);
        Y2 = D + (E * X1) + (F * Y1);
is
        Y1 = (Y2 - ((E/B) * X2) - D + ((E*A)/B)) / (F - ((C*E)/B));
        X1 = (Y2 - ((F/C) * X2) - D + ((F*A)/C)) / (E - ((F*B)/C));
or
 X1 = (-D + ((F*A)/C)) / (E - ((F*B)/C)) +
      (X2 * -((F/C) / (E - ((F*B)/C)))) +
             (Y2 * (1.0 / (E - ((F*B)/C))));
        Y1 = (-D + ((E*A)/B))/(F - ((C*E)/B)) +
             (X2 * -((E/B) / (F - ((C*E)/B)))) +
             (Y2 * (1.0 / (F - ((C*E)/B))));
 
XXX: Since thre is now a general psPlaneTransformInvert() function, we
should rename this.
 
 *****************************************************************************/
psPlaneTransform *p_psPlaneTransformLinearInvert(psPlaneTransform *transform)
{
    PS_PTR_CHECK_NULL(transform, 0);
    PS_PTR_CHECK_NULL(transform->x, 0);
    PS_PTR_CHECK_NULL(transform->y, 0);

    psF64 A = 0.0;
    psF64 B = 0.0;
    psF64 C = 0.0;
    psF64 D = 0.0;
    psF64 E = 0.0;
    psF64 F = 0.0;

    // XXX: Test this for correctness.
    A = transform->x->coeff[0][0];
    if (transform->x->nX >= 2) {
        B = transform->x->coeff[1][0];
    }
    if (transform->x->nY >= 2) {
        C = transform->x->coeff[0][1];
    }
    D = transform->y->coeff[0][0];
    if (transform->y->nX >= 2) {
        E = transform->y->coeff[1][0];
    }
    if (transform->y->nY >= 2) {
        F = transform->y->coeff[0][1];
    }

    // XXX: Use the constructor here.
    psPlaneTransform *out = psPlaneTransformAlloc(2, 2);

    out->x->coeff[0][0] = -D + ((F*A)/C) / (E - ((F*B)/C));
    out->x->coeff[1][0] = -(F/C) / (E - ((F*B)/C));
    out->x->coeff[0][1] =  1.0 / (E - ((F*B)/C));
    out->y->coeff[0][0] = -D + ((E*A)/B) / (F - ((C*E)/B));
    out->y->coeff[1][0] = -(E/B) / (F - ((C*E)/B));
    out->y->coeff[0][1] =  1.0 / (F - ((C*E)/B));

    return(out);
}

/*****************************************************************************
p_psIsProjectionLinear(): this is a private function which simply determines
if the supplied psPlaneTransform transform is linear: if any of the
cooefficients of order 2 are higher are non-zero, then it is not linear.
 *****************************************************************************/
psS32 p_psIsProjectionLinear(psPlaneTransform *transform)
{
    PS_PTR_CHECK_NULL(transform, 0);
    PS_PTR_CHECK_NULL(transform->x, 0);
    PS_PTR_CHECK_NULL(transform->y, 0);

    for (psS32 i=0;i<(transform->x->nX);i++) {
        for (psS32 j=0;j<(transform->x->nY);j++) {
            if (transform->x->coeff[i][j] != 0.0) {
                if (!(((i == 0) && (j == 0)) ||
                        ((i == 0) && (j == 1)) ||
                        ((i == 1) && (j == 0)))) {
                    return(0);
                }
            }
        }
    }

    for (psS32 i=0;i<(transform->y->nX);i++) {
        for (psS32 j=0;j<(transform->y->nY);j++) {
            if (transform->y->coeff[i][j] != 0.0) {
                if (!(((i == 0) && (j == 0)) ||
                        ((i == 0) && (j == 1)) ||
                        ((i == 1) && (j == 0)))) {
                    return(0);
                }
            }
        }
    }

    return(1);
}

// XXX: Must test psPlaneAlloc() and planeFree().
// XXX: Must rewrite code and tests to use these functions.
psPlane* psPlaneAlloc(void)
{
    psPlane *p = psAlloc(sizeof(psPlane));

    p_psMemSetDeallocator(p, (psFreeFcn) planeFree);
    return(p);
}


static void sphereFree(psSphere *s)
{
    // There are non dynamic allocated items
}

psSphere* psSphereAlloc(void)
{
    psSphere *s = psAlloc(sizeof(psSphere));

    p_psMemSetDeallocator(s, (psFreeFcn) sphereFree);
    return(s);
}

static void planeTransformFree(psPlaneTransform *pt)
{
    psFree(pt->x);
    psFree(pt->y);
}

psPlaneTransform* psPlaneTransformAlloc(psS32 n1, psS32 n2)
{
    PS_INT_CHECK_NON_NEGATIVE(n1, NULL);
    PS_INT_CHECK_NON_NEGATIVE(n2, NULL);

    psPlaneTransform *pt = psAlloc(sizeof(psPlaneTransform));
    pt->x = psDPolynomial2DAlloc(n1, n2, PS_POLYNOMIAL_ORD);
    pt->y = psDPolynomial2DAlloc(n1, n2, PS_POLYNOMIAL_ORD);

    p_psMemSetDeallocator(pt, (psFreeFcn) planeTransformFree);
    return(pt);
}

psPlane* psPlaneTransformApply(psPlane* out,
                               const psPlaneTransform* transform,
                               const psPlane* coords)
{
    PS_PTR_CHECK_NULL(transform, NULL);
    PS_PTR_CHECK_NULL(transform->x, NULL);
    PS_PTR_CHECK_NULL(transform->y, NULL);
    PS_PTR_CHECK_NULL(coords, NULL);

    if (out == NULL) {
        out = (psPlane* ) psAlloc(sizeof(psPlane));
    }
    out->x = psDPolynomial2DEval(
                 transform->x,
                 coords->x,
                 coords->y
             );
    out->y = psDPolynomial2DEval(
                 transform->y,
                 coords->x,
                 coords->y
             );
    return (out);
}

static void planeDistortFree(psPlaneDistort *pt)
{
    psFree(pt->x);
    psFree(pt->y);
}

psPlaneDistort* psPlaneDistortAlloc(psS32 n1, psS32 n2, psS32 n3, psS32 n4)
{
    PS_INT_CHECK_NON_NEGATIVE(n1, NULL);
    PS_INT_CHECK_NON_NEGATIVE(n2, NULL);
    PS_INT_CHECK_NON_NEGATIVE(n3, NULL);
    PS_INT_CHECK_NON_NEGATIVE(n4, NULL);

    psPlaneDistort *pt = psAlloc(sizeof(psPlaneDistort));
    pt->x = psDPolynomial4DAlloc(n1, n2, n3, n4, PS_POLYNOMIAL_ORD);
    pt->y = psDPolynomial4DAlloc(n1, n2, n3, n4, PS_POLYNOMIAL_ORD);

    p_psMemSetDeallocator(pt, (psFreeFcn) planeDistortFree);
    return(pt);
}

/******************************************************************************
This transformation takes into account parameters beyond an objects spatial
coordinates: term3 and term4 (magnitude and color).
 *****************************************************************************/
psPlane* psPlaneDistortApply(psPlane* out,
                             const psPlaneDistort* transform,
                             const psPlane* coords,
                             float color,
                             float magnitude)
{
    PS_PTR_CHECK_NULL(transform, NULL);
    PS_PTR_CHECK_NULL(transform->x, NULL);
    PS_PTR_CHECK_NULL(transform->y, NULL);
    PS_PTR_CHECK_NULL(coords, NULL);

    if (out == NULL) {
        out = (psPlane* ) psAlloc(sizeof(psPlane));
    }
    out->x = psDPolynomial4DEval(
                 transform->x,
                 coords->x,
                 coords->y,
                 color,
                 magnitude
             );
    out->y = psDPolynomial4DEval(
                 transform->y,
                 coords->x,
                 coords->y,
                 color,
                 magnitude
             );
    return (out);
}

/******************************************************************************
alpha is LONGITUDE
delta is LATITUDE
 
    alphaP: Take the target pole in the source system; calculate its LONGITUDE
     in the target system.  That longitude is alphaP.
    DeltaP: Take the target pole in the source system; calculate its LATITUDE
     in the target system.  That longitude is deltaP.
    phiP:   This is the LONGITUDE of the ascending node in the target system.
 *****************************************************************************/
psSphereTransform* psSphereTransformAlloc(psF64 alphaP,
        psF64 deltaP,
        psF64 phiP)
{
    psSphereTransform* tmp = (psSphereTransform* ) psAlloc(sizeof(psSphereTransform));

    tmp->cosDeltaP = cos(deltaP);
    tmp->sinDeltaP = sin(deltaP);
    tmp->alphaP = alphaP;
    tmp->phiP = phiP;

    return (tmp);
}

/******************************************************************************
XXX: Private Function.
 
piNormalize(): take an input angle in radians and convert it to the range 0:2*PI.
 *****************************************************************************/
psF32 piNormalize(psF32 angle)
{
    while (angle < FLT_EPSILON) {
        angle+=PS_PI*2;
    }

    while (angle >= (PS_PI*2)) {
        angle-=PS_PI*2;
    }
    return(angle);
}

/******************************************************************************
XXX: We convert Right Ascension angles to the range 0:PI.  Is that acceptable?
XXX: Should we do something for Declination as well?
 *****************************************************************************/
psSphere* psSphereTransformApply(psSphere* out,
                                 const psSphereTransform* transform,
                                 const psSphere* coord)
{
    PS_PTR_CHECK_NULL(transform, NULL);
    PS_PTR_CHECK_NULL(coord, NULL);

    if (out == NULL) {
        out = (psSphere* ) psAlloc(sizeof(psSphere));
    }

    psF64 alpha = coord->r;
    psF64 delta = coord->d;
    psF64 alphaMinusAlphaP = alpha - transform->alphaP;

    psF64 eq55 = (sin(delta) * transform->cosDeltaP) -
                 (cos(delta) * transform->sinDeltaP * sin(alphaMinusAlphaP));
    psF64 eq56 = (cos(delta) * transform->cosDeltaP * sin(alphaMinusAlphaP)) +
                 (sin(delta) * transform->sinDeltaP);
    psF64 eq57 = cos(delta) * cos(alphaMinusAlphaP);

    psF64 theta = asin(eq55);
    psF64 phi = atan2(eq56, eq57) + transform->phiP;
    out->r = piNormalize(phi);
    out->d = theta;

    return(out);
}

psSphereTransform* psSphereTransformICRSToEcliptic(psTime *time)
{
    psF64 T;

    // Check for null parameter
    PS_PTR_CHECK_NULL(time, NULL);

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

    // Check the specified MJD is greater than 1900
    if ( MJD < MJD_1900 ) {
        psError(PS_ERR_BAD_PARAMETER_TYPE,true,PS_ERRORTEXT_psCoord_INVALID_MJD);
        return NULL;
    }

    // Calculate number of Julian centuries since 1900
    T = ( MJD - MJD_1900 ) / JULIAN_CENTURY;

    psF64 alphaP = 0.0;
    psF64 deltaP = DEG_TO_RAD(23.0) +
                   MIN_TO_RAD(27.0) +
                   SEC_TO_RAD(8.26) -
                   (SEC_TO_RAD(46.845) * T) -
                   (SEC_TO_RAD(0.0059) * T * T) +
                   (SEC_TO_RAD(0.00181) * T * T * T);
    psF64 phiP = 0.0;

    // Don't neglect the minus sign on deltaP (bug 244):
    return (psSphereTransformAlloc(alphaP, deltaP, phiP));
}


psSphereTransform* psSphereTransformEclipticToICRS(psTime *time)
{
    psF64 T;

    // Check for null parameter
    PS_PTR_CHECK_NULL(time, NULL);

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

    // Check the specified MJD is greater than 1900
    if ( MJD < MJD_1900 ) {
        psError(PS_ERR_BAD_PARAMETER_TYPE,true,PS_ERRORTEXT_psCoord_INVALID_MJD);
        return NULL;
    }

    // Calculate number of Julian centuries since 1900
    T = ( MJD - MJD_1900 ) / JULIAN_CENTURY;

    psF64 alphaP = 0.0;
    psF64 deltaP = DEG_TO_RAD(23.0) +
                   MIN_TO_RAD(27.0) +
                   SEC_TO_RAD(8.26) -
                   (SEC_TO_RAD(46.845) * T) -
                   (SEC_TO_RAD(0.0059) * T * T) +
                   (SEC_TO_RAD(0.00181) * T * T * T);
    psF64 phiP = 0.0;

    return (psSphereTransformAlloc(alphaP, -deltaP, phiP));
}

// XXX: This is bug 245: alphaP swaps with phiP from psSphereTransformGalacticToICRS()
psSphereTransform* psSphereTransformGalacticToICRS(void)
{
    psF64 alphaP = DEG_TO_RAD(32.93192);
    psF64 deltaP = DEG_TO_RAD(-62.87175);
    psF64 phiP = DEG_TO_RAD(282.85948);

    return (psSphereTransformAlloc(alphaP, deltaP, phiP));
}

psSphereTransform* psSphereTransformICRSToGalactic(void)
{
    psF64 alphaP = DEG_TO_RAD(282.85948);
    psF64 deltaP = DEG_TO_RAD(62.87175);
    psF64 phiP = DEG_TO_RAD(32.93192);

    return (psSphereTransformAlloc(alphaP, deltaP, phiP));
}

void projectionFree(psProjection *p)
{
    // There are no dynamically allocated items
}

psProjection* psProjectionAlloc(
    psF64 R,
    psF64 D,
    psF64 Xs,
    psF64 Ys,
    psProjectionType type)
{
    psProjection *p = psAlloc(sizeof(psProjection));
    p->D = D;
    p->R = R;
    p->Xs = Xs;
    p->Ys = Ys;
    p->type = type;

    p_psMemSetDeallocator(p, (psFreeFcn) projectionFree);
    return(p);
}

psPlane* psProject(const psSphere* coord,
                   const psProjection* projection)
{
    PS_PTR_CHECK_NULL(coord, NULL);
    PS_PTR_CHECK_NULL(projection, NULL);

    psF64   theta = 0.0;
    psF64   phi   = 0.0;

    // Allocate return value
    psPlane* out = psPlaneAlloc();

    // Convert to projection spherical coordinate system
    theta = asin( sin(coord->d)*sin(projection->D) +
                  cos(coord->d)*cos(projection->D)*cos(coord->r-projection->R));
    phi = atan2( -1.0*cos(coord->d)*sin(coord->r-projection->R),
                 sin(coord->d)*cos(projection->D) - cos(coord->d)*sin(projection->D)*cos(coord->r-projection->R) );

    // Perform the specified projection
    // Gnomonic projection
    if (projection->type == PS_PROJ_TAN) {
        out->x = (cos(theta)*sin(phi))/sin(theta);
        out->y = (-1.0*cos(theta)*cos(phi))/sin(theta);
        // Othrographic projection
    } else if (projection->type == PS_PROJ_SIN) {
        out->x = cos(theta)*sin(phi);
        out->y = -1.0*cos(theta)*cos(phi);
        // Hammer-Aitoff projection
    } else if ( projection->type == PS_PROJ_AIT) {
        psF64 zeta = 1.0/sqrt(0.5*(1.0+cos(theta)*cos(phi/2.0)));
        out->x = 2.0*zeta*cos(theta)*sin(phi/2.0);
        out->y = zeta*sin(theta);
        // Parabolic projection
    } else if ( projection->type == PS_PROJ_PAR) {
        out->x = phi*(2.0*cos(2.0*theta/3.0) - 1.0);
        out->y = PS_PI*sin(theta/3.0);
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psCoord_PROJECTION_TYPE_UNKNOWN,
                projection->type);
        psFree(out);
        return NULL;
    }

    // Apply plate scales
    out->x *= projection->Xs;
    out->y *= projection->Ys;

    // Return output
    return out;
}

psSphere* psDeproject(const psPlane* coord,
                      const psProjection* projection)
{
    PS_PTR_CHECK_NULL(coord, NULL);
    PS_PTR_CHECK_NULL(projection, NULL);

    psF64  theta = 0.0;
    psF64  phi   = 0.0;

    // Allocate return sphere structure
    psSphere* out = psSphereAlloc();

    // Remove plate scales
    psF64  x = coord->x/projection->Xs;
    psF64  y = coord->y/projection->Ys;

    // Perform inverse projection
    // Gnonomic deprojection
    if ( projection->type == PS_PROJ_TAN) {
        phi = atan(-1.0*x/y);
        theta = atan(1.0/sqrt(x*x+y*y));
        // Orhtographic deprojection
    } else if ( projection->type == PS_PROJ_SIN) {
        phi = atan((-1.0*x)/y);
        theta = atan( sqrt(1.0-(x*x+y*y)) / sqrt(x*x+y*y));
        // Hammer-Aitoff deprojection
    } else if ( projection->type == PS_PROJ_AIT) {
        psF64 z = sqrt(1.0 - ((x/4.0)*(x/4.0)) - ((y/2.0)*(y/2.0)));
        phi = 2.0*atan((z*x) / (2.0*(2.0*z*z-1.0)) );
        theta = asin(y*z);
        // Parabolic deprojection
    } else if ( projection->type == PS_PROJ_PAR) {
        psF64 rho = y/PS_PI;
        phi = x/(1.0 - 4.0*rho*rho);
        theta = 3.0*asin(rho);
        // Invalid deprojection type
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psCoord_PROJECTION_TYPE_UNKNOWN,
                projection->type);
        psFree(out);
        return NULL;
    }

    // Convert from projection spherical coordinates
    out->d = asin( sin(theta)*sin(projection->D) +
                   cos(theta)*cos(projection->D)*cos(phi) );
    out->r = projection->R + atan2( -1.0*cos(theta)*sin(phi),
                                    sin(theta)*cos(projection->D) -
                                    cos(theta)*sin(projection->D)*cos(phi) );

    // Return sphere coordinate
    return out;
}

/******************************************************************************
The basic idea is to project both positions onto the linear plane, with
position1 at the center, then calculate the linear offset between those
projections.
 
XXX: Do I need to check for unacceptable transformation parameters?  Maybe,
     if the points are on the North/South Pole, etc?
 
XXX: Do I need to somehow scale this projection?
 
XXX: Does PS_LINEAR mode make sense?  The result must be returned in psSphere
     regardless of the mode.
 
XXX: How to compound errors?
 *****************************************************************************/
psSphere* psSphereGetOffset(const psSphere* position1,
                            const psSphere* position2,
                            psSphereOffsetMode mode,
                            psSphereOffsetUnit unit)
{
    PS_PTR_CHECK_NULL(position1, NULL);
    PS_PTR_CHECK_NULL(position2, NULL);

    // Check positions near 90 degree and issue warnings if necessary
    if (position1->d >= DEG_TO_RAD(90.0)) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "WARNING: psDeproject(): position1->d is larger than 90 degrees.  Returning NULL.");
        return NULL;
    }
    if (position2->d >= DEG_TO_RAD(90.0)) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "WARNING: psDeproject(): position2->d is larger than 90 degrees.  Returning NULL.");
        return NULL;
    }

    // Allocate return structure
    psSphere* tmp = psSphereAlloc();

    // Mode is LINEAR - Use first position as projection center and project second point
    // onto tangent plane, set point projected into psSphere structure x->r y->d
    if (mode == PS_LINEAR) {
        psProjection* proj = psProjectionAlloc(position1->r,
                                               position1->d,
                                               1.0,
                                               1.0,
                                               PS_PROJ_TAN);

        // Perform projection onto tangent plane
        psPlane* lin = psProject(position2, proj);

        // Set return values
        tmp->r = lin->x;
        tmp->d = lin->y;

        // Free data structures allocated
        psFree(proj);
        psFree(lin);

        // Mode is SPHERICAL - Get difference between positiion 1 and position 2 and convert
        // offset value from radians to desired units and return
    } else if (mode == PS_SPHERICAL) {
        tmp->r = position2->r - position1->r;
        tmp->d = position2->d - position1->d;

        // Wrap these to an acceptable range.  This assumes that all
        // angles are in radians.
        tmp->r = fmod(tmp->r, 2*PS_PI);
        tmp->d = fmod(tmp->d, 2*PS_PI);
        tmp->rErr = 0.0;
        tmp->dErr = 0.0;

        // Convert to desired units
        if (unit == PS_ARCSEC) {
            tmp->r = RAD_TO_SEC(tmp->r);
            tmp->d = RAD_TO_SEC(tmp->d);
        } else if (unit == PS_ARCMIN) {
            tmp->r = RAD_TO_MIN(tmp->r);
            tmp->d = RAD_TO_MIN(tmp->d);
        } else if (unit == PS_DEGREE) {
            tmp->r = RAD_TO_DEG(tmp->r);
            tmp->d = RAD_TO_DEG(tmp->d);
        } else if (unit == PS_RADIAN) {}
        else {
            psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                    PS_ERRORTEXT_psCoord_UNITS_UNKNOWN,
                    unit);
            psFree(tmp);
            return NULL;
        }
        // Invalid mode
    } else {

        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                PS_ERRORTEXT_psCoord_OFFSET_MODE_UNKNOWN,
                mode);
        psFree(tmp);
        return NULL;
    }

    // Return value
    return tmp;
}

/******************************************************************************
XXX: Do we need to check for unacceptable transformation parameters?  Maybe,
     if the points are on the North/South Pole, etc?
 
XXX: Do we need to somehow scale this projection?
 
XXX: I copied the algorithm from the ADD exactly.
 
XXX: Should we compound errors?
 *****************************************************************************/

psSphere* psSphereSetOffset(const psSphere* position,
                            const psSphere* offset,
                            psSphereOffsetMode mode,
                            psSphereOffsetUnit unit)
{
    PS_PTR_CHECK_NULL(position, NULL);
    PS_PTR_CHECK_NULL(offset, NULL);

    psSphere* tmp;
    psF64 tmpR = 0.0;
    psF64 tmpD = 0.0;

    // If mode is linear then set position to projection center
    // and offset to linear coordinate then deproject to obtain
    // new sphere coordinate
    if (mode == PS_LINEAR) {

        // Allocate plane coordinate and set coordinate
        psPlane*  lin = psPlaneAlloc();
        lin->x = offset->r;
        lin->y = offset->d;

        // Allocate and set projection structure
        psProjection* proj = psProjectionAlloc(position->r,
                                               position->d,
                                               1.0,
                                               1.0,
                                               PS_PROJ_TAN);

        // Project tangent plane coord to spherical coord
        tmp = psDeproject(lin, proj);

        // Free data structures used
        psFree(proj);
        psFree(lin);

        // If mode is spherical then convert offset to radians, add the offset
        // to the position and wrap to 0 to 2pi
    } else if (mode == PS_SPHERICAL) {

        // Convert offset unit to radians
        if (unit == PS_ARCSEC) {
            tmpR = SEC_TO_RAD(offset->r);
            tmpD = SEC_TO_RAD(offset->d);
        } else if (unit == PS_ARCMIN) {
            tmpR = MIN_TO_RAD(offset->r);
            tmpD = MIN_TO_RAD(offset->d);
        } else if (unit == PS_DEGREE) {
            tmpR = DEG_TO_RAD(offset->r);
            tmpD = DEG_TO_RAD(offset->d);
        } else if (unit == PS_RADIAN) {
            tmpR = offset->r;
            tmpD = offset->d;
        } else {
            psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                    PS_ERRORTEXT_psCoord_UNITS_UNKNOWN,
                    unit);
            return NULL;
        }

        // Allocate sphere structure to return
        tmp = psSphereAlloc();

        // Add offset and wrap to 0 to 2PI if necessary
        tmp->r = position->r + tmpR;
        tmp->r = fmod(tmp->r, 2.0*PS_PI);
        tmp->d = position->d + tmpD;
        tmp->d = fmod(tmp->d, 2.0*PS_PI);
        tmp->rErr = 0.0;
        tmp->dErr = 0.0;

        // Invalid mode report error
    } else {

        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                PS_ERRORTEXT_psCoord_OFFSET_MODE_UNKNOWN,
                mode);
        return NULL;
    }

    return tmp;
}



/******************************************************************************
psSpherePrecess(coords, fromTime, toTime):
 
XXX: Use static memory for tmpST.
 *****************************************************************************/
psSphere *psSpherePrecess(psSphere *coords,
                          const psTime *fromTime,
                          const psTime *toTime)
{
    // Check input for NULL pointers
    PS_PTR_CHECK_NULL(coords, NULL);
    PS_PTR_CHECK_NULL(fromTime, NULL);
    PS_PTR_CHECK_NULL(toTime, NULL);

    // Calculate Julian centuries
    psF64 fromMJD = psTimeToMJD(fromTime);
    psF64 toMJD = psTimeToMJD(toTime);
    psF64 T = (toMJD - fromMJD) / JULIAN_CENTURY;

    // Calculate conversion constants
    psF64 alphaP = DEG_TO_RAD(90.0) - ((DEG_TO_RAD(0.6406161) * T) +
                                       (DEG_TO_RAD(0.0000839) * T * T) +
                                       (DEG_TO_RAD(0.000005) * T * T * T));

    psF64 deltaP = (DEG_TO_RAD(0.5567530) * T) -
                   (DEG_TO_RAD(0.0001185) * T * T) -
                   (DEG_TO_RAD(0.0000116) * T * T * T);

    psF64 phiP = DEG_TO_RAD(90.0) + ((DEG_TO_RAD(0.6406161) * T) +
                                     (DEG_TO_RAD(0.0003041) * T * T) +
                                     (DEG_TO_RAD(0.0000051) * T * T * T));

    // Create transform with proper constants
    psSphereTransform *tmpST = psSphereTransformAlloc(alphaP, deltaP, phiP);

    // Apply transform to coordinates
    psSphere *out = psSphereTransformApply(NULL, tmpST, coords);

    psFree(tmpST);

    return(out);
}



/*****************************************************************************
psPlaneTransformCombine(out, trans1, trans2)
 
XXX: Much room for optimization.  Currently, we call the polyMultiply
routine far too many times.
 *****************************************************************************/
psPlaneTransform *psPlaneTransformCombine(psPlaneTransform *out,
        const psPlaneTransform *trans1,
        const psPlaneTransform *trans2)
{
    PS_PTR_CHECK_NULL(trans1, NULL);
    PS_PTR_CHECK_NULL(trans2, NULL);

    //
    // Determine the size of the new psPlaneTransform.
    //
    // PS_MAX(  Number of x terms in T2->x * number of x terms in T1->x,
    //          Number of y terms in T2->x * number of x terms in T1->y,
    psS32 orderXnX = PS_MAX((trans2->x->nX * trans1->x->nX),
                            (trans2->x->nY * trans1->y->nX));
    psS32 orderXnY = PS_MAX((trans2->x->nX * trans1->x->nY),
                            (trans2->x->nY * trans1->y->nY));

    psS32 orderYnX = PS_MAX((trans2->y->nX * trans1->x->nX),
                            (trans2->y->nY * trans1->y->nX));
    psS32 orderYnY = PS_MAX((trans2->y->nX * trans1->x->nY),
                            (trans2->y->nY * trans1->y->nY));
    psS32 orderX = PS_MAX(orderXnX, orderYnX);
    psS32 orderY = PS_MAX(orderXnY, orderYnY);

    //
    // Allocate the new psPlaneTransform, if necessary.
    //
    psPlaneTransform *myPT = NULL;
    if (out == NULL) {
        myPT = psPlaneTransformAlloc(orderX, orderY);
    } else {
        if ((out->x->nX == orderX) && (out->x->nY == orderY) &&
                (out->y->nX == orderX) && (out->y->nY == orderY)) {
            myPT = out;
        } else {
            psFree(out);
            myPT = psPlaneTransformAlloc(orderX, orderY);
        }
    }

    //
    // Initialize the new psPlaneTransform, if necessary.
    //
    for (psS32 i = 0 ; i < orderX ; i++) {
        for (psS32 j = 0 ; j < orderY ; j++) {
            myPT->x->coeff[i][j] = 0.0;
            myPT->x->mask[i][j] = 0;
            myPT->y->coeff[i][j] = 0.0;
            myPT->y->mask[i][j] = 0;
        }
    }

    //
    // For each term (a * x^i * y^j) in trans2, we substitute the appropriate
    // equation from trans1, and raise it to the appropriate power.  This is
    // done via the multiplyDPoly2D().  The result is a polynomial (currPoly)
    // and its coefficients are added into the myPT coeff matrix.
    //
    // XXX: This is horribly inefficient in that the trans1 polys are repeatedly
    // multiplied against themselves.  This can easily be improved.
    //
    for (psS32 t2x = 0 ; t2x < trans2->x->nX ; t2x++) {
        for (psS32 t2y = 0 ; t2y < trans2->x->nY ; t2y++) {
            psDPolynomial2D *currPoly = psDPolynomial2DAlloc(1, 1, PS_POLYNOMIAL_ORD);
            currPoly->coeff[0][0] = 1.0;
            currPoly->mask[0][0] = 0;
            psDPolynomial2D *newPoly = NULL;

            if (trans2->x->mask[t2x][t2y] == 0) {

                // Must raise trans1->y to the t2y-power.
                for (psS32 c = 0 ; c < t2y; c++) {
                    newPoly = multiplyDPoly2D(currPoly, trans1->y);
                    psFree(currPoly);
                    currPoly = newPoly;
                }

                // Must raise trans1->x to the t2x-power.
                for (psS32 c = 0 ; c < t2x; c++) {
                    newPoly = multiplyDPoly2D(currPoly, trans1->x);
                    psFree(currPoly);
                    currPoly = newPoly;
                }

                // Set the appropriate coeffs in myPT->x
                for (psS32 i = 0 ; i < currPoly->nX ; i++) {
                    for (psS32 j = 0 ; j < currPoly->nY ; j++) {
                        myPT->x->coeff[i][j]+= currPoly->coeff[i][j] * trans2->x->coeff[t2x][t2y];
                    }
                }
            }
            psFree(currPoly);
        }
    }



    for (psS32 t2x = 0 ; t2x < trans2->y->nX ; t2x++) {
        for (psS32 t2y = 0 ; t2y < trans2->y->nY ; t2y++) {
            psDPolynomial2D *currPoly = psDPolynomial2DAlloc(1, 1, PS_POLYNOMIAL_ORD);
            currPoly->coeff[0][0] = 1.0;
            currPoly->mask[0][0] = 0;
            psDPolynomial2D *newPoly = NULL;

            if (trans2->y->mask[t2x][t2y] == 0) {

                // Must raise trans1->y to the t2y-power.
                for (psS32 c = 0 ; c < t2y; c++) {
                    newPoly = multiplyDPoly2D(currPoly, trans1->y);
                    psFree(currPoly);
                    currPoly = newPoly;
                }

                // Must raise trans1->x to the t2x-power.
                for (psS32 c = 0 ; c < t2x; c++) {
                    newPoly = multiplyDPoly2D(currPoly, trans1->x);
                    psFree(currPoly);
                    currPoly = newPoly;
                }

                // Set the appropriate coeffs in myPT->x
                for (psS32 i = 0 ; i < currPoly->nX ; i++) {
                    for (psS32 j = 0 ; j < currPoly->nY ; j++) {
                        myPT->y->coeff[i][j]+= currPoly->coeff[i][j] * trans2->y->coeff[t2x][t2y];
                    }
                }
            }
            psFree(currPoly);
        }
    }

    return(myPT);
}

/*****************************************************************************
psPlaneTransformFit(trans, source, dest, nRejIter, sigmaClip)
 
XXX: What about nRejIter?  Iterations?
XXX: Use static vectors for internal data.
 *****************************************************************************/
bool psPlaneTransformFit(psPlaneTransform *trans,
                         const psArray *source,
                         const psArray *dest,
                         int nRejIter,
                         float sigmaClip)
{
    PS_PTR_CHECK_NULL(trans, NULL);
    PS_PTR_CHECK_NULL(source, NULL);
    PS_PTR_CHECK_NULL(dest, NULL);

    psS32 numCoords = PS_MIN(source->n, dest->n);
    // This is not really necessary because of above conditionals.
    psS32 order = PS_MAX(trans->x->nX, trans->x->nY);

    //
    // Create fake polynomial to use in evaluation
    //
    psDPolynomial2D *fakePoly = psDPolynomial2DAlloc(order, order, PS_POLYNOMIAL_ORD);
    for (int i = 0; i < order; i++) {
        for (int j = 0; j < order; j++) {
            fakePoly->coeff[i][j] = 1.0;
            fakePoly->mask[i][j] = 1;       // Mask all coefficients; unmask to evaluate
        }
    }

    //
    // Initialize the matrix and vectors
    //
    psS32 nCoeff = order * (order + 1) / 2; // Number of polynomial coefficients
    psImage *matrix = psImageAlloc(nCoeff, nCoeff, PS_TYPE_F64); // Matrix for solution
    psVector *xVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in x
    psVector *yVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in y
    for (psS32 i = 0; i < nCoeff; i++) {
        for (psS32 j = 0; j < nCoeff; j++) {
            matrix->data.F64[i][j] = 0.0;
        }
        xVector->data.F64[i] = 0.0;
        yVector->data.F64[i] = 0.0;
    }

    //
    // Iterate over the grid points
    //
    for (psS32 g = 0; g < numCoords; g++) {
        // Iterate over the polynomial coefficients, accumulating the matrix and vectors

        for (psS32 i = 0, ijIndex = 0; i < order; i++) {
            for (psS32 j = 0; j < order - i; j++, ijIndex++) {
                fakePoly->mask[i][j] = 0;
                psF64 xIn = ((psPlane *) source->data[g])->x;
                psF64 yIn = ((psPlane *) source->data[g])->y;
                psF64 xOut = ((psPlane *) dest->data[g])->x;
                psF64 yOut = ((psPlane *) dest->data[g])->y;
                psF64 ijPoly = psDPolynomial2DEval(fakePoly, xIn, yIn);
                fakePoly->mask[i][j] = 1;

                for (psS32 m = 0, mnIndex = 0; m < order; m++) {
                    for (psS32 n = 0; n < order - m; n++, mnIndex++) {
                        fakePoly->mask[m][n] = 0;
                        psF64 mnPoly = psDPolynomial2DEval(fakePoly, xIn, yIn);
                        fakePoly->mask[m][n] = 1;

                        matrix->data.F64[ijIndex][mnIndex] += ijPoly * mnPoly;
                    }
                }

                xVector->data.F64[ijIndex] += ijPoly * xOut;
                yVector->data.F64[ijIndex] += ijPoly * yOut;
            }
        }
    }

    //
    // Solution via LU Decomposition
    //
    psVector *permutation = psVectorAlloc(nCoeff, PS_TYPE_F64); // Permutation vector for LU Decomposition
    psImage *luMatrix = psMatrixLUD(NULL, &permutation, matrix); // LU decomposed matrix
    psVector *xSolution = psMatrixLUSolve(NULL, luMatrix, xVector, permutation); // Solution in x
    psVector *ySolution = psMatrixLUSolve(NULL, luMatrix, yVector, permutation); // Solution in y

    //
    // XXX: Should check the output of the matrix routines and return false if bad.
    //

    //
    // Stuff coefficients into transformation
    //
    for (psS32 i = 0, ijIndex = 0; i < order; i++) {
        for (psS32 j = 0; j < order - i; j++, ijIndex++) {
            trans->x->coeff[i][j] = xSolution->data.F64[ijIndex];
            trans->y->coeff[i][j] = ySolution->data.F64[ijIndex];
        }
    }

    psFree(fakePoly);
    psFree(permutation);
    psFree(luMatrix);
    psFree(xSolution);
    psFree(ySolution);
    psFree(matrix);
    psFree(xVector);
    psFree(yVector);

    return(true);
}


/*****************************************************************************
psPlaneTransformInvert(out, in, region, nSamples)
 
// XXX: Use static data structures.
 *****************************************************************************/
psPlaneTransform *psPlaneTransformInvert(psPlaneTransform *out,
        const psPlaneTransform *in,
        psRegion *region,
        int nSamples)
{
    PS_PTR_CHECK_NULL(in, NULL);
    //
    // If the transform is linear, then invert it exactly and return.
    //
    if (p_psIsProjectionLinear((psPlaneTransform *) in)) {
        printf("COOL: is linear\n");
        return(p_psPlaneTransformLinearInvert((psPlaneTransform *) in));
    }
    PS_PTR_CHECK_NULL(region, NULL);
    PS_INT_COMPARE(1, nSamples, NULL);

    // Ensure that the input transformation is symmetrical.
    if ((in->x->nX != in->x->nY) ||
            (in->y->nX != in->y->nY) ||
            (in->x->nX != in->y->nX)) {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Input transformation must have same nX==nY.");
    }
    psS32 order = PS_MAX(in->x->nX, in->x->nY);

    psPlaneTransform *myPT = NULL;
    psPlane *inCoord = psPlaneAlloc();
    psPlane *outCoord = psPlaneAlloc();

    //
    // Allocate a new psPlaneTransform if "out" is NULL, or has the wrong size.
    //
    if (out == NULL) {
        myPT = psPlaneTransformAlloc(order, order);
    } else {
        if ((out->x->nX == order) && (out->x->nY == order) &&
                (out->y->nX == order) && (out->y->nY == order)) {
            myPT = out;
        } else {
            psFree(out);
            myPT = psPlaneTransformAlloc(order, order);
        }
    }

    //
    // Copy the input transform to myPT.
    //
    for (psS32 i = 0 ; i < in->x->nX ; i++) {
        for (psS32 j = 0 ; j < in->x->nY ; j++) {
            myPT->x->coeff[i][j] = in->x->coeff[i][j];
        }
    }
    for (psS32 i = 0 ; i < in->y->nX ; i++) {
        for (psS32 j = 0 ; j < in->y->nY ; j++) {
            myPT->y->coeff[i][j] = in->y->coeff[i][j];
        }
    }

    //
    // Create a grid of xin,yin --> xout,yout
    //
    psArray *inData = psArrayAlloc(nSamples * nSamples);
    psArray *outData = psArrayAlloc(nSamples * nSamples);
    for (psS32 i = 0 ; i < inData->n; i++) {
        inData->data[i] = (psPtr *) psPlaneAlloc();
        outData->data[i] = (psPtr *) psPlaneAlloc();
    }

    //
    // Initialize the grid.
    //
    psS32 cnt = 0;
    for (int yint = 0; yint < nSamples; yint++) {
        inCoord->y = region->y0 + ((psF32) yint) * ((region->y1 - region->y0) / ((psF32) nSamples));
        for (int xint = 0; xint < nSamples; xint++) {
            inCoord->x = region->x0 + ((psF32) xint) * ((region->x1 - region->x0) / ((psF32) nSamples));
            (void)psPlaneTransformApply(outCoord, in, inCoord);

            ((psPlane *) outData->data[cnt])->x = inCoord->x;
            ((psPlane *) outData->data[cnt])->y = inCoord->y;
            ((psPlane *) inData->data[cnt])->x = outCoord->x;
            ((psPlane *) inData->data[cnt])->y = outCoord->y;

            cnt++;
        }
    }
    bool rc = psPlaneTransformFit(myPT, inData, outData, 10, 100.0);

    psFree(inCoord);
    psFree(outCoord);
    psFree(inData);
    psFree(outData);

    if (rc == true) {
        return(myPT);
    }

    // XXX: Generate an error message, or warning message.
    return(NULL);
}
