/** @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.106 $ $Name: not supported by cvs2svn $
*  @date $Date: 2006-02-07 23:14:21 $
*
*  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 "psTrace.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.
 
XXX: Since there is now a general psPlaneTransformInvert() function, we
should rename this.
 
XXX: This code no longer makes sense.  The merge must be reviewed.
 *****************************************************************************/
// XXX EAM : 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, NULL);
    PS_ASSERT_PTR_NON_NULL(transform->x, NULL);
    PS_ASSERT_PTR_NON_NULL(transform->y, NULL);
    if ((transform->x->nX < 1) ||
            (transform->x->nY < 1) ||
            (transform->y->nX < 1) ||
            (transform->y->nY < 1)) {
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: input transform is not invertible.");
        return(NULL);
    }

    psPlaneTransform *out = psPlaneTransformAlloc(1, 1);

    if (0) {
        // XXX: Get rid of this code.
        psF64 A = transform->x->coeff[1][0];
        psF64 B = transform->x->coeff[0][1];
        psF64 C = transform->y->coeff[1][0];
        psF64 D = transform->y->coeff[0][1];
        psF64 E = transform->x->coeff[0][0];
        psF64 F = transform->y->coeff[0][0];
        psF64 invDet = 1.0 / (A * D - B * C);

        out->x->coeff[1][0] = invDet *  D;
        out->x->coeff[0][1] = invDet * -B;
        out->y->coeff[1][0] = invDet * -C;
        out->y->coeff[0][1] = invDet *  A;

        //
        // This comes from substituting the (x_2 = x_1 - e) and (y_2 = y_1 - f) equations:
        //
        out->x->coeff[0][0] = invDet * (B * F - D * E);
        out->y->coeff[0][0] = invDet * (E * C - A * F);

    }

    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:
    true: if linear
    false: otherwise
 
Why isn't this called p_psIsPlaneTransformLinear()?
 *****************************************************************************/
psBool p_psIsProjectionLinear(psPlaneTransform *transform)
{
    PS_ASSERT_PTR_NON_NULL(transform, false);
    PS_ASSERT_PTR_NON_NULL(transform->x, false);
    PS_ASSERT_PTR_NON_NULL(transform->y, false);

    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(false);
                }
            }
        }
    }

    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(false);
                }
            }
        }
    }

    return(true);
}

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

psPlaneTransform* psPlaneTransformAlloc(int order1, int order2)
{
    PS_ASSERT_INT_NONNEGATIVE(order1, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order2, NULL);

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

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

psPlaneDistort* psPlaneDistortAlloc(int order1, int order2, int order3, int order4)
{
    PS_ASSERT_INT_NONNEGATIVE(order1, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order2, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order3, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order4, NULL);

    psPlaneDistort *pt = psAlloc(sizeof(psPlaneDistort));
    pt->x = psPolynomial4DAlloc(PS_POLYNOMIAL_ORD, order1, order2, order3, order4);
    pt->y = psPolynomial4DAlloc(PS_POLYNOMIAL_ORD, order1, order2, order3, order4);

    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.
 
XXX: Optimize this.  We don't need a while loop here.
 
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* p_psProject(
    psPlane *outPlane,
    const psSphere* coord,
    const psProjection* projection)
{
    PS_ASSERT_PTR_NON_NULL(coord, NULL);
    PS_ASSERT_PTR_NON_NULL(projection, NULL);

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

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

    // Allocate return value
    psPlane* out = NULL;
    if (outPlane == NULL) {
        out = psPlaneAlloc();
    } else {
        out = outPlane;
    }

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

psPlane* psProject(const psSphere* coord,
                   const psProjection* projection)
{
    return(p_psProject(NULL, coord, projection));
}

psSphere* p_psDeproject(
    psSphere *outSphere,
    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 = NULL;
    if (outSphere == NULL) {
        out = psSphereAlloc();
    } else {
        out = outSphere;
        // XXX: Do a memory checkpointer.
    }

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

psSphere* psDeproject(const psPlane* coord,
                      const psProjection* projection)
{
    return(p_psDeproject(NULL, coord, projection));
}


/*****************************************************************************
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)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    psTrace(__func__, 5, "multiplyDPoly2D(%d %d: %d %d)\n", trans1->nX, trans1->nY, trans2->nX, trans2->nY);
    psS32 orderX = trans1->nX + trans2->nX;
    psS32 orderY = trans1->nY + trans2->nY;
    psTrace(__func__, 5, "out poly (nX, nY) is (%d, %d)\n", orderX, orderY);

    psPolynomial2D *out = psPolynomial2DAlloc(PS_POLYNOMIAL_ORD, orderX, orderY);
    psTrace(__func__, 5, "Creating poly (%d, %d)\n", orderX, orderY);

    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]);
                    }
                }
            }
        }
    }
    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    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)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    psTrace(__func__, 3, "tracelevel(%s) is %d\n", __func__, psTraceGetLevel(__func__));

    PS_ASSERT_PTR_NON_NULL(trans1, NULL);
    PS_ASSERT_PTR_NON_NULL(trans2, NULL);
    psTrace(__func__, 5, "trans1->x is (%d, %d) order.\n", trans1->x->nX, trans1->x->nY);
    psTrace(__func__, 5, "trans1->y is (%d, %d) order.\n", trans1->y->nX, trans1->y->nY);
    psTrace(__func__, 5, "trans2->x is (%d, %d) order.\n", trans2->x->nX, trans2->x->nY);
    psTrace(__func__, 5, "trans2->y is (%d, %d) order.\n", trans2->y->nX, trans2->y->nY);
    if (psTraceGetLevel(__func__) >= 6) {
        PS_POLY_PRINT_2D(trans1->x);
        PS_POLY_PRINT_2D(trans1->y);
        PS_POLY_PRINT_2D(trans2->x);
        PS_POLY_PRINT_2D(trans2->y);
    }

    //
    // Determine the size of the new psPlaneTransform.
    //
    psS32 orderXnX = (trans2->x->nX * trans1->x->nX) + (trans2->x->nY * trans1->y->nX);
    psS32 orderXnY = (trans2->x->nX * trans1->x->nY) + (trans2->x->nY * trans1->y->nY);
    psS32 orderYnX = (trans2->y->nX * trans1->x->nX) + (trans2->y->nY * trans1->y->nX);
    psS32 orderYnY = (trans2->y->nX * trans1->x->nY) + (trans2->y->nY * trans1->y->nY);
    psS32 orderX = PS_MAX(orderXnX, orderYnX);
    psS32 orderY = PS_MAX(orderXnY, orderYnY);
    psTrace(__func__, 5, "The new (orderX, orderY) is (%d, %d)\n", orderX, orderY);

    //
    // 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 ((out->x->nX == orderX) &&
                (out->x->nY == orderY) &&
                (out->y->nX == orderX) &&
                (out->y->nY == orderY)) {
            myPT = out;
            //
            // 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;
                }
            }
        } else {
            psFree(out);
            myPT = psPlaneTransformAlloc(orderX, orderY);
        }
    }
    psTrace(__func__, 5, "New polynomial ranks are (%d %d %d %d)\n", myPT->x->nX, myPT->x->nY, myPT->y->nX, myPT->y->nY);

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

    //
    // Determine the new x-polynomial
    //
    psTrace(__func__, 5, "Determine the new x-polynomial\n");
    for (psS32 t2x = 0 ; t2x < (1 + trans2->x->nX) ; t2x++) {
        for (psS32 t2y = 0 ; t2y < (1 + trans2->x->nY) ; t2y++) {
            psTrace(__func__, 6, "X: -------------------- (t2x, t2y) (%d, %d) --------------------\n", t2x, t2y);
            if (trans2->x->mask[t2x][t2y] == 0) {
                psTrace(__func__, 6, "In this iteration, we raise trans1->x to the %d power and trans1->y to the %d-power.\n", t2x, t2y);
                // Create the constant "f(x, y) = 1" polynomial.
                psPolynomial2D *currPoly = psPolynomial2DAlloc(PS_POLYNOMIAL_ORD, 0, 0);
                currPoly->coeff[0][0] = 1.0;

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

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

                // 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];
                    }
                }
                if (psTraceGetLevel(__func__) >= 6) {
                    PS_POLY_PRINT_2D(myPT->x);
                }
                psFree(currPoly);
            }
        }
    }
    if (psTraceGetLevel(__func__) >= 6) {
        psTrace(__func__, 6, "The final x-polynomial\n");
        PS_POLY_PRINT_2D(myPT->x);
    }

    //
    // Determine the new y-polynomial
    //
    psTrace(__func__, 5, "Determine the new y-polynomial\n");
    for (psS32 t2x = 0 ; t2x < (1 + trans2->y->nX) ; t2x++) {
        for (psS32 t2y = 0 ; t2y < (1 + trans2->y->nY) ; t2y++) {
            psTrace(__func__, 5, "Y: -------------------- (t2x, t2y) (%d, %d) --------------------\n", t2x, t2y);
            if (trans2->y->mask[t2x][t2y] == 0) {
                psTrace(__func__, 5, "In this iteration, we raise trans1->x to the %d power and trans1->y to the %d-power.\n", t2x, t2y);
                // Create the constant "f(x, y) = 1" polynomial.
                psPolynomial2D *currPoly = psPolynomial2DAlloc(PS_POLYNOMIAL_ORD, 0, 0);
                currPoly->coeff[0][0] = 1.0;

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

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

                // 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];
                    }
                }
                if (psTraceGetLevel(__func__) >= 6) {
                    PS_POLY_PRINT_2D(myPT->x);
                }
                psFree(currPoly);
            }
        }
    }
    if (psTraceGetLevel(__func__) >= 6) {
        psTrace(__func__, 6, "The final y-polynomial\n");
        PS_POLY_PRINT_2D(myPT->y);
    }

    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    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(PS_POLYNOMIAL_ORD, order-1, order-1);
    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
    //
    // XXX: Check return codes
    //
    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

    //
    // 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);
    PS_ASSERT_PTR_NON_NULL(in->x, NULL);
    PS_ASSERT_PTR_NON_NULL(in->y, NULL);
    if ((in->x->nX < 1) ||
            (in->x->nY < 1) ||
            (in->y->nX < 1) ||
            (in->y->nY < 1)) {
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: input transform is not invertible.");
        return(NULL);
    }
    //
    // If the transform is linear, then invert it exactly and return.
    //
    if (p_psIsProjectionLinear((psPlaneTransform *) in)) {
        return(p_psPlaneTransformLinearInvert((psPlaneTransform *) in));
    }
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(nSamples, 1, 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 multiply the (x, y) coefficient
            // by (x^(loop_x-1) * y^loop_y) * loop_x
            //

            out->x+= xPoly->coeff[loop_x][loop_y] * xSum * ySum * ((psF32) loop_x);
            psTrace(__func__, 6, "out->x+= (%.2f * %.2f * %.2f * %.2f)\n", xPoly->coeff[loop_x][loop_y], xSum, ySum, ((psF32) loop_x));
            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 multiply the (x, y) coefficient
            // by (x^(loop_x-1) * y^loop_y) * loop_y
            // by (x^(loop_x) * y^(loop_y-1))
            //

            out->y+= yPoly->coeff[loop_x][loop_y] * xSum * ySum * ((psF32) loop_y);
            psTrace(__func__, 6, "out->y+= (%.2f * %.2f * %.2f * %.2f)\n", yPoly->coeff[loop_x][loop_y], xSum, ySum, ((psF32) loop_y));
            ySum*= coord->y;
        }
        xSum*= coord->x;
    }

    return(out);
}

psPixels *psPixelsTransform(psPixels *out,
                            const psPixels *input,
                            const psPlaneTransform *inToOut)
{
    PS_ASSERT_PTR_NON_NULL(input, NULL);
    PS_ASSERT_PTR_NON_NULL(inToOut, NULL);
    if (out == NULL) {
        //XXX: Should the length (nalloc) be 1 and append be used everytime a pixel is added?
        //        out = psPixelsAlloc(input->nalloc);
        out = psPixelsAlloc(0);
    }
    psPlane *coord = psPlaneAlloc();
    psPlane *deriv = psPlaneAlloc();
    psPlane *fxnVal = psPlaneAlloc();

    int i = 0;
    //    int m = 0;
    //    while (input->data.x[i] != 0.0 && input->data.y[i] != 0.0) {
    for ( i = 0; i < input->n; i++) {
        coord->x = input->data[i].x;
        coord->y = input->data[i].y;
        deriv = psPlaneTransformDeriv(deriv, inToOut, coord);
        fxnVal = psPlaneTransformApply(fxnVal, inToOut, coord);
        if (fabs(fxnVal->x - coord->x) <= fabs(deriv->x) &&
                fabs(fxnVal->y - coord->y) <= fabs(deriv->y)) {
            int x = (int)(ceil(fabs(deriv->x)));
            int y = (int)(ceil(fabs(deriv->y)));
            for (int j = -x; j <= x; j++) {
                for (int k = -y; k <= y; k++) {
                    //                    out->data[m].x = fxnVal->x + x;
                    //                    out->data[m].y = fxnVal->y + y;
                    //                    m++;
                    out = p_psPixelsAppend(out, 1, (float)(fxnVal->x+j),
                                           (float)(fxnVal->y+k) );
                }
            }
        }
    }

    psFree(coord);
    psFree(deriv);
    psFree(fxnVal);
    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));
    psCube *cube2 = psCubeAlloc();
    *cube2 = *cube;
    double mag = sqrt(cube->x*cube->x + cube->y*cube->y + cube->z*cube->z);
    if (mag > 1.0) {
        cube2->x = cube2->x/mag;
        cube2->y = cube2->y/mag;
        cube2->z = cube2->z/mag;
    }

    sphere->r = atan2(cube2->y, cube2->x);
    sphere->d = asin(cube2->z);
    //    sphere->d = atan2((cube->x*cube->x + cube->y*cube->y), cube->z);
    sphere->rErr = atan2(cube2->yErr, cube2->xErr);
    sphere->dErr = asin(cube2->zErr);
    psFree(cube2);

    return(sphere);
}
