# 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]


from collections import defaultdict
from lfd.rapprentice import math_utils, LOG
import multiprocessing
import numpy as np
        import trajoptpy, openravepy
    import snappy
        import traceback
        import traceback
    import cPickle
    while True:
        counter = 1
        crossings = defaultdict(list)
        # Walk along rope: for each segment, compute intersections with all other segments
        for i in range(len(ctl_pts) - 1):
            curr_seg = ctl_pts[i:i+2,:]
            intersections, ts, us = intersect_segs(ctl_pts[:,:2], curr_seg[:,:2])

            if len(intersections) == 0:
                continue
            if len(intersections) != 1:
                LOG.debug('warning: more than one intersection for segment %d, now upsampling', i)
                need_upsample_ind = i
                break

            # for each intersection, determine and record over/undercrossing
            i_int = intersections[0]
            if plotting:
                handles.append(env.drawlinestrip(ctl_pts[i_int:i_int+2], 5, [1, 0, 0]))
            int_point_rope = ctl_pts[i_int] + ts[i_int]*(ctl_pts[i_int+1] - ctl_pts[i_int])
            int_point_curr_seg = curr_seg[0] + us[i_int]*(curr_seg[1] - curr_seg[0])
            #assert np.allclose(int_point_rope[:2], int_point_curr_seg[:2])
            above = int_point_curr_seg[2] > int_point_rope[2]
            crossings[tuple(sorted((i, i_int)))].append(-counter if counter % 2 == 0 and above else counter)
            counter += 1
        if plotting: viewer.Idle()
        # upsample if necessary
        if need_upsample_ind is not None and upsample_iters < max_upsample_iters:
            spacing = np.linspace(0, 1, len(ctl_pts))
            new_spacing = np.insert(spacing, need_upsample_ind+1, (spacing[need_upsample_ind]+spacing[need_upsample_ind+1])/2.)

# 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/knot_identification.py
from collections import defaultdict
from lfd.rapprentice import math_utils, LOG
import multiprocessing
import numpy as np
        import trajoptpy, openravepy
    import snappy
        import traceback
        import traceback
    import cPickle

    while True:
        counter = 1
        crossings = defaultdict(list)
        # Walk along rope: for each segment, compute intersections with all other segments
        for i in range(len(ctl_pts) - 1):
            curr_seg = ctl_pts[i:i+2,:]
            intersections, ts, us = intersect_segs(ctl_pts[:,:2], curr_seg[:,:2])

            if len(intersections) == 0:
                continue
            if len(intersections) != 1:
                LOG.debug('warning: more than one intersection for segment %d, now upsampling', i)
                need_upsample_ind = i
                break

            # for each intersection, determine and record over/undercrossing
            i_int = intersections[0]
            if plotting:
                handles.append(env.drawlinestrip(ctl_pts[i_int:i_int+2], 5, [1, 0, 0]))
            int_point_rope = ctl_pts[i_int] + ts[i_int]*(ctl_pts[i_int+1] - ctl_pts[i_int])
            int_point_curr_seg = curr_seg[0] + us[i_int]*(curr_seg[1] - curr_seg[0])
            #assert np.allclose(int_point_rope[:2], int_point_curr_seg[:2])
            above = int_point_curr_seg[2] > int_point_rope[2]
            crossings[tuple(sorted((i, i_int)))].append(-counter if counter % 2 == 0 and above else counter)
            counter += 1
        if plotting: viewer.Idle()
        # upsample if necessary
        if need_upsample_ind is not None and upsample_iters < max_upsample_iters:
            spacing = np.linspace(0, 1, len(ctl_pts))
            new_spacing = np.insert(spacing, need_upsample_ind+1, (spacing[need_upsample_ind]+spacing[need_upsample_ind+1])/2.)

            ctl_pts = math_utils.interp2d(new_spacing, spacing, ctl_pts)