import numpy as np


def barycentric_weights(x):
    """Compute barycentric weights for nodes x."""
    n = len(x)
    w = np.ones(n)
    for j in range(n):
        for k in range(n):
            if j != k:
                w[j] /= (x[j] - x[k])
    return w


def lagrange_interp_and_derivative(x_nodes, y_nodes, x_eval):
    """
    Evaluate Lagrange interpolation polynomial and its derivative
    at arbitrary points x_eval, including derivatives at node positions.

    Parameters
    ----------
    x_nodes : array_like
        Node positions (e.g., time in seconds)
    y_nodes : array_like
        Function values (e.g., satellite position, clock bias)
    x_eval : array_like
        Evaluation points (can include node positions)

    Returns
    -------
    y_interp : ndarray
        Interpolated values at x_eval
    dy_interp : ndarray
        Derivative values at x_eval
    """
    x_nodes = np.asarray(x_nodes)
    y_nodes = np.asarray(y_nodes)
    x_eval = np.asarray(x_eval)
    w = barycentric_weights(x_nodes)

    y_interp = np.zeros_like(x_eval, dtype=float)
    dy_interp = np.zeros_like(x_eval, dtype=float)

    for idx, x in enumerate(x_eval):
        diff = x - x_nodes

        # Case 1: evaluation point equals a node
        if np.any(diff == 0):
            j = np.where(diff == 0)[0][0]
            y_interp[idx] = y_nodes[j]

            # Derivative at node j (analytical formula)
            s = 0.0
            for k in range(len(x_nodes)):
                if k != j:
                    s += (w[k] / w[j]) * (y_nodes[j] - y_nodes[k]) / (x_nodes[j] - x_nodes[k])
            dy_interp[idx] = s
            continue

        # Case 2: general barycentric evaluation
        w_over_diff = w / diff
        w_over_diff_sq = w_over_diff / diff

        num = np.sum(w_over_diff * y_nodes)
        den = np.sum(w_over_diff)
        num_d = np.sum(w_over_diff_sq * y_nodes)
        den_d = np.sum(w_over_diff_sq)

        y_interp[idx] = num / den
        dy_interp[idx] = - num_d / den + (num * den_d) / (den ** 2)

    return y_interp, dy_interp

# Example nodes: 5-minute data
x_nodes = np.arange(0, 3600, 300)  # every 300s
y_nodes = np.sin(2 * np.pi * x_nodes / 3600)  # simple periodic function

# Evaluate every 30s, including node positions
x_eval = np.arange(0, 3600, 30)
y_interp, dy_interp = lagrange_interp_and_derivative(x_nodes, y_nodes, x_eval)

# Show example
for t, val, der in zip(x_eval[:5], y_interp[:5], dy_interp[:5]):
    print(f"{t:6.0f}s ->  P11(x)={val: .6f},  dP11(x)={der: .6f}")