def test_optimize():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
bd_con = (-0.01, 70., 30., np.pi/6, 0.01)
init_val = (0., 0., 100)
# init_val = (-0.0033333333333333335, 0.0033333333333333335, 78.775724936630581)
pp1 = TG.optimize(bd_con, init_val=init_val)
p1 = (bd_con[0], pp1[0], pp1[1], bd_con[4], pp1[2])
r1 = (TG.__a(p1), TG.__b(p1), TG.__c(p1), TG.__d(p1))
print('p1 = {0}'.format(p1))
# q0 = (0., 0., 0., -0.01)
# q1 = (70., 30., np.pi/6., 0.01)
# p2, r2 = TG.calc_path(cursor, q0, q1)
# print('p2 = {0}'.format(p2))
line1 = TG.spiral3_calc(p1, r=r1, q=(0.,0.,0.))
# line2 = TG.spiral3_calc(p2, r=r2, q=q0)
plt.plot(line1[:,1], line1[:,2], color='black', linewidth=4)
# plt.plot(line2[:,1], line2[:,2], color='green', linewidth=4)
plt.axis('equal')
# figManager = plt.get_current_fig_manager()
# figManager.window.showMaximized()
# plt.savefig('img/coordinate_transform.png',dpi=600)
plt.show()
cursor.close()
conn.close()
