import matplotlib.pyplot as plt
import numpy as np
from timeit import timeit

from autopilot.transform.geometry import Order_Points

# order all points, with no thresholded distance
orderer = Order_Points(1)

runs = 100
total_time = timeit(
    'points = orderer.process(points)',
    setup='points = np.column_stack([np.random.rand(100), np.random.rand(100)])',
    globals=globals(),
    number=runs
)
print(f'mean time per execution (ms): {total_time*1000/runs}')

points = np.column_stack([np.random.rand(100), np.random.rand(100)])
ordered_points = orderer.process(points)

# lower threshold!
orderer.closeness_threshold = 0.25
lowthresh_points = orderer.process(points)

fig, ax = plt.subplots(1,3)
ax[0].scatter(points[:,0], points[:,1])
ax[1].scatter(points[:,0], points[:,1])
ax[2].scatter(points[:,0], points[:,1])
ax[1].plot(ordered_points[:,0], ordered_points[:,1], c='r')
ax[2].plot(lowthresh_points[:,0], lowthresh_points[:,1], c='r')

ax[1].set_title('threshold = 1')
ax[2].set_title('threshold = 0.25')
plt.show()