''' Module provides functions for points interpolation using splines of 3rd order '''

import numpy as np
from CRS_commander import Commander
from matplotlib import pyplot as plt


def circle_points(c, #type: Commander
             x=500, y0=250, z0=500, r=50, step=5):

    sol = np.array([[0]*6])
    prev_a = None
    rng = 360/step
    for i in range(rng+1):
        y = y0 + r*np.cos((i*step)/180.0*np.pi)
        z = z0 + r*np.sin((i*step)/180.0*np.pi)
        pos = [x, y, z, 0, 0, 0]
        # print('pos', pos)
        # print('i', i)
        prev_a = move_to_pos(c, pos, prev_a)
        sol = np.append(sol, [prev_a], axis=0)
    sol = np.delete(sol, 0, 0)
    return sol

def move_to_pos(c, pos, prev_pos=None):
    a = c.robot.ikt(c.robot, pos)
    valid_lst = []
    num = -1
    for i in range(len(a)):
        irc = c.anglestoirc(a[i])
        validm = irc > c.robot.bound[0]
        validp = irc < c.robot.bound[1]
        valid = np.logical_and(validm, validp)
        if np.all(valid):
            num = i if num == -1 else num
            valid_lst.append(i)

    if prev_pos is not None:
        min_dist = 10000
        for i, s in enumerate(a):
            dist = np.linalg.norm(np.array(a[i]) - prev_pos)
            if dist < min_dist and i in valid_lst:
                min_dist = dist
                num = i
    # print c.anglestoirc(a[num])
    return c.anglestoirc(a[num])


def cubic_spline(x):
    dim = len(x[0])
    v0 = (4 * x[1] - 3 * x[0] - x[2])/2
    vn = (4 * x[-2] - 3 * x[-1] - x[-3]) / 2

    b = -3 * x[1:-3] + 3* x[3:-1]
    b0 = -3 * x[0] + 3 * x[2] - v0
    bn = -3 * x[-3] + 3 * x[-1] - vn
    b = np.insert(b, 0, b0, axis=0)
    b = np.append(b, [bn], axis=0)
    A = np.eye(x.shape[0] - 2) * 4
    i, j = np.indices(A.shape)
    A[i == j - 1] = 1
    A[i - 1 == j] = 1
    v1ton = np.linalg.inv(A).dot(b)
    v = np.insert(v1ton, 0, v0, axis=0)
    v = np.append(v, [vn], axis=0)

    k0 = [[0]*dim, x[1] - x[0] - v0, v0, x[0]]
    kn = [[0]*dim, -x[-2] +x[-1] + vn, -vn, x[-2]]
    A = 2 * x[1:-2] - 2 * x[2:-1] + v[2:-1] + v[1:-2]
    B = -3 * x[1:-2] + 3 * x[2:-1] - v[2:-1] - 2 * v[1:-2]
    C = v[1:-2]
    D = x[1:-2]

    lst = [A, B, C, D]
    # k = np.vstack([k0, np.hstack([A, B, C, D]), kn])
    for i, m in enumerate(lst):
        lst[i] = np.insert(lst[i], 0, k0[i], axis=0)
        lst[i] = np.append(lst[i], [kn[i]], axis=0)

    [A, B, C, D] = lst
    param_lst = []
    for i in range(len(lst[0])):
        param = np.vstack((C[i], B[i], A[i]))
        param = np.reshape(param.T, [1, 18], order='C')
        param_lst.append(list(param[0]))

    np.save('param', param_lst)
    # print(param_lst)
    return lst

def interpolate(points, graph=False):
    res = 100 #step of x axis

    [A, B, C, D] = cubic_spline(points)

    if graph:
        t = np.arange(0, points.shape[0])
        x = []
        t2 = np.linspace(0, 1, res)
        N = len(A)
        for K in range(N):
            for i in range(len(t2)):
                x.append(np.array(A[K])*t2[i]**3 + np.array(B[K])*t2[i]**2 + np.array(C[K])*t2[i]+ D[K])
        x = np.array(x)[np.newaxis].T
        t3 = np.linspace(0, N, N*res)
        plt.plot(t, points.T[0], t, points.T[1], t, points.T[2], t, points.T[3], t, points.T[4], t, points.T[5],
                 t3, x[0], t3, x[1], t3, x[2], t3, x[3], t3, x[4], t3, x[5] )

        plt.show()

@@ -108,1298 +108,39 @@
 rom 
-CRS_commander import Commander%0Afrom matplotlib import pyplot as plt%0A%0A%0Adef circle_points(c, #type: Commander%0A             x=500, y0=250, z0=500, r=50, step=5):%0A%0A    sol = np.array(%5B%5B0%5D*6%5D)%0A    prev_a = None%0A    rng = 360/step%0A    for i in range(rng+1):%0A        y = y0 + r*np.cos((i*step)/180.0*np.pi)%0A        z = z0 + r*np.sin((i*step)/180.0*np.pi)%0A        pos = %5Bx, y, z, 0, 0, 0%5D%0A        # print('pos', pos)%0A        # print('i', i)%0A        prev_a = move_to_pos(c, pos, prev_a)%0A        sol = np.append(sol, %5Bprev_a%5D, axis=0)%0A    sol = np.delete(sol, 0, 0)%0A    return sol%0A%0Adef move_to_pos(c, pos, prev_pos=None):%0A    a = c.robot.ikt(c.robot, pos)%0A    valid_lst = %5B%5D%0A    num = -1%0A    for i in range(len(a)):%0A        irc = c.anglestoirc(a%5Bi%5D)%0A        validm = irc %3E c.robot.bound%5B0%5D%0A        validp = irc %3C c.robot.bound%5B1%5D%0A        valid = np.logical_and(validm, validp)%0A        if np.all(valid):%0A            num = i if num == -1 else num%0A            valid_lst.append(i)%0A%0A    if prev_pos is not None:%0A        min_dist = 10000%0A        for i, s in enumerate(a):%0A            dist = np.linalg.norm(np.array(a%5Bi%5D) - prev_pos)%0A            if dist %3C min_dist and i in valid_lst:%0A                min_dist = dist%0A                num = i%0A    # print c.anglestoirc(a%5Bnum%5D)%0A    return c.anglestoirc(a%5Bnum%5D)
+matplotlib import pyplot as plt
 %0A%0A%0Ad
@@ -216,17 +216,19 @@
  - x%5B2%5D)
-/
+ / 
 2%0A    vn

Remove duplicit functions.