/** @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.88.4.2 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-11-21 19:04:02 $
*
*  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 "psErrorText.h"
#include "psMatrix.h"
#include <math.h>
#include <float.h>


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

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

static void cubeFree(psCube* c)
{
    // There are non dynamic allocated items
}

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

/*****************************************************************************
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 there is now a general psPlaneTransformInvert() function, we
should rename this.
 
XXX: Use the ADD version which is based on determinants.
 *****************************************************************************/
psPlaneTransform *p_psPlaneTransformLinearInvert_MHPCC(psPlaneTransform *transform)
{
    PS_ASSERT_PTR_NON_NULL(transform, 0);
    PS_ASSERT_PTR_NON_NULL(transform->x, 0);
    PS_ASSERT_PTR_NON_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;

    A = transform->x->coeff[0][0];
    if (transform->x->nX >= 1) {
        B = transform->x->coeff[1][0];
    }
    if (transform->x->nY >= 1) {
        C = transform->x->coeff[0][1];
    }
    D = transform->y->coeff[0][0];
    if (transform->y->nX >= 1) {
        E = transform->y->coeff[1][0];
    }
    if (transform->y->nY >= 1) {
        F = transform->y->coeff[0][1];
    }

    psPlaneTransform *out = psPlaneTransformAlloc(1, 1);

    /* This is sample code from IfA.  It didn't work initially, and I did not
       spend any time debugging it.
       XXX EAM : here is the correct matrix inversion code

        psF64 r11 = transform->x->coeff[1][0];
        psF64 r12 = transform->x->coeff[0][1];
        psF64 r21 = transform->y->coeff[1][0];
        psF64 r22 = transform->y->coeff[0][1];
        psF64 xo  = transform->x->coeff[0][0];
        psF64 yo  = transform->y->coeff[0][0];

        psF64 invDet = 1.0 / (r11 * r22 - r12 * r21); // Inverse of the determinant

        out->x->coeff[1][0] = +invDet * r22;
        out->x->coeff[0][1] = -invDet * r12;
        out->y->coeff[1][0] = -invDet * r21;
        out->y->coeff[0][1] = +invDet * r11;

        out->x->coeff[0][0] = - invDet * (r22 * xo - r12 * yo);
        out->y->coeff[0][0] = - invDet * (r11 * yo - r21 * xo);
    */
    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);
}

// XXX EAM : the above code yielded NaNs for the y coeffs.  below is the code
//           using the standard matrix representation.  note that this inversion
//           requires x->nX == 1, y->nY == 1 and x->nY <= 1, y->nX <= 1
psPlaneTransform *p_psPlaneTransformLinearInvert(psPlaneTransform *transform)
{
    PS_ASSERT_PTR_NON_NULL(transform, 0);
    PS_ASSERT_PTR_NON_NULL(transform->x, 0);
    PS_ASSERT_PTR_NON_NULL(transform->y, 0);

    if (transform->x->nX != 1)
        return NULL;
    if (transform->y->nY != 1)
        return NULL;
    if (transform->x->nY > 1)
        return NULL;
    if (transform->y->nX > 1)
        return NULL;

    // this choice is consistent with the nOrder form for the polynomials
    psPlaneTransform *out = psPlaneTransformAlloc(1, 1);

    // unless the cross terms are available, set these matrix elements to 0
    psF64 r12 = 0.0;
    if (transform->x->nY == 1) {
        r12 = transform->x->coeff[0][1];
    }
    psF64 r21 = 0.0;
    if (transform->y->nX == 1) {
        r21 = transform->y->coeff[1][0];
    }
    psF64 r11 = transform->x->coeff[1][0];
    psF64 r22 = transform->y->coeff[0][1];
    psF64 xo  = transform->x->coeff[0][0];
    psF64 yo  = transform->y->coeff[0][0];

    psF64 invDet = 1.0 / (r11 * r22 - r12 * r21);

    // apply the results back to the polynomials
    out->x->coeff[0][0] = -invDet * (r22 * xo - r12 * yo);
    out->y->coeff[0][0] = -invDet * (r11 * yo - r21 * xo);
    out->x->coeff[1][0] = +invDet * r22;
    out->y->coeff[0][1] = +invDet * r11;
    if (transform->x->nY == 1) {
        out->x->coeff[0][1] = -invDet * r12;
    }
    if (transform->y->nX == 1) {
        out->y->coeff[1][0] = -invDet * r21;
    }
    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.
 
Returns:
    1: if linear
    0: otherwise
 
XXX: This should be a psBool
 *****************************************************************************/
psS32 p_psIsProjectionLinear(psPlaneTransform *transform)
{
    PS_ASSERT_PTR_NON_NULL(transform, 0);
    PS_ASSERT_PTR_NON_NULL(transform->x, 0);
    PS_ASSERT_PTR_NON_NULL(transform->y, 0);

    for (psS32 i=0;i<(1 + transform->x->nX);i++) {
        for (psS32 j=0;j<(1 + 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<(1 + transform->y->nX);i++) {
        for (psS32 j=0;j<(1 + 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));
    psMemSetDeallocator(p, (psFreeFunc) planeFree);

    return(p);
}


psSphere* psSphereAlloc(void)
{
    psSphere *s = psAlloc(sizeof(psSphere));
    psMemSetDeallocator(s, (psFreeFunc) sphereFree);

    return(s);
}

psCube* psCubeAlloc(void)
{
    psCube *c = psAlloc(sizeof(psCube));

    psMemSetDeallocator(c, (psFreeFunc) cubeFree);
    return(c);
}

// XXX: Verify the order/nterms poly changes
psPlaneTransform* psPlaneTransformAlloc(int n1, int n2)
{
    PS_ASSERT_INT_NONNEGATIVE(n1, NULL);
    PS_ASSERT_INT_NONNEGATIVE(n2, NULL);

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

    psMemSetDeallocator(pt, (psFreeFunc) planeTransformFree);
    return(pt);
}


bool psMemCheckPlane(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)planeFree );
}

bool psMemCheckSphere(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)sphereFree );
}

bool psMemCheckCube(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)cubeFree );
}

bool psMemCheckPlaneTransform(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)planeTransformFree );
}

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

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

    out->x = psPolynomial2DEval(
                 transform->x,
                 coords->x,
                 coords->y
             );

    out->y = psPolynomial2DEval(
                 transform->y,
                 coords->x,
                 coords->y
             );

    return (out);
}

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

// XXX: Verify the order/nterms poly changes
psPlaneDistort* psPlaneDistortAlloc(int n1, int n2, int n3, int n4)
{
    PS_ASSERT_INT_NONNEGATIVE(n1, NULL);
    PS_ASSERT_INT_NONNEGATIVE(n2, NULL);
    PS_ASSERT_INT_NONNEGATIVE(n3, NULL);
    PS_ASSERT_INT_NONNEGATIVE(n4, NULL);

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

    psMemSetDeallocator(pt, (psFreeFunc) planeDistortFree);
    return(pt);
}

bool psMemCheckPlaneDistort(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)planeDistortFree );
}



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

    if (out == NULL) {
        out = (psPlane* ) psAlloc(sizeof(psPlane));
    }
    out->x = psPolynomial4DEval(
                 distort->x,
                 coords->x,
                 coords->y,
                 mag,
                 color
             );
    out->y = psPolynomial4DEval(
                 distort->y,
                 coords->x,
                 coords->y,
                 mag,
                 color
             );
    return (out);
}


/******************************************************************************
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+=M_PI*2;
    }

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

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

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

    psMemSetDeallocator(p, (psFreeFunc) projectionFree);
    return(p);
}

bool psMemCheckProjection(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)projectionFree );
}



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

    psF64 phi, theta;
    psF64 sinDp, cosDp, sinAlpha, cosAlpha, sinDelta, cosDelta;
    psF64 sinTheta, cosPhiCT, sinPhiCT, zeta;

    bool zenithal = (projection->type == PS_PROJ_TAN) ||(projection->type == PS_PROJ_SIN);

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

    if (zenithal) {
        sinDp = sin(projection->D);
        cosDp = cos(projection->D);
        sinAlpha = sin(coord->r-projection->R);
        cosAlpha = cos(coord->r-projection->R);
        sinDelta = sin(coord->d);
        cosDelta = cos(coord->d);

        sinTheta =  sinDelta*sinDp + cosDelta*cosDp*cosAlpha;
        cosPhiCT =  sinDelta*cosDp - cosDelta*sinDp*cosAlpha;
        sinPhiCT = -cosDelta*sinAlpha;
    } else {
        phi = coord->r - projection->R;
        theta = coord->d - projection->D;
    }

    // Perform the specified projection
    switch (projection->type) {
    case PS_PROJ_TAN:
        // Gnomonic projection
        out->x = +sinPhiCT / sinTheta;
        out->y = -cosPhiCT / sinTheta;
        break;
    case PS_PROJ_SIN:
        // Othrographic projection
        out->x = +sinPhiCT;
        out->y = -cosPhiCT;
        break;
    case PS_PROJ_AIT:
        // Hammer-Aitoff projection
        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);
        break;
    case PS_PROJ_PAR:
        // Parabolic projection
        out->x = phi*(2.0*cos(2.0*theta/3.0) - 1.0);
        out->y = M_PI*sin(theta/3.0);
    default:
        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_ASSERT_PTR_NON_NULL(coord, NULL);
    PS_ASSERT_PTR_NON_NULL(projection, NULL);

    psF64 rho      = 0.0;
    psF64 sinTheta = 0.0;
    psF64 cosTheta = 0.0;
    psF64 sinPhi   = 0.0;
    psF64 cosPhi   = 0.0;

    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;
    psF64  R = sqrt(x*x + y*y);

    bool zenithal = (projection->type == PS_PROJ_TAN) ||(projection->type == PS_PROJ_SIN);

    // Perform inverse projection
    switch (projection->type) {
    case PS_PROJ_TAN:
        // Gnonomic deprojection
        rho      = sqrt (1 + R*R);
        sinTheta = 1 / rho;
        cosTheta = R / rho;
        sinPhi   = (R == 0) ? 0.0 : +x / R;
        cosPhi   = (R == 0) ? 1.0 : -y / R;
        break;
    case PS_PROJ_SIN:
        // Orhtographic deprojection
        sinTheta = sqrt (1 - R*R);
        cosTheta = R;
        sinPhi   = (R == 0) ? 0.0 : +x / R;
        cosPhi   = (R == 0) ? 1.0 : -y / R;
        break;
    case PS_PROJ_AIT:
        // Hammer-Aitoff deprojection
        // XXX EAM : need range check on z^2 : must be > 0
        // XXX EAM : old code, ADD, and elixir code are discrepant re x/4, y/2
        rho = sqrt(1.0 - PS_SQR(x/4.0) - PS_SQR(y/2.0));
        phi = 2.0*atan2((2.0*rho*rho-1.0), x*rho);
        theta = asin(y*rho);
        break;
    case PS_PROJ_PAR:
        // Parabolic deprojection
        rho = y/M_PI;
        phi = x/(1.0 - 4.0*rho*rho);
        theta = 3.0*asin(rho);
        break;
    default:
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psCoord_PROJECTION_TYPE_UNKNOWN,
                projection->type);
        psFree(out);
        return NULL;
    }

    if (zenithal) {
        psF64 sinDp = sin(projection->D);
        psF64 cosDp = cos(projection->D);

        // Convert from projection spherical coordinates
        psF64 delta = asin(sinTheta*sinDp + cosTheta*cosDp*cosPhi);
        psF64 sinAlphaF = -cosTheta*sinPhi;
        psF64 cosAlphaF = -cosTheta*cosPhi*sinDp + sinTheta*cosDp;

        out->d = delta;
        out->r = atan2(sinAlphaF, cosAlphaF) + projection->R;
    } else {
        out->r = phi   + projection->R;
        out->d = theta + projection->D;
    }

    // Return sphere coordinate
    return out;
}

/*****************************************************************************
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.
 *****************************************************************************/

static psPolynomial2D *multiplyDPoly2D(psPolynomial2D *trans1,
                                       psPolynomial2D *trans2)
{
    //TRACE: printf("multiplyDPoly2D(%d %d: %d %d)\n", trans1->nX, trans1->nY, trans2->nX, trans2->nY);
    // XXX: OLD:  psS32 orderX = (trans1->nX + trans2->nX) - 1;
    // XXX: Verify this (the poly order change)
    psS32 orderX = trans1->nX + trans2->nX + 1;
    psS32 orderY = trans1->nY + trans2->nY + 1;

    // XXX: Verify this (the poly order change)
    psPolynomial2D *out = psPolynomial2DAlloc(orderX-1, orderY-1, PS_POLYNOMIAL_ORD);
    //TRACE: printf("Creating poly (%d, %d)\n", orderX, orderY);
    for (psS32 i = 0 ; i < (1 + out->nX); i++) {
        for (psS32 j = 0 ; j < (1 + out->nY); j++) {
            out->coeff[i][j] = 0.0;
            out->mask[i][j] = 0;
        }
    }

    for (psS32 t1x = 0 ; t1x < (1 + trans1->nX) ; t1x++) {
        for (psS32 t1y = 0 ; t1y < (1 + trans1->nY) ; t1y++) {
            if (0.0 != trans1->coeff[t1x][t1y]) {
                for (psS32 t2x = 0 ; t2x < (1 + trans2->nX) ; t2x++) {
                    for (psS32 t2y = 0 ; t2y < (1 + trans2->nY) ; t2y++) {
                        /* Possible debug-only macro which checks these coords?
                        if ((t1x+t2x) >= orderX)
                            printf("BAD 1\n");
                        if ((t1y+t2y) >= orderY)
                            printf("BAD 2\n");
                        */
                        out->coeff[t1x+t2x][t1y+t2y]+= (trans1->coeff[t1x][t1y] * trans2->coeff[t2x][t2y]);
                    }
                }
            }
        }
    }
    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,
        psRegion region,
        int nSamples)
{
    PS_ASSERT_PTR_NON_NULL(trans1, NULL);
    PS_ASSERT_PTR_NON_NULL(trans2, NULL);
    //TRACE: printf("psPlaneTransformCombine(%d, %d, %d, %d: %d, %d, %d, %d)\n", trans1->x->nX, trans1->x->nY, trans1->y->nX, trans1->y->nY, trans2->x->COOL_2D_nX, trans2->x->COOL_2D_nY, trans2->y->COOL_2D_nX, trans2->y->COOL_2D_nY);
    //
    // 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(((1 + trans2->x->nX) * (1 + trans1->x->nX)),
                            ((1 + trans2->x->nY) * (1 + trans1->y->nX)));
    psS32 orderXnY = PS_MAX(((1 + trans2->x->nX) * (1 + trans1->x->nY)),
                            ((1 + trans2->x->nY) * (1 + trans1->y->nY)));

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

    //
    // Allocate the new psPlaneTransform, if necessary.
    //
    // XXX: rename, or verify, or recode, after the poly norder/nterm change.
    psPlaneTransform *myPT = NULL;
    if (out == NULL) {
        myPT = psPlaneTransformAlloc(orderX, orderY);
    } else {
        if (((1 + out->x->nX) == orderX) && ((1 + out->x->nY) == orderY) &&
                ((1 + out->y->nX) == orderX) && ((1 + 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 < (1 + trans2->x->nX) ; t2x++) {
        for (psS32 t2y = 0 ; t2y < (1 + trans2->x->nX) ; t2y++) {
            // XXX: Verify this (poly order/nterm change)
            psPolynomial2D *currPoly = psPolynomial2DAlloc(1-1, 1-1, PS_POLYNOMIAL_ORD);

            currPoly->coeff[0][0] = 1.0;
            currPoly->mask[0][0] = 0;
            psPolynomial2D *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 < (1 + currPoly->nX) ; i++) {
                    for (psS32 j = 0 ; j < (1 + currPoly->nY) ; j++) {
                        myPT->x->coeff[i][j]+= currPoly->coeff[i][j] * trans2->x->coeff[t2x][t2y];
                    }
                }
            }
            psFree(currPoly);
        }
    }


    for (psS32 t2x = 0 ; t2x < (1 + trans2->y->nX) ; t2x++) {
        for (psS32 t2y = 0 ; t2y < (1 + trans2->y->nY) ; t2y++) {
            // XXX: Verify this (poly order/nterm change)
            psPolynomial2D *currPoly = psPolynomial2DAlloc(1-1, 1-1, PS_POLYNOMIAL_ORD);
            currPoly->coeff[0][0] = 1.0;
            currPoly->mask[0][0] = 0;
            psPolynomial2D *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 < (1 + currPoly->nX) ; i++) {
                    for (psS32 j = 0 ; j < (1 + currPoly->nY) ; j++) {
                        myPT->y->coeff[i][j]+= currPoly->coeff[i][j] * trans2->y->coeff[t2x][t2y];
                    }
                }
            }
            psFree(currPoly);
        }
    }

    //TRACE: printf("Exiting combine()\n");
    return(myPT);
}

/*****************************************************************************
psPlaneTransformFit(trans, source, dest, nRejIter, sigmaClip)
 
XXX: What about nRejIter?  Iterations?
XXX: Use static vectors for internal data.
XXX: This code has problems with data that corresponds to a non-linear fit.
 *****************************************************************************/
bool psPlaneTransformFit(psPlaneTransform *trans,
                         const psArray *source,
                         const psArray *dest,
                         int nRejIter,
                         float sigmaClip)
{
    PS_ASSERT_PTR_NON_NULL(trans, NULL);
    PS_ASSERT_PTR_NON_NULL(source, NULL);
    PS_ASSERT_PTR_NON_NULL(dest, NULL);

    psS32 numCoords = PS_MIN(source->n, dest->n);
    psS32 order = PS_MAX(trans->x->nX, trans->x->nY);
    order = PS_MAX(order, trans->y->nX);
    order = PS_MAX(order, trans->y->nY);
    // XXX: Verify, or recode, after the poly norder/nterm change.
    order++;

    //
    // Create fake polynomial to use in evaluation
    //
    // XXX: Verify this (poly order/nterm change)
    psPolynomial2D *fakePoly = psPolynomial2DAlloc(order-1, order-1, 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 = psPolynomial2DEval(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 = psPolynomial2DEval(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_ASSERT_PTR_NON_NULL(in, NULL);
    //
    // If the transform is linear, then invert it exactly and return.
    //
    if (p_psIsProjectionLinear((psPlaneTransform *) in)) {
        return(p_psPlaneTransformLinearInvert((psPlaneTransform *) in));
    }
    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.");
    }
    // XXX: recode or verify after poly changes
    psS32 order = 1 + in->x->nX;

    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) {
        // XXX: Verify this (poly order/nterm change)
        myPT = psPlaneTransformAlloc(order, order);
    } else {
        // XXX: recode or verify after poly changes
        if (((1 + out->x->nX) == order) && ((1 + out->x->nY) == order) &&
                ((1 + out->y->nX) == order) && ((1 + out->y->nX) == order)) {
            myPT = out;
        } else {
            psFree(out);
            myPT = psPlaneTransformAlloc(order, order);
        }
    }

    //
    // Copy the input transform to myPT.
    //
    for (psS32 i = 0 ; i < (1 + in->x->nX) ; i++) {
        for (psS32 j = 0 ; j < (1 + in->x->nY) ; j++) {
            myPT->x->coeff[i][j] = in->x->coeff[i][j];
        }
    }
    for (psS32 i = 0 ; i < (1 + in->y->nX) ; i++) {
        for (psS32 j = 0 ; j < (1 + 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.  Since we want the inverse of the transformation, the
    // inCoords are written to the outData vector, and the outCoords are written
    // to the inData vector.
    //
    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++;
        }
    }
    // XXX: what values should be used here?
    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);
}

psPlane *psPlaneTransformDeriv(
    psPlane *out,
    const psPlaneTransform *transformation,
    const psPlane *coord
)
{
    PS_ASSERT_PTR_NON_NULL(transformation, NULL);
    PS_ASSERT_POLY_NON_NULL(transformation->x, NULL);
    PS_ASSERT_POLY_NON_NULL(transformation->y, NULL);
    PS_ASSERT_PTR_NON_NULL(coord, NULL);

    if (out == NULL) {
        out = psPlaneAlloc();
    }

    out->x = 0.0;
    out->y = 0.0;
    out->xErr = 0.0;
    out->yErr = 0.0;

    psPolynomial2D *xPoly = transformation->x;
    psPolynomial2D *yPoly = transformation->y;

    //
    // Calculate the derivative with respect to x.
    //
    psF32 xSum = 1.0;
    psF32 ySum = 1.0;

    // This loop starts at loop_x=1 since the derivative of the loop_x=0 terms are all 0.0
    for (psS32 loop_x = 1; loop_x < (1 + xPoly->nX); loop_x++) {
        ySum = 1.0;
        for (psS32 loop_y = 0; loop_y < (1 + xPoly->nY); loop_y++) {
            //
            // For each iteration of the loop, we multiple the (x, y) coefficient
            // by (coord->x^(loop_x-1) * coord->y^loop_y)
            //

            out->x+= xPoly->coeff[loop_x][loop_y] * xSum * ySum;
            ySum*= coord->y;
        }
        xSum*= coord->x;
    }

    //
    // Calculate the derivative with respect to x.
    //
    xSum = 1.0;

    // This loop starts at loop_y=1 since the derivative of the loop_y=0 terms are all 0.0
    for (psS32 loop_x = 0; loop_x < (1 + yPoly->nX); loop_x++) {
        ySum = 1.0;
        for (psS32 loop_y = 1; loop_y < (1 + yPoly->nY); loop_y++) {
            //
            // For each iteration of the loop, we multiple the (x, y) coefficient
            // by (coord->x^(loop_x-1) * coord->y^loop_y)
            //

            out->y+= yPoly->coeff[loop_x][loop_y] * xSum * ySum;
            ySum*= coord->y;
        }
        xSum*= coord->x;
    }
    return(out);
}

psCube *psSphereToCube(const psSphere *sphere)
{
    if(sphere == NULL) {
        psError( PS_ERR_UNKNOWN, true, "psSphere argument is NULL.  Returning NULL.\n");
        return NULL;
    }

    psCube *cube = NULL;

    cube = psCubeAlloc();
    cube->x = cos(sphere->d) * cos(sphere->r);
    cube->y = cos(sphere->d) * sin(sphere->r);
    cube->z = sin(sphere->d);
    cube->xErr = cos(sphere->dErr) * cos(sphere->rErr);
    cube->yErr = cos(sphere->dErr) * sin(sphere->rErr);
    cube->zErr = sin(sphere->dErr);

    return(cube);
}

psSphere *psCubeToSphere(const psCube *cube)
{
    if(cube == NULL) {
        psError( PS_ERR_UNKNOWN, true, "psCube argument is NULL.  Returning NULL.\n");
        return NULL;
    }

    psSphere *sphere = NULL;
    sphere = psSphereAlloc();
    //    sphere->r = arctan(cube->x/cube->y);
    //    sphere->d = arctan(sqrt(cube->x*cube->x + cube->y*cube->y)/cube->z);
    //    sphere->rErr = arctan(cube->xErr/cube->yErr);
    //    sphere->dErr = arctan(sqrt(cube->xErr*cube->xErr + cube->yErr*cube->yErr)/cube->zErr);
    sphere->r = 1 / (atan(cube->x/cube->y));
    sphere->d = 1 / (atan(sqrt(cube->x*cube->x + cube->y*cube->y)/cube->z));
    sphere->rErr = 1 / (atan(cube->xErr/cube->yErr));
    sphere->dErr = 1 / (atan(sqrt(cube->xErr*cube->xErr + cube->yErr*cube->yErr)/cube->zErr));

    return(sphere);
}
