/** @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 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-10-12 21:02:20 $
*
*  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(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(2, 2);

    /* This is sample code from IfA.  It didn't work initially, and I did not
       spend any time debugging it.

        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); // Inverse of the determinant

        // Not entirely sure why this works, but it appears to do so....................................!
        out->x->coeff[1][0] = invDet * a;
        out->x->coeff[0][1] = - invDet * b;
        out->y->coeff[1][0] = - invDet * c;
        out->y->coeff[0][1] = invDet * d;

        out->x->coeff[0][0] = - invDet * (d * e + c * f);
        out->y->coeff[0][0] = - invDet * (b * e + a * f);
    */
    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.
 
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-1, n2-1, PS_POLYNOMIAL_ORD);
    pt->y = psPolynomial2DAlloc(n1-1, n2-1, 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-1, n2-1, n3-1, n4-1, PS_POLYNOMIAL_ORD);
    pt->y = psPolynomial4DAlloc(n1-1, n2-1, n3-1, n4-1, 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   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 = M_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_ASSERT_PTR_NON_NULL(coord, NULL);
    PS_ASSERT_PTR_NON_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/M_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;
}

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