Piecewise cubic Hermite interpolating polynomial (PCHIP)

// populate w_interp with interpolated values from piecewise cubic 
// Hermite spline
static function SBL_PCHIP(w_nodesX, w_nodesY, w_interp, [xwave])    
    wave w_nodesX, w_nodesY, w_interp
    wave /Z xwave
    
    if(numpnts(w_nodesX)==2) // linear interpolation
        if(waveexists(xwave))   
            w_interp=w_nodesY[0]+(xwave[p]-w_nodesX[0])/(w_nodesX[1]-w_nodesX[0])*(w_nodesY[1]-w_nodesY[0])
        else
            w_interp=w_nodesY[0]+(x-w_nodesX[0])/(w_nodesX[1]-w_nodesX[0])*(w_nodesY[1]-w_nodesY[0])
        endif
        return 1
    endif
    
    wave w_d=SBL_setDerivativesPCHIP(w_nodesX, w_nodesY)    
    if(waveexists(xwave))   
        w_interp=SBL_InterpolatePCHIP(w_nodesX, w_nodesY, w_d, xwave[p])
    else
        w_interp=SBL_InterpolatePCHIP(w_nodesX, w_nodesY, w_d, x)
    endif   
end
 
// The trick is to choose good values for the slope at each node. See:
// FN Fritsch and RE Carlson (1980) Monotone piecewise cubic 
// interpolation. SIAM J. Numer. Anal. 17: 238. DOI:10.1137/0717021 
// KW Brodlie (1980) A review of methods for curve and function drawing. 
// In Mathematical Methods in Computer Graphics and Design, KW Brodlie, 
// ed., Academic Press, London, pp. 1-37.
// FN Fritsch and J Butland (1984) A method for constructing local 
// monotone piecewise cubic interpolants, SIAM Journal on Scientific and 
// Statistical Computing 5: 300-304.
static function /WAVE SBL_setDerivativesPCHIP(w_nodesX, w_nodesY)
    wave w_nodesX, w_nodesY
 
    duplicate /free w_nodesX, w_d, w_a
    // w_d will be set to desired derivatives at node positions
    // w_a will be used as weighting for harmonic means
    w_d=0
    
    make /free/n=(numpnts(w_nodesX)-1), w_m, w_delx
    // w_m[i] is gradient between node i and i+1
    // w_delx[i] and w_delx[i-1] are x offsets of nodes i+1 and i-1 
    
    w_m = (w_nodesY[p+1]-w_nodesY)/(w_nodesX[p+1]-w_nodesX)
    w_delx = w_nodesX[p+1]-w_nodesX
    variable pmax=numpnts(w_d)-2  // pmax is numpnts(w_m)-1 = numpnts(w_d)-2
    
    w_a[1,pmax] = (1+(w_delx[p-1])/(w_delx[p-1]+w_delx))/3  
    
    // Brodlie modification of Butland formula (for same sign slopes)   
    w_d [1,pmax] = ( w_m!=0 && w_m[p-1]!=0 && (sign(w_m)==sign(w_m[p-1])) ) ? w_m[p-1]*w_m/(w_a*w_m[p-1]+(1-w_a)*w_m) : 0
        
    // deal with ends
    w_d[0]=SBL_EndDerivativePCHIP(w_delx[0], w_delx[1], w_m[0], w_m[1])
    w_d[pmax+1]=SBL_EndDerivativePCHIP(w_delx[pmax], w_delx[pmax-1], w_m[pmax], w_m[pmax-1])
    
    // clean up any division by zero errors
    w_d = numtype(w_d)==0 ? w_d : 0
            
    return w_d
end
 
// one-sided three-point estimate for the derivative adjusted to be shape
// preserving
static function SBL_EndDerivativePCHIP(delX0, delX1, m0, m1)
    variable delX0, delX1, m0, m1
    
    variable derivative
    
    derivative = ( m0*(2*delX0+delX1) - delX0*m1 ) / (delX0+delX1)
    if(sign(derivative)!=sign(m0))
        derivative=0
    elseif( (sign(m0)!=sign(m1)) && (abs(derivative)>3*abs(m0)) )
        derivative=3*m0
    endif
    return derivative
end
 
threadsafe static function SBL_InterpolatePCHIP(w_nodesX, w_nodesY, w_nodesM, x)
    wave w_nodesX, w_nodesY, w_nodesM
    variable x
        
    variable p0, p1
    if(x<wavemin(w_nodesX))
        p0=0
    elseif(x>=wavemax(w_nodesX))
        p0=numpnts(w_nodesX)-2
    else
        findlevel /P/EDGE=1/Q w_nodesX, x
        if(v_flag==1) // not found          
            return nan
        endif
        p0=floor(v_levelX)
    endif
    
    p1=p0+1     
    return SBL_InterpolateHermite(x, w_nodesX[p0], w_nodesY[p0], w_nodesX[p1], w_nodesY[p1], w_nodesM[p0], w_nodesM[p1])
end
 
// https: en.wikipedia.org/wiki/Cubic_Hermite_spline
// calculate cubic function f(x) such that f(x1)=y1, f(x2)=y2, f'(x1)=m1 and f'(x2)=m2
threadsafe static function SBL_InterpolateHermite(x, x0, y0, x1, y1, m0, m1)
    variable x, x0, y0, x1, y1, m0, m1
    
    variable t=(x-x0)/(x1-x0)
    m0*=(x1-x0)
    m1*=(x1-x0)
    return y0*(2*t^3-3*t^2+1) + m0*(t^3-2*t^2+t) + y1*(-2*t^3+3*t^2) + m1*(t^3-t^2) 
end