fig = plt.figure(1, figsize=(10,10))
fig.clf()
ax = fig.add_subplot(111)

#Loading dataset files
left_250_20 = sorted(glob.iglob('./250_20/left_new/*.log'))

legends3 = []
last_positions = np.zeros((len(left_250_20), 2))

for file in range(len(left_250_20)):
    legends3.append("{} trace".format(file))
    f = open(left_250_20[file],"r")
    lines = f.readlines()
    x = []
    y = []

    for i in lines:
        x.append(i.split(' ')[2])
        y.append(i.split(' ')[3])
    f.close()

    x = np.asarray(x, dtype=float)
    x = -1*x + x[0]
    y = np.asarray(y, dtype=float)
    y = -1*y + y[0]
    last_positions[file, 0] = x[-1]
    last_positions[file, 1] = y[-1]
    ax.plot(x,y, marker='o', linestyle='--', markersize=5)

average_x = np.average(last_positions[:,0])
average_y = np.average(last_positions[:,1])
difference = np.sqrt((average_x - trajectory_250_20_l[-1,0])**2 + (average_y - trajectory_250_20_l[-1,1])**2)
print(difference)
ax.plot(trajectory_250_20_l[:,0], trajectory_250_20_l[:,1], marker='o', linestyle='--', markersize=15)
ax.plot(average_x, average_y, marker='o', linestyle='--', markersize=15)
plt.xlabel('x (mm)',fontsize=34)
plt.ylabel('y (mm)',fontsize=34)
plt.xticks(fontsize=24)
plt.yticks(fontsize=24)
x_start, x_end = ax.get_xlim()
y_start, y_end = ax.get_ylim()
plt.axis('equal')
plt.grid()
plt.savefig('left_250_20.png')
plt.show()