import matplotlib.pyplot as plt
import numpy as np

from autopilot.transform.geometry import Order_Points, Linefit_Prasad

fs, f, t = 2, 1/50, 200

x = np.arange(t*fs)/fs
y = (np.sin(2*np.pi*f*x)+(np.random.rand(len(x))*0.5-0.25))*50
points = np.column_stack([x,y])

orderer = Order_Points(closeness_threshold=0.2)
prasad  = Linefit_Prasad()

ordered = orderer.process(points)
segs    = prasad.process(ordered)

fig, ax = plt.subplots(2,1)
ax[0].scatter(x,y)
ax[1].scatter(x,y)
ax[0].plot(ordered[:,0], ordered[:,1], color='r')
ax[1].plot(segs[:,0], segs[:,1], color='r')
ax[1].scatter(segs[:,0], segs[:,1], color='y')
ax[0].set_title('ordered points')
ax[1].set_title('prasad fit line')
fig.tight_layout()
plt.show()