# Path: lfd/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]


import numpy as np
import scipy.interpolate as si
from numpy import pi
from lfd.rapprentice import math_utils as mu
    if len(good_inds) == 1:
        return np.zeros(positions[0:1].shape)
    
    (tck, _) = si.splprep(good_positions.T,s = tol**2*(n+1), u=good_times, k=deg)
    #smooth_positions = np.r_[si.splev(times,tck,der=0)].T
    velocities = np.r_[si.splev(times,tck,der=1)].T    
    return velocities

def smooth_positions(positions, tol):
    times = np.arange(len(positions))
    positions = np.atleast_2d(positions)
    n = len(positions)
    deg = min(3, n - 1)
    
    good_inds = np.r_[True,(abs(times[1:] - times[:-1]) >= 1e-6)]
    good_positions = positions[good_inds]
    good_times = times[good_inds]
    
    if len(good_inds) == 1:
        return np.zeros(positions[0:1].shape)
    
    (tck, _) = si.splprep(good_positions.T,s = tol**2*(n+1), u=good_times, k=deg)
    smooth_positions = np.r_[si.splev(times,tck,der=0)].T
    return smooth_positions

def unif_resample(x,n,weights,tol=.001,deg=3):    
    x = np.atleast_2d(x)
    weights = np.atleast_2d(weights)
    x = mu.remove_duplicate_rows(x)
    x_scaled = x * weights

# Path: lfd/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]


# Path: lfd/rapprentice/kinematics_utils.py
import numpy as np
import scipy.interpolate as si
from numpy import pi
from lfd.rapprentice import math_utils as mu

    if len(good_inds) == 1:
        return np.zeros(positions[0:1].shape)
    
    (tck, _) = si.splprep(good_positions.T,s = tol**2*(n+1), u=good_times, k=deg)
    #smooth_positions = np.r_[si.splev(times,tck,der=0)].T
    velocities = np.r_[si.splev(times,tck,der=1)].T    
    return velocities

def smooth_positions(positions, tol):
    times = np.arange(len(positions))
    positions = np.atleast_2d(positions)
    n = len(positions)
    deg = min(3, n - 1)
    
    good_inds = np.r_[True,(abs(times[1:] - times[:-1]) >= 1e-6)]
    good_positions = positions[good_inds]
    good_times = times[good_inds]
    
    if len(good_inds) == 1:
        return np.zeros(positions[0:1].shape)
    
    (tck, _) = si.splprep(good_positions.T,s = tol**2*(n+1), u=good_times, k=deg)
    smooth_positions = np.r_[si.splev(times,tck,der=0)].T
    return smooth_positions

def unif_resample(x,n,weights,tol=.001,deg=3):    
    x = np.atleast_2d(x)
    weights = np.atleast_2d(weights)
    x = mu.remove_duplicate_rows(x)
    x_scaled = x * weights

    dl = mu.norms(x_scaled[1:] - x_scaled[:-1],1)