From f0c6d19bad71816f5c6d95803f734e77567931ea Mon Sep 17 00:00:00 2001
From: Maximilian Gruber <maximilian.gruber@ptb.de>
Date: Fri, 23 Apr 2021 12:44:24 +0200
Subject: [PATCH] feat(interp1d_unc): add cubic bspline interpolation-kind

---
 PyDynamic/uncertainty/interpolate.py | 82 +++++++++++++++++++++++++++-
 1 file changed, 80 insertions(+), 2 deletions(-)

diff --git a/PyDynamic/uncertainty/interpolate.py b/PyDynamic/uncertainty/interpolate.py
index ccb0267ca..864e8956d 100644
--- a/PyDynamic/uncertainty/interpolate.py
+++ b/PyDynamic/uncertainty/interpolate.py
@@ -12,7 +12,7 @@
 from typing import Optional, Tuple, Union
 
 import numpy as np
-from scipy.interpolate import interp1d
+from scipy.interpolate import interp1d, splrep, BSpline
 
 __all__ = ["interp1d_unc", "make_equidistant"]
 
@@ -60,7 +60,7 @@ def interp1d_unc(
             associated with y.
         kind : str, optional
             Specifies the kind of interpolation for y as a string ('previous',
-            'next', 'nearest' or 'linear'). Default is ‘linear’.
+            'next', 'nearest', 'linear' or 'cubic'). Default is ‘linear’.
         copy : bool, optional
             If True, the method makes internal copies of t and y. If False,
             references to t and y are used. The default is to copy.
@@ -303,6 +303,84 @@ def interp1d_unc(
                     (t_new[interp_range] - t_hi) ** 2 * uy_prev_sqr
                     + (t_new[interp_range] - t_lo) ** 2 * uy_next_sqr
                 ) / (t_hi - t_lo)
+    elif kind == "cubic":
+        # Calculate boolean arrays of indices from t_new which are outside t's bounds...
+        extrap_range_below = t_new < np.min(t)
+        extrap_range_above = t_new > np.max(t)
+        extrap_range = extrap_range_below | extrap_range_above
+        # .. and inside t's bounds.
+        interp_range = ~extrap_range
+
+        # Initialize the result array for the standard uncertainties.
+        uy_new = np.empty_like(y_new)
+
+        # Initialize the sensitivity matrix of shape (M, N)
+        C = np.zeros((len(t_new), len(uy)), "float64")
+
+        # First extrapolate the according values if required and then
+        # compute interpolated uncertainties following White, 2017.
+
+        # If extrapolation is needed, fill in the values provided via fill_unc.
+        if np.any(extrap_range):
+            # At this point fill_unc is either a float (np.nan is a float as well) or
+            # a 2-tuple of floats. In case we have one float we set uy_new to this value
+            # inside the extrapolation range.
+            if isinstance(fill_unc, float):
+                uy_new[extrap_range] = fill_unc
+            else:
+                # Now fill_unc should be a 2-tuple, which we can fill into uy_new.
+                uy_new[extrap_range_below], uy_new[extrap_range_above] = fill_unc
+
+            if returnC:
+                # In each row of C corresponding to an extrapolation value below the
+                # original range set the first column to 1 and in each row of C
+                # corresponding to an extrapolation value above the original range set
+                # the last column to 1. It is important to do this before
+                # interpolating, because in general those two columns can contain
+                # non-zero values in the interpolation range.
+                C[:, 0], C[:, -1] = extrap_range_below, extrap_range_above
+
+        # If interpolation is needed, compute uncertainties following White, 2017.
+        if np.any(interp_range):
+            # This following section is taken mainly from scipy.interpolate.interp1d to
+            # determine the indices of the relevant original timestamps (or frequencies)
+            # just for the interpolation range.
+            # --------------------------------------------------------------------------
+            # 2. Find where in the original data, the values to interpolate
+            #    would be inserted.
+            #    Note: If t_new[n] == t[m], then m is returned by searchsorted.
+            t_new_indices = np.searchsorted(t, t_new[interp_range])
+
+            # 3. Clip x_new_indices so that they are within the range of
+            #    self.x indices and at least 1.  Removes mis-interpolation
+            #    of x_new[n] = x[0]
+            t_new_indices = t_new_indices.clip(1, len(t) - 1).astype(int)
+
+            # 4. Calculate the uncertainty
+            # generate spline of sensitivities, this procedure is described by eq. (19) of White2017
+            F = []
+            for i in range(len(t)):
+                tf = t
+                xf = np.zeros_like(t)
+                xf[i] = 1.0
+                f_tck = splrep(tf, xf, k=3)
+                Fi = BSpline(*f_tck)
+                F.append(Fi)
+
+            # calculate sensitivity
+            C[interp_range] = np.array([Fi(t_new[interp_range]) for Fi in F]).T
+            C2 = np.square(C[interp_range])
+
+            # if at some point time-uncertainties are of interest, White2017 already provides the formulas
+            # ut = np.zeros_like(t)
+            # ut_new = np.zeros_like(t_new)
+            # a1 = np.dot(C2, np.square(uy))
+            # a2 = np.dot(C2, np.squeeze(np.square(interp_y._spline(t, nu=1))) * np.square(ut))
+            # a3 = np.square(np.squeeze(interp_y._spline(t_new, nu=1))) * np.square(ut_new)
+            # uy_new[interp_range] = np.sqrt(a1 - a2 + a3)
+
+            # without consideration of time-uncertainty
+            uy_new[interp_range] = np.sqrt(np.dot(C2, np.square(uy)))
     else:
         raise NotImplementedError(
             "%s is unsupported yet. Let us know, that you need it." % kind