/** @file psMathUtils.c
 *
 *  This file contains standard math rotines.
 *
 *  @version $Revision: 1.1 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2006-02-02 21:09:07 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 *
 */
/*****************************************************************************/
/*  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 "psMathUtils.h"
#include "psConstants.h"
#include "psErrorText.h"

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

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

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

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

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

/*****************************************************************************
vectorBinDisectTYPE(): This is a macro for a private function which takes as
input an array of data as well as a single value for that data.  The input
vector values are assumed to be non-decreasing (v[i-1] <= v[i] for all i).
This routine does a binary disection of the vector and returns "i" such that
(v[i] <= x <= v[i+1).  If x lies outside the range of v[], then this routine
prints a warning message and returns (-2 or -1).
 *****************************************************************************/
#define FUNC_MACRO_VECTOR_BIN_DISECT(TYPE) \
static psS32 vectorBinDisect##TYPE(ps##TYPE *bins, \
                                   psS32 numBins, \
                                   ps##TYPE x) \
{ \
    psS32 min; \
    psS32 max; \
    psS32 mid; \
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__); \
    /* psTrace(__func__, 6, "Determining the bin for: %f\n", x); */\
    if (x < bins[0]) { \
        psLogMsg(__func__, PS_LOG_WARN, \
                 "vectorBinDisect%s(): ordinate %f is outside vector range (%f - %f).", \
                 #TYPE, x, bins[0], bins[numBins-1]); \
        return(-2); \
    } \
    if (x > bins[numBins-1]) { \
        psLogMsg(__func__, PS_LOG_WARN, \
                 "vectorBinDisect%s(): ordinate %f is outside vector range (%f - %f).", \
                 #TYPE, x, bins[0], bins[numBins-1]); \
        return(-1); \
    } \
    \
    min = 0; \
    max = numBins-2; \
    mid = ((max+1)-min)/2; \
    while (min != max) { \
        /*psTrace(__func__, 6, "(min, mid, max) is (%u, %u, %u): (x, bins[mid]) is (%f, %f)\n", min, mid, max, x, bins[mid]); */\
        \
        if (x == bins[mid]) { \
            /*psTrace(__func__, 6, "%.2f should be in this range (%.2f - %.2f)\n", x, bins[mid], bins[mid+1]); */\
            psTrace(__func__, 4, "---- %s(%d) end (1) ----\n", __func__, mid); \
            return(mid); \
        } else if (x < bins[mid]) { \
            max = mid-1; \
        } else { \
            min = mid; \
        } \
        mid = ((max+1)+min)/2; \
    } \
    /*psTrace(__func__, 6, "%.2f should be in this range (%.2f - %.2f)\n", x, bins[min], bins[min+1]);*/ \
    psTrace(__func__, 4, "---- %s(%d) end (2) ----\n", __func__, min); \
    return(min); \
} \

FUNC_MACRO_VECTOR_BIN_DISECT(S8)
FUNC_MACRO_VECTOR_BIN_DISECT(S16)
FUNC_MACRO_VECTOR_BIN_DISECT(S32)
FUNC_MACRO_VECTOR_BIN_DISECT(S64)
FUNC_MACRO_VECTOR_BIN_DISECT(U8)
FUNC_MACRO_VECTOR_BIN_DISECT(U16)
FUNC_MACRO_VECTOR_BIN_DISECT(U32)
FUNC_MACRO_VECTOR_BIN_DISECT(U64)
FUNC_MACRO_VECTOR_BIN_DISECT(F32)
FUNC_MACRO_VECTOR_BIN_DISECT(F64)

/*****************************************************************************
p_psVectorBinDisect(): A wrapper to the above p_psVectorBinDisect().
  *****************************************************************************/
psS32 p_psVectorBinDisect(
    psVector *bins,
    psScalar *x)
{
    PS_ASSERT_VECTOR_NON_NULL(bins, -4);
    PS_ASSERT_VECTOR_NON_EMPTY(bins, -4);
    PS_ASSERT_PTR_NON_NULL(x, -6);
    PS_ASSERT_PTR_TYPE_EQUAL(x, bins, -3);
    char* strType;

    switch (x->type.type) {
    case PS_TYPE_U8:
        return(vectorBinDisectU8(bins->data.U8, bins->n, x->data.U8));
    case PS_TYPE_U16:
        return(vectorBinDisectU16(bins->data.U16, bins->n, x->data.U16));
    case PS_TYPE_U32:
        return(vectorBinDisectU32(bins->data.U32, bins->n, x->data.U32));
    case PS_TYPE_U64:
        return(vectorBinDisectU64(bins->data.U64, bins->n, x->data.U64));
    case PS_TYPE_S8:
        return(vectorBinDisectS8(bins->data.S8, bins->n, x->data.S8));
    case PS_TYPE_S16:
        return(vectorBinDisectS16(bins->data.S16, bins->n, x->data.S16));
    case PS_TYPE_S32:
        return(vectorBinDisectS32(bins->data.S32, bins->n, x->data.S32));
    case PS_TYPE_S64:
        return(vectorBinDisectS64(bins->data.S64, bins->n, x->data.S64));
    case PS_TYPE_F32:
        return(vectorBinDisectF32(bins->data.F32, bins->n, x->data.F32));
    case PS_TYPE_F64:
        return(vectorBinDisectF64(bins->data.F64, bins->n, x->data.F64));
    default:
        PS_TYPE_NAME(strType, x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE, "psLib type %s is not supported.", strType);
        return -1;
    }
    return(-3);
}



/*****************************************************************************
fullInterpolateTYPE(): This routine will take as input n-element psVectors
domain and range, and the x value, assumed to lie with the domain vector.  It
produces as output the (order-1)-order LaGrange interpolated value of x.
 
XXX: do we error check for non-distinct domain values?
 
This routine will only be called inside this file.  So, we do not have to
do PS_ASSERTS.
 *****************************************************************************/
#define FUNC_MACRO_FULL_INTERPOLATE(TYPE) \
static psScalar *vectorInterpolate##TYPE( \
        psScalar *out, \
        psVector *domain, \
        psVector *range, \
        psS32 order, \
        psScalar *x) \
{ \
    psTrace(__func__, 4, "---- %s() begin %u-order.) (%d data points) ----\n", __func__, order, order+1); \
    if (x->data.TYPE < domain->data.TYPE[0]) { \
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: x is outside the domain of input data.\n"); \
        out->data.TYPE = range->data.TYPE[0]; \
        return(out); \
    } \
    if (x->data.TYPE > domain->data.TYPE[domain->n-1]) { \
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: x is outside the domain of input data.\n"); \
        out->data.TYPE = range->data.TYPE[domain->n-1]; \
        return(out); \
    } \
    psVector *p = psVectorCopy(NULL, range, PS_TYPE_##TYPE); \
    psS32 binNum = p_psVectorBinDisect(domain, x); \
    \
    psS32 numIntPoints = order+1; \
    psS32 origin; \
    if (0 == numIntPoints%2) { \
        origin = binNum - ((numIntPoints/2) - 1); \
    } else { \
        origin = binNum - (numIntPoints/2); \
        if ((x->data.TYPE-domain->data.TYPE[binNum]) > (domain->data.TYPE[binNum+1]-x->data.TYPE)) { \
            /* x is closer to binNum+1. */\
            origin = 1 + (binNum - (numIntPoints/2)); \
        } \
    } \
    origin = PS_MAX(origin, 0); \
    origin = PS_MIN(origin, (domain->n - numIntPoints)); \
    \
    /* From NR, during each iteration of the m loop, we are computing the p_{i ... i+m} terms. */ \
    for (psU32 m = 1 ; m < numIntPoints ; m++) { \
        for (psU32 i = origin ; i < (numIntPoints+origin-m) ; i++) { \
            p->data.TYPE[i] = (((x->data.TYPE - domain->data.TYPE[i+m]) * p->data.TYPE[i]) + \
                               ((domain->data.TYPE[i] - x->data.TYPE) * p->data.TYPE[i+1])) / \
                              (domain->data.TYPE[i] - domain->data.TYPE[i+m]); \
        } \
    } \
    out->data.TYPE = p->data.TYPE[origin]; \
    psFree(p); \
    psTrace(__func__, 4, "---- %s(....) end ----\n", __func__); \
    return(out); \
} \

FUNC_MACRO_FULL_INTERPOLATE(U8)
FUNC_MACRO_FULL_INTERPOLATE(U16)
FUNC_MACRO_FULL_INTERPOLATE(U32)
FUNC_MACRO_FULL_INTERPOLATE(U64)
FUNC_MACRO_FULL_INTERPOLATE(S8)
FUNC_MACRO_FULL_INTERPOLATE(S16)
FUNC_MACRO_FULL_INTERPOLATE(S32)
FUNC_MACRO_FULL_INTERPOLATE(S64)
FUNC_MACRO_FULL_INTERPOLATE(F32)
FUNC_MACRO_FULL_INTERPOLATE(F64)

/*****************************************************************************
p_psVectorInterpolate(): This routine will take as input psVectors domain and
range, and the x value, assumed to lie with the domain vector.  It produces
as output the LaGrange interpolated value of a polynomial of the specified
order around the point x.
 
XXX: This stuff does not currently work with a mask.
 *****************************************************************************/
psScalar *p_psVectorInterpolate(
    psScalar *out,
    psVector *domain,
    psVector *range,
    psS32 order,
    psScalar *x)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    PS_ASSERT_VECTOR_NON_NULL(domain, NULL);
    PS_ASSERT_VECTOR_NON_NULL(range, NULL);
    PS_ASSERT_PTR_NON_NULL(x, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(domain, range, NULL);
    PS_ASSERT_PTR_TYPE_EQUAL(domain, range, NULL);
    PS_ASSERT_PTR_TYPE_EQUAL(domain, x, NULL);
    if (out == NULL) {
        out = psScalarAlloc(0, x->type.type);
    } else {
        PS_ASSERT_PTR_TYPE_EQUAL(domain, out, NULL);
    }
    char* strType;

    switch (x->type.type) {
    case PS_TYPE_U8:
        return(vectorInterpolateU8(out, domain, range, order, x));
    case PS_TYPE_U16:
        return(vectorInterpolateU16(out, domain, range, order, x));
    case PS_TYPE_U32:
        return(vectorInterpolateU32(out, domain, range, order, x));
    case PS_TYPE_U64:
        return(vectorInterpolateU64(out, domain, range, order, x));
    case PS_TYPE_S8:
        return(vectorInterpolateS8(out, domain, range, order, x));
    case PS_TYPE_S16:
        return(vectorInterpolateS16(out, domain, range, order, x));
    case PS_TYPE_S32:
        return(vectorInterpolateS32(out, domain, range, order, x));
    case PS_TYPE_S64:
        return(vectorInterpolateS64(out, domain, range, order, x));
    case PS_TYPE_F32:
        return(vectorInterpolateF32(out, domain, range, order, x));
    case PS_TYPE_F64:
        return(vectorInterpolateF64(out, domain, range, order, x));
    default:
        PS_TYPE_NAME(strType, x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE, "psLib type %s is not supported.", strType);
        return NULL;
    }

    return NULL;
}

/*****************************************************************************
These macros and functions define the following functions:
 
    p_psNormalizeVectorRange(myData, low, high)
 
That assumes that the low/high arguments are PS_TYPE_F64; the vector myData
can be of any type.  Arguments low/high will be converted to the appropriate
type and one of the type-specific functions below will be called:
 
    p_psNormalizeVectorRangeU8(myData, low, high)
    p_psNormalizeVectorRangeU16(myData, low, high)
    p_psNormalizeVectorRangeU32(myData, low, high)
    p_psNormalizeVectorRangeU64(myData, low, high)
    p_psNormalizeVectorRangeS8(myData, low, high)
    p_psNormalizeVectorRangeS16(myData, low, high)
    p_psNormalizeVectorRangeS32(myData, low, high)
    p_psNormalizeVectorRangeS64(myData, low, high)
    p_psNormalizeVectorRangeF32(myData, low, high)
    p_psNormalizeVectorRangeF64(myData, low, high)
 *****************************************************************************/
#define PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(TYPE) \
static void p_psNormalizeVectorRange##TYPE( \
        psVector* myData, \
        ps##TYPE outLow, \
        ps##TYPE outHigh) \
{ \
    ps##TYPE min = (ps##TYPE) PS_MAX_##TYPE; \
    ps##TYPE max = (ps##TYPE) -PS_MAX_##TYPE; \
    psS32 i = 0; \
    \
    for (i = 0; i < myData->n; i++) { \
        if (myData->data.TYPE[i] < min) { \
            min = myData->data.TYPE[i]; \
        } \
        if (myData->data.TYPE[i] > max) { \
            max = myData->data.TYPE[i]; \
        } \
    } \
    \
    /* Ensure that max!=min before we divide by (max-min) */ \
    if (max != min) { \
        for (i = 0; i < myData->n; i++) { \
            myData->data.TYPE[i] = (outLow + (myData->data.TYPE[i] - min) * \
                                    (outHigh - outLow) / (max - min)); \
        } \
    } else { \
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: (max==min).  Setting all elements to min.\n"); \
        for (i = 0; i < myData->n; i++) \
        { \
            \
            myData->data.TYPE[i] = outLow; \
            \
        } \
    } \
} \

PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(U8)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(U16)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(U32)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(U64)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(S8)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(S16)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(S32)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(S64)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(F32)
PS_FUNC_MACRO_NORMALIZE_VECTOR_RANGE(F64)

psS32 p_psNormalizeVectorRange(psVector* myData,
                               psF64 outLow,
                               psF64 outHigh)
{
    PS_ASSERT_VECTOR_NON_NULL(myData, -1);
    char* strType;

    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    switch (myData->type.type) {
    case PS_TYPE_U8:
        p_psNormalizeVectorRangeU8(myData, (psU8) outLow, (psU8) outHigh);
        break;
    case PS_TYPE_U16:
        p_psNormalizeVectorRangeU16(myData, (psU16) outLow, (psU16) outHigh);
        break;
    case PS_TYPE_U32:
        p_psNormalizeVectorRangeU32(myData, (psU32) outLow, (psU32) outHigh);
        break;
    case PS_TYPE_U64:
        p_psNormalizeVectorRangeU64(myData, (psU64) outLow, (psU64) outHigh);
        break;
    case PS_TYPE_S8:
        p_psNormalizeVectorRangeS8(myData, (psS8) outLow, (psS8) outHigh);
        break;
    case PS_TYPE_S16:
        p_psNormalizeVectorRangeS16(myData, (psS16) outLow, (psS16) outHigh);
        break;
    case PS_TYPE_S32:
        p_psNormalizeVectorRangeS32(myData, (psS32) outLow, (psS32) outHigh);
        break;
    case PS_TYPE_S64:
        p_psNormalizeVectorRangeS64(myData, (psS64) outLow, (psS64) outHigh);
        break;
    case PS_TYPE_F32:
        p_psNormalizeVectorRangeF32(myData, (psF32) outLow, (psF32) outHigh);
        break;
    case PS_TYPE_F64:
        p_psNormalizeVectorRangeF64(myData, (psF64) outLow, (psF64) outHigh);
        break;
    case PS_TYPE_C32:
    case PS_TYPE_C64:
    default:
        PS_TYPE_NAME(strType, myData->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE, "psLib type %s is not supported.", strType);
        break;
    }
    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    return(0);
}


