/** @file  psSphereOps.c
 *
 *  @brief Contains spherical rotation and offset operations
 *
 *  @ingroup CoordinateTransform
 *
 *  @author Robert DeSonia, MHPCC
 *  @author Dave Robbins, MHPCC
 *
 *  @version $Revision: 1.9 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-11-10 00:13:50 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 */

#include <math.h>
#include <float.h>

#include "psSphereOps.h"
#include "psType.h"
#include "psCoord.h"
#include "psMemory.h"
#include "psTime.h"
#include "psConstants.h"
#include "psError.h"
#include "psLogMsg.h"
#include "psErrorText.h"


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

// Days in Julian century
#define JULIAN_CENTURY 36525.0

static void sphereRotFree(psSphereRot *result)
{
    // There are non dynamic allocated items
}

psSphereRot* psSphereRotAlloc(double alphaP,
                              double deltaP,
                              double phiP)
{
    psSphereRot r,s,t;

    // directly from ADD -- there must be a better way?!
    r.q0=0;
    r.q1=0;
    r.q2=sin(alphaP/2.0);
    r.q3=cos(alphaP/2.0);

    s.q0=0;
    s.q1=sin(deltaP/2.0);
    s.q2=0;
    s.q3=cos(deltaP/2.0);

    t.q0=0;
    t.q1=0;
    t.q2=sin(phiP/2.0);
    t.q3=cos(phiP/2.0);

    // calculate t*s*r.
    psSphereRot* temp = psSphereRotCombine(NULL,&t,&s);
    psSphereRot* result = psSphereRotCombine(NULL, temp, &r);
    psFree(temp);

    return result;
}

bool psMemCheckSphereRot(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)sphereRotFree );
}


psSphereRot* psSphereRotQuat(double q0,
                             double q1,
                             double q2,
                             double q3)
{
    psSphereRot* rot = (psSphereRot*)psAlloc(sizeof(psSphereRot));
    psMemSetDeallocator(rot, (psFreeFunc)sphereRotFree);

    double len = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    rot->q0 = q0 / len;
    rot->q1 = q1 / len;
    rot->q2 = q2 / len;
    rot->q3 = q3 / len;

    return rot;
}

psSphereRot* psSphereRotConjugate(psSphereRot *out, const psSphereRot *in)
{
    if (in == NULL) {
        psError(PS_ERR_BAD_PARAMETER_NULL, true,
                "psSphereRot input cannot be NULL.\n");
        return NULL;
    }
    if (out == NULL) {
        out = (psSphereRot* ) psAlloc(sizeof(psSphereRot));
        psMemSetDeallocator(out, (psFreeFunc)sphereRotFree);
    }
    out->q0 = -in->q0;
    out->q1 = -in->q1;
    out->q2 = -in->q2;
    out->q3 = in->q3;

    return out;
}

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

    if (out == NULL) {
        out = psSphereAlloc();
    }
    //    double r = -coord->r;
    // apply the transform by creating a new psSphereRot from the input coord
    // and combining it with the input transform (see ADD)
    double cosD = cos(coord->d);
    psSphereRot* coordQuat = psSphereRotQuat(
                                 cosD*cos(coord->r),
                                 cosD*sin(coord->r),
                                 sin(coord->d),
                                 0.0);

    /*    psSphereRot *test = psSphereRotICRSToGalactic();
        if (fabs(coord->d-DEG_TO_RAD(27.1283)) < 0.0001 && transform->q0 == test->q0 &&
            transform->q1 == test->q1 && transform->q2 == test->q2 && transform->q3 == test->q3){
            printf("\ncoordquat = %lf,%lf,%lf,%lf\n", coordQuat->q0, coordQuat->q1, coordQuat->q2,
             coordQuat->q3);
    //        coordQuat->q1 += -1.0;
            test->q3 = - test->q3;
            *(psSphereRot**)&transform = test;
        }
        psFree(test);
    */
    // Inserted by PAP
    psSphereRot *conjugate = psSphereRotConjugate(NULL, transform);
    psSphereRot *temp = psSphereRotCombine(NULL, transform, coordQuat);
    psSphereRot *result = psSphereRotCombine(NULL, temp, conjugate);
    out->r = atan2(result->q1, result->q0);
    //    out->r = atan2(result->q1, result->q0);
    out->d = asin(result->q2);
    out->rErr = 0.0;
    out->dErr = 0.0;

    if (out->r < -0.0001) {
        out->r += 2.0 * M_PI;
    }

    psFree(conjugate);
    psFree(temp);
    psFree(result);
    psFree(coordQuat);
    return out;
    // Pau.

    #if 0
    //    psSphereRot* coordQuatConjugate = psSphereRotQuat(
    //                                       coordQuat->q0, coordQuat->q1, coordQuat->q2, coordQuat->q3);
    //    coordQuat = psSphereRotInvert(coordQuat);

    // calculate q=(rp)r'
    //    coordQuat = psSphereRotCombine(coordQuat, transform, coordQuat);
    //    coordQuat = psSphereRotCombine(coordQuat, coordQuat, coordQuatConjugate);
    // N.B., we can recycle coordQuat right away due to the implementation of
    // psSphereRotCombine; it puts the input values in a local variable first

    //    out->r = atan2(coordQuat->q1,coordQuat->q0);
    //    out->d = asin(coordQuat->q2);


    //     psSphereRot *inv = psSphereRotInvert(transform);
    psSphereRot *inv = (psSphereRot*)psAlloc(sizeof(psSphereRot));
    *inv = *transform;
    //    psSphereRot *result = psSphereRotCombine(NULL, transform, coordQuat);
    //    inv = psSphereRotInvert(inv);
    //    psSphereRotCombine(result, result, inv);
    //    psSphereRot *result = psSphereRotCombine(NULL, inv, coordQuat);
    //    psSphereRotCombine(result, result, transform);
    //    psSphereRot *result = psSphereRotCombine(NULL, coordQuat, transform);
    //    psSphereRotCombine(result, inv, result);

    psSphereRot *result = psSphereRotCombine(NULL, coordQuat, transform);
    inv = psSphereRotInvert(inv);
    //    *(psSphereRot**)&transform = psSphereRotInvert( *(psSphereRot**)&transform);
    result = psSphereRotCombine(result, inv, result);
    //    *(psSphereRot**)&transform = psSphereRotInvert( *(psSphereRot**)&transform);

    //        out->r = atan2(result->q0, result->q1);
    out->r = atan2(result->q1, result->q0);
    out->d = asin(result->q2);
    out->rErr = 0.0;
    out->dErr = 0.0;
    psFree(inv);
    psFree(result);

    psFree(coordQuat);
    //    psFree(coordQuatConjugate);

    return out;
    #endif
}

psSphereRot* psSphereRotCombine(psSphereRot* out,
                                const psSphereRot* rot1,
                                const psSphereRot* rot2)
{
    PS_ASSERT_PTR_NON_NULL(rot1, NULL);
    PS_ASSERT_PTR_NON_NULL(rot2, NULL);

    if (out == NULL) {
        out = (psSphereRot* ) psAlloc(sizeof(psSphereRot));
        psMemSetDeallocator(out, (psFreeFunc)sphereRotFree);
    }

    double a0 = rot1->q0;
    double a1 = rot1->q1;
    double a2 = rot1->q2;
    double a3 = rot1->q3;
    double b0 = rot2->q0;
    double b1 = rot2->q1;
    double b2 = rot2->q2;
    double b3 = rot2->q3;

    // following came from ADD
    //    out->q0 = b3*a0 + b2*a1 - b1*a2 + b0*a3;
    //    out->q1 = b3*a1 - b2*a0 + b1*a3 + b0*a2;
    //    out->q2 = b3*a2 + b2*a3 + b1*a0 - b0*a1;
    out->q0 = a3*b0 + a0*b3 + a1*b2 - a2*b1;
    out->q1 = a3*b1 - a0*b2 + a1*b3 + a2*b0;
    out->q2 = a3*b2 + a0*b1 - a1*b0 + a2*b3;

    out->q3 = b3*a3 - b2*a2 - b1*a1 - b0*a0;

    return out;
}

psSphereRot *psSphereRotInvert(double alphaP,
                               double deltaP,
                               double phiP)
{
    return (psSphereRotAlloc(-phiP, -deltaP, -alphaP));
}

psSphereRot* psSphereRotEclipticToICRS(const psTime *time)
{
    psF64 T;

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

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

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

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

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

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

psSphereRot* psSphereRotICRSToEcliptic(const psTime *time)
{
    psF64 T;

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

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

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

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

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

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

// XXX: This is bug 245: alphaP swaps with phiP from psSphereTransformGalacticToICRS()
psSphereRot* psSphereRotGalacticToICRS(void)
{
    /*    psF64 phiP = - DEG_TO_RAD(180.0-192.85948);
        psF64 deltaP = - DEG_TO_RAD(90.0 - 27.12825);
        psF64 alphaP = - DEG_TO_RAD(90.0+32.93192);
    */
    psF64 alphaP = DEG_TO_RAD(180.0-192.85948);
    psF64 deltaP = DEG_TO_RAD(90.0 - 27.12825);
    psF64 phiP = DEG_TO_RAD(90.0+32.93192);
    return (psSphereRotAlloc(-phiP,-deltaP,-alphaP));
}

psSphereRot* psSphereRotICRSToGalactic(void)
{
    psF64 alphaP = DEG_TO_RAD(180.0-192.85948);
    psF64 deltaP = DEG_TO_RAD(90.0 - 27.12825);
    psF64 phiP = DEG_TO_RAD(90.0+32.93192);

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

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

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

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

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

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

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

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

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

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

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

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

    // Return value
    return tmp;
}

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

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

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

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

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

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

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

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

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

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

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

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

        // Invalid mode report error
    } else {

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

    return tmp;
}

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

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

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

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

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

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

    // Apply transform to coordinates
    psSphere *out = psSphereRotApply(NULL, tmpST, coords);
    if (out->r < -0.0001) {
        out->r += 2.0 * M_PI;
    }

    psFree(tmpST);

    return(out);
}

