print('Testing reshape_f')
ytrue = np.array([[0, 1, 2, 3, 4, 5], [3, 3, 6, 2, 9, 9]], dtype=float)
x = np.array([2, 3, 5, 6, 2, 9])
y = reshape_f(x, p0, v0, vf, psi0, psif, dt, deg)
if not np.all(y == ytrue):
print('--> [!] Test failed')
else:
print('--> Test passed')
print('Testing init_guess_f')
x0true = np.array([2, 3, 5, 5, 7, 9], dtype=float)
x0 = init_guess_f(p0, trgt, v0, vf, psi0, psif, dt, deg)
if not np.all(x0 == x0true):
print('--> [!] Test failed')
else:
print('--> Test passed')
print('Testing init_guess_m')
x0true = np.array([], dtype=float)
x0 = init_guess_m(p0, trgt, v0, vf, psi0, dt, deg, params)
c = Bezier(reshape_m(x0, p0, v0, psi0, dt, deg, trgt, params.innerR))
ax = c.plot()
ax.add_artist(Circle(trgt, radius=params.innerR, fill=None))
ax.set_aspect('equal')
# cpts1 = np.array([[0, 1, 2, 3, 4, 5],
# [3, 4, 6, 2, 7, 9]], dtype=float)
# cpts2 = np.array([[5, 4, 3, 2, 1, 0],
# [8, 3, 6, 6, 2, 5]], dtype=float)
