#include <pslib.h>
# include "pmGrowthCurve.h"

static void pmGrowthCurveFree (pmGrowthCurve *growth)
{

    if (growth == NULL)
        return;

    psFree (growth->radius);
    psFree (growth->apMag);
    return;
}

pmGrowthCurve *pmGrowthCurveAlloc (psF32 minRadius, psF32 maxRadius, psF32 dRadius)
{

    pmGrowthCurve *growth = psAlloc (sizeof(pmGrowthCurve));
    psMemSetDeallocator(growth, (psFreeFunc) pmGrowthCurveFree);

    growth->radius = psVectorCreate (NULL, minRadius, maxRadius, dRadius, PS_TYPE_F32);
    growth->apMag  = psVectorAlloc (growth->radius->n, PS_TYPE_F32);

    // XXX may want to extend this to allow for a different refRadius;
    growth->refRadius = maxRadius;
    growth->maxRadius = maxRadius;
    growth->apLoss = 0.0;
    growth->fitMag = 0.0;
    return growth;
}

psF32 pmGrowthCurveCorrect (pmGrowthCurve *growth, psF32 radius)
{

    float apRad = psVectorInterpolate (growth->radius, growth->apMag, radius);
    float apCor = growth->apRef - apRad;
    return apCor;
}

// return the last entry below or first entry above key value
int psVectorBracket (psVector *index, psF32 key, bool above)
{

    int N;
    int Nlo = 0;
    int Nhi = index->n;

    if (above) {
        while (Nhi - Nlo > 10) {
            N = 0.5*(Nlo + Nhi);
            if (index->data.F32[N] > key) {
                Nhi = N;
            } else {
                Nlo = N - 1;
            }
        }
        // at this point, index[Nhi] > key >= index[Nlo]
        N = Nlo;
        while ((index->data.F32[N] <= key) && (N < Nhi)) {
            N++;
        }
        return (N);
    }
    while (Nhi - Nlo > 10) {
        N = 0.5*(Nlo + Nhi);
        if (index->data.F32[N] < key) {
            Nlo = N;
        } else {
            Nhi = N + 1;
        }
    }
    // at this point, index[Nhi] >= key > index[Nlo]
    N = Nhi;
    while ((index->data.F32[N] >= key) && (N > Nlo)) {
        N--;
    }
    return (N);
}

// search for the bins bounding key in index, interpolate the corresponding values
psF32 psVectorInterpolate (psVector *index, psVector *value, psF32 key)
{

    int n0 = 0;
    int n1 = 0;

    // extrapolate at ends
    if (key < index->data.F32[0]) {
        n0 = 0;
        n1 = 1;
    }

    // extrapolate at ends
    if (key > index->data.F32[index->n-1]) {
        n0 = index->n-2;
        n1 = index->n-1;
    }

    if (n1 == 0) {
        int n0 = psVectorBracket (index, key, FALSE);
        n1 = n0 + 1;
    }

    if (n0 == index->n-1) {
        n1 = n0;
        n0 = n1 - 1;
    }

    float dy = value->data.F32[n1] - value->data.F32[n0];
    float dx = index->data.F32[n1] - index->data.F32[n0];
    float dX = key - index->data.F32[n0];
    float dY = dX * (dy/dx);
    float result = value->data.F32[n0] + dY;
    return result;
}

