def safe_subpath(path, altitudes, h):
    """
    Computes the maximum subpath of path along which the safety constraint is
    not violated
    Parameters
    ----------
    path: np.array
        Contains the nodes that are visited along the path
    altitudes: np.array
        1-d vector with altitudes for each node
    h: float
        Safety threshold

    Returns
    -------
    subpath: np.array
        Maximum subpath of path that fulfills the safety constraint

    """
    # Initialize subpath
    subpath = [path[0]]

    # Loop through path
    for j in range(len(path) - 1):
        prev = path[j]
        succ = path[j + 1]

        # Check safety constraint
        if altitudes[prev] - altitudes[succ] >= h:
            subpath = subpath + [succ]
        else:
            break
    return subpath