/** @file psSpline.c
*
*  @brief Contains basic spline allocation, deallocation, fitting,
*         and evaluation routines.
*
*  This file contains the routines that allocate, free, and evaluate splines.
*
*  @version $Revision: 1.157 $ $Name: not supported by cvs2svn $
*  @date $Date: 2007-01-09 22:38:53 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*/

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

/*****************************************************************************/
/*  INCLUDE FILES                                                            */
/*****************************************************************************/
#include <stdio.h>
#include <stdbool.h>
#include <float.h>
#include <math.h>

#include "psMemory.h"
#include "psVector.h"
#include "psScalar.h"
#include "psTrace.h"
#include "psError.h"
#include "psLogMsg.h"
#include "psPolynomial.h"
#include "psSpline.h"
#include "psAssert.h"

#include "psMathUtils.h"

/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/

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

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

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

/*****************************************************************************/
/* FUNCTION IMPLEMENTATION - LOCAL                                           */
/*****************************************************************************/

static void spline1DFree(psSpline1D *tmpSpline)
{
    if (tmpSpline == NULL) {
        return;
    }

    if (tmpSpline->spline != NULL) {
        for (psS32 i=0;i<tmpSpline->n;i++) {
            psFree((tmpSpline->spline)[i]);
        }
        psFree(tmpSpline->spline);
    }

    if (tmpSpline->p_psDeriv2 != NULL) {
        psFree(tmpSpline->p_psDeriv2);
    }

    psFree(tmpSpline->knots);

    return;
}


static void PS_POLY1D_PRINT(
    psPolynomial1D *poly)
{
    printf("-------------- PS_POLY1D_PRINT() --------------\n");
    printf("poly->nX is %d\n", poly->nX);
    for (psS32 i = 0 ; i < (1 + poly->nX) ; i++) {
        printf("poly->coeff[%d] is %f\n", i, poly->coeff[i]);
    }
}

static void PS_PRINT_SPLINE2(psSpline1D *mySpline)
{
    printf("-------------- PS_PRINT_SPLINE2() --------------\n");
    printf("mySpline->n is %d\n", mySpline->n);
    for (psS32 i = 0 ; i < mySpline->n ; i++) {
        PS_POLY1D_PRINT(mySpline->spline[i]);
    }
    PS_VECTOR_PRINT_F32(mySpline->knots);
}

/*****************************************************************************
CalculateSecondDerivs(): Given a set of x/y vectors corresponding to a
tabulated function at n points, this routine calculates the second
derivatives of the interpolating cubic splines at those n points.
 
The first and second derivatives at the endpoints were previously undefined in
the SDR.  From bugzilla #???, they are required to be 0.0, implementing natural
splines.
 
Endpoints are defined by
    PS_LEFT_SPLINE_DERIV
    PS_RIGHT_SPLINE_DERIV
 
This routine assumes that vectors x and y are of the appropriate types/sizes
(F32).
 
XXX: use recycled vectors for internal data.
XXX: do an F64 version?
 *****************************************************************************/
#define PS_LEFT_SPLINE_DERIV 0.0
#define PS_RIGHT_SPLINE_DERIV 0.0
static psF32 *calculateSecondDerivs(
    const psVector* x,                  ///< Ordinates
    const psVector* y)                  ///< Coordinates
{
    psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__);
    if (psTraceGetLevel("psLib.math") >= 6) {
        p_psVectorPrint(1, (psVector *) x, "x");
        p_psVectorPrint(1, (psVector *) y, "y");
    }
    psS32 n = y->n;
    psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32));
    psF32 *derivs2 = (psF32 *) psAlloc(n * sizeof(psF32));
    psF32 *X = (psF32 *) & (x->data.F32[0]);
    psF32 *Y = (psF32 *) & (y->data.F32[0]);
    //
    // The second derivatives at the endpoints, undefined in the SDR,
    // are set in psAssert.h: PS_LEFT_SPLINE_DERIV, PS_RIGHT_SPLINE_DERIV.
    //
    derivs2[0] = -0.5;
    u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - PS_LEFT_SPLINE_DERIV);

    for (psS32 i=1;i<=(n-2);i++) {
        psF32 sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
        psF32 p = sig * derivs2[i-1] + 2.0;
        derivs2[i] = (sig - 1.0) / p;
        u[i] = ((Y[i+1] - Y[i])/(X[i+1]-X[i])) - ((Y[i]-Y[i-1])/(X[i]-X[i-1]));
        u[i] = ((6.0 * u[i] / (X[i+1] - X[i-1])) - (sig * u[i-1])) / p;

        psTrace("psLib.math", 6, "X[%d] is %f\n", i, X[i]);
        psTrace("psLib.math", 6, "Y[%d] is %f\n", i, Y[i]);
        psTrace("psLib.math", 6, "u[%d] is %f\n", i, u[i]);
    }

    psF32 qn = 0.5;
    u[n-1] = (3.0/(X[n-1]-X[n-2])) * (PS_RIGHT_SPLINE_DERIV - (Y[n-1]-Y[n-2])/(X[n-1]-X[n-2]));
    derivs2[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * derivs2[n-2]) + 1.0);

    for (psS32 k=(n-2);k>=0;k--) {
        derivs2[k] = derivs2[k] * derivs2[k+1] + u[k];
        psTrace("psLib.math", 6, "derivs2[%d] is %f\n", k, derivs2[k]);
    }
    psFree(u);
    psTrace("psLib.math", 4, "---- %s() end ----\n", __func__);
    return(derivs2);
}


/*****************************************************************************/
/* FUNCTION IMPLEMENTATION - PUBLIC                                          */
/*****************************************************************************/

bool psMemCheckSpline1D(psPtr ptr)
{
    PS_ASSERT_PTR(ptr, false);
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree );
}

psSpline1D *psSpline1DAlloc()
{
    psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
    tmpSpline->n = 0;
    tmpSpline->spline = NULL;
    tmpSpline->knots = NULL;
    tmpSpline->p_psDeriv2 = NULL;
    psMemSetDeallocator(tmpSpline, (psFreeFunc) spline1DFree);

    return(tmpSpline);
}

/*****************************************************************************
psVectorFitSpline1D(): given a set of x/y vectors, this routine generates the
linear or cublic splines which satisfy those data points.
 
The formula for calculating the spline polynomials is derived from Numerical
Recipes in C.  The basic idea is that the polynomial is
 (1)     y = (A * y[0]) +
 (2)         (B * y[1]) +
 (3)         ((((A*A*A)-A) * mySpline->p_psDeriv2[0]) * H^2)/6.0 +
 (4)         ((((B*B*B)-B) * mySpline->p_psDeriv2[1]) * H^2)/6.0
Where:
 H = x[1]-x[0]
 A = (x[1]-x)/H
 B = (x-x[0])/H
The bulk of the code in this routine is the expansion of the above equation
into a polynomial in terms of x, and then saving the coefficients of the
powers of x in the spline polynomials.  This gets pretty complicated.
 
XXX: What types must be supported?
 *****************************************************************************/
psSpline1D *psVectorFitSpline1D(
    const psVector* x,                  ///< Ordinates.
    const psVector* y)                  ///< Coordinates.
{
    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    PS_ASSERT_LONG_LARGER_THAN_OR_EQUAL(y->n, (long)2, NULL);
    psS32 numSplines = (y->n)-1;
    psTrace("psLib.math", 5, "numSplines is %d\n", numSplines);

    //
    // Create the psSpline1D struct.
    //
    psSpline1D *spline = psSpline1DAlloc();
    spline->n = numSplines;
    spline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    for (psS32 i=0;i<numSplines;i++) {
        spline->spline[i] = psPolynomial1DAlloc(PS_POLYNOMIAL_ORD, 3);
    }

    //
    // The following code ensures that xPtr and yPtr points to a psF32 psVector.
    //
    // XXX: Use the vector copy and create routines here:
    //

    spline->knots = psVectorAlloc(y->n, PS_TYPE_F32);
    if (x != NULL) {
        PS_ASSERT_VECTOR_NON_NULL(x, NULL);
        PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
        if (x->type.type == PS_TYPE_F32) {
            for (psS32 i = 0 ; i < x->n ; i++) {
                spline->knots->data.F32[i] = x->data.F32[i];
            }
        } else if (x->type.type == PS_TYPE_F64) {
            for (psS32 i = 0 ; i < x->n ; i++) {
                spline->knots->data.F32[i] = (psF32) x->data.F64[i];
            }
        }
    } else {
        for (psS32 i = 0 ; i < y->n ; i++) {
            spline->knots->data.F32[i] = (psF32) i;
        }
    }
    psVector *xPtr = spline->knots;

    psVector *yPtr = NULL;
    // Convert y to F32 if necessary.
    if (PS_TYPE_F64 == y->type.type) {
        yPtr = psVectorCopy(NULL, y, PS_TYPE_F32);
    } else {
        yPtr = (psVector *) y;
    }

    //
    // Generate the second derivatives at each data point.
    //
    spline->p_psDeriv2 = calculateSecondDerivs(xPtr, yPtr);

    //
    // We generate the coefficients of the spline polynomials.  I can't
    // concisely explain how this code works.  See above function comments
    // and Numerical Recipes in C.
    //
    for (psS32 i=0 ; i < numSplines ; i++) {
        psF32 H = xPtr->data.F32[i+1] - xPtr->data.F32[i];
        if (fabs(H) <= FLT_EPSILON) {
            psError(PS_ERR_UNKNOWN, false, "x data points are not distinct (%d %d) (%f %f).\n",
                    i, i+1, xPtr->data.F32[i], xPtr->data.F32[i+1]);
        }
        psTrace("psLib.math", 6, "x data (%f - %f) (%f)\n", xPtr->data.F32[i], xPtr->data.F32[i+1], H);
        //
        // ******** Calculate 0-order term ********
        //
        // From (1)
        spline->spline[i]->coeff[0] = yPtr->data.F32[i] * xPtr->data.F32[i+1]/H;
        // From (2)
        spline->spline[i]->coeff[0]-= (yPtr->data.F32[i+1] * xPtr->data.F32[i])/H;
        // From (3)
        psF32 tmp = (xPtr->data.F32[i+1] * xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H);
        tmp-= xPtr->data.F32[i+1] / H;
        tmp*= spline->p_psDeriv2[i] * H * H / 6.0;
        spline->spline[i]->coeff[0]+= tmp;
        // From (4)
        tmp = -(xPtr->data.F32[i] * xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H);
        tmp+= xPtr->data.F32[i] / H;
        tmp*= spline->p_psDeriv2[i+1] * H * H / 6.0;
        spline->spline[i]->coeff[0]+= tmp;

        //
        // ******** Calculate 1-order term ********
        //
        // From (1)
        spline->spline[i]->coeff[1] = -(yPtr->data.F32[i]) / H;
        // From (2)
        spline->spline[i]->coeff[1]+= yPtr->data.F32[i+1] / H;
        // From (3)
        tmp = -3.0 * xPtr->data.F32[i+1] * xPtr->data.F32[i+1] / (H * H * H);
        tmp+= (1.0 / H);
        tmp*= spline->p_psDeriv2[i] * H * H / 6.0;
        spline->spline[i]->coeff[1]+= tmp;
        // From (4)
        tmp = 3.0 * xPtr->data.F32[i] * xPtr->data.F32[i] / (H * H * H);
        tmp-= 1.0 / H;
        tmp*= spline->p_psDeriv2[i+1] * H * H / 6.0;
        spline->spline[i]->coeff[1]+= tmp;

        //
        // ******** Calculate 2-order term ********
        //
        // From (3)
        spline->spline[i]->coeff[2] = spline->p_psDeriv2[i] * 3.0 * xPtr->data.F32[i+1] / (6.0 * H);
        // From (4)
        spline->spline[i]->coeff[2]-= spline->p_psDeriv2[i+1] * 3.0 * xPtr->data.F32[i] / (6.0 * H);

        //
        // ******** Calculate 3-order term ********
        //
        // From (3)
        spline->spline[i]->coeff[3] = -spline->p_psDeriv2[i] / (6.0 * H);
        // From (4)
        spline->spline[i]->coeff[3]+=  spline->p_psDeriv2[i+1] / (6.0 * H);

        psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[0] is %f\n", i, spline->spline[i]->coeff[0]);
        psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[1] is %f\n", i, spline->spline[i]->coeff[1]);
        psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[2] is %f\n", i, spline->spline[i]->coeff[2]);
        psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[3] is %f\n", i, spline->spline[i]->coeff[3]);
    }

    if (PS_TYPE_F64 == y->type.type) {
        psFree(yPtr);
    }
    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    return(spline);
}


/*****************************************************************************
psSpline1DEval(): this routine takes an existing spline of arbitrary order
and an independent x value.  It determines which spline that x corresponds
to by doing a bracket disection on the knots of the spline data structure
(vectorBinDisectF32()).  Then it evaluates the spline at that x location
by a call to the 1D polynomial functions.
 
XXX: The spline eval functions require input and output to be F32.  however
     the spline fit functions require F32 and F64.
 
XXX: This only works if spline->knots if psF32.  Must we add support for psU32 and
psF64?
 *****************************************************************************/
float psSpline1DEval(
    const psSpline1D *spline,
    float x)
{
    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_PTR_NON_NULL(spline, NAN);
    PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN);
    PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NAN);

    psS32 n = spline->n;
    if ((x < spline->knots->data.F32[0]) || (x > spline->knots->data.F32[spline->knots->n-1])) {
        // If x is outside the range of spline->knots, generate a warning
        // message, then return the left, or right, endpoint.
        psLogMsg(__func__, PS_LOG_WARN,
                 "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f) (%d).",
                 x, spline->knots->data.F32[0], spline->knots->data.F32[n-1], n);

        psS32 binNum = (x < spline->knots->data.F32[0]) ? 0 : n-1;
        psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
        return(psPolynomial1DEval(spline->spline[binNum], x));
    }

    psScalar tmpScalar;
    tmpScalar.type.type = PS_TYPE_F32;
    tmpScalar.data.F32 = x;
    psS32 binNum = p_psVectorBinDisect(spline->knots, &tmpScalar);
    if (binNum < 0) {
        psError(PS_ERR_UNKNOWN, false, "Could not perform bin dissection on spline->knots.\n");
        return(NAN);
    }

    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    return(psPolynomial1DEval(spline->spline[binNum], x));
}


/*****************************************************************************
 *****************************************************************************/
psVector *psSpline1DEvalVector(
    const psSpline1D *spline,
    const psVector *x)
{
    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_PTR_NON_NULL(spline, NULL);
    PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    if (psTraceGetLevel("psLib.math") >= 6) {
        PS_VECTOR_PRINT_F32(x);
        PS_PRINT_SPLINE2((psSpline1D *) spline);
    }

    psVector *tmpVector = psVectorAlloc(x->n, PS_TYPE_F32);
    if (x->type.type == PS_TYPE_F32) {
        for (psS32 i=0;i<x->n;i++) {
            tmpVector->data.F32[i] = psSpline1DEval(spline, x->data.F32[i]);
        }
    } else if (x->type.type == PS_TYPE_F64) {
        for (psS32 i=0;i<x->n;i++) {
            tmpVector->data.F32[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);
        }
    }

    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    return(tmpVector);
}
