# 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 bulletsimpy
import numpy as np
import trajoptpy
from lfd.rapprentice import math_utils, retiming
        from openravepy import matrixFromAxisAngle
    return True

def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1):
    """retime a trajectory so that it executes slowly enough for the simulation"""
    cart_traj = np.empty((len(traj), 6))
    finger_traj = np.empty((len(traj),2))
    leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator("rightarm")
    with robot:
        for i in range(len(traj)):
            robot.SetDOFValues(traj[i], inds)
            cart_traj[i,:3] = leftarm.GetTransform()[:3,3]
            cart_traj[i,3:] = rightarm.GetTransform()[:3,3]
            finger_traj[i,:1] = robot.GetDOFValues(leftarm.GetGripperIndices())
            finger_traj[i,1:] = robot.GetDOFValues(rightarm.GetGripperIndices())

    times = retiming.retime_with_vel_limits(np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6),np.repeat(max_finger_vel,2)])
    times_up = np.linspace(0, times[-1], times[-1]/upsample_time) if times[-1] > upsample_time else times
    traj_up = math_utils.interp2d(times_up, times, traj)
    return traj_up

def observe_cloud(pts, radius, upsample=0, upsample_rad=1):
    """
    If upsample > 0, the number of points along the rope's backbone is resampled to be upsample points
    If upsample_rad > 1, the number of points perpendicular to the backbone points is resampled to be upsample_rad points, around the rope's cross-section
    The total number of points is then: (upsample if upsample > 0 else len(self.rope.GetControlPoints())) * upsample_rad
    """
    if upsample > 0:
        lengths = np.r_[0, np.apply_along_axis(np.linalg.norm, 1, np.diff(pts, axis=0))]
        summed_lengths = np.cumsum(lengths)
        assert len(lengths) == len(pts)

# 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/ropesim.py
import bulletsimpy
import numpy as np
import trajoptpy
from lfd.rapprentice import math_utils, retiming
        from openravepy import matrixFromAxisAngle

    return True

def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1):
    """retime a trajectory so that it executes slowly enough for the simulation"""
    cart_traj = np.empty((len(traj), 6))
    finger_traj = np.empty((len(traj),2))
    leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator("rightarm")
    with robot:
        for i in range(len(traj)):
            robot.SetDOFValues(traj[i], inds)
            cart_traj[i,:3] = leftarm.GetTransform()[:3,3]
            cart_traj[i,3:] = rightarm.GetTransform()[:3,3]
            finger_traj[i,:1] = robot.GetDOFValues(leftarm.GetGripperIndices())
            finger_traj[i,1:] = robot.GetDOFValues(rightarm.GetGripperIndices())

    times = retiming.retime_with_vel_limits(np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6),np.repeat(max_finger_vel,2)])
    times_up = np.linspace(0, times[-1], times[-1]/upsample_time) if times[-1] > upsample_time else times
    traj_up = math_utils.interp2d(times_up, times, traj)
    return traj_up

def observe_cloud(pts, radius, upsample=0, upsample_rad=1):
    """
    If upsample > 0, the number of points along the rope's backbone is resampled to be upsample points
    If upsample_rad > 1, the number of points perpendicular to the backbone points is resampled to be upsample_rad points, around the rope's cross-section
    The total number of points is then: (upsample if upsample > 0 else len(self.rope.GetControlPoints())) * upsample_rad
    """
    if upsample > 0:
        lengths = np.r_[0, np.apply_along_axis(np.linalg.norm, 1, np.diff(pts, axis=0))]
        summed_lengths = np.cumsum(lengths)
        assert len(lengths) == len(pts)

        pts = math_utils.interp2d(np.linspace(0, summed_lengths[-1], upsample), summed_lengths, pts)