import matplotlib.pyplot as plt
import ev3_dc as ev3

MAC = '00:16:53:50:8A:86'

plt.ion()
fig, ax = plt.subplots()
ax.figure.set_size_inches(5, 5)
ax.grid(True)

ax.set_xlim([-1.1, 1.1])
ax.set_xticks([-1, -0.5, 0.5, 1])
ax.spines['left'].set_position('zero')
ax.spines['right'].set_color('none')

ax.set_ylim([-1.1, 1.1])
ax.set_yticks([-1, -0.5, 0.5, 1])
ax.spines['bottom'].set_position('zero')
ax.spines['top'].set_color('none')

x_values = [0.0,]
y_values = [0.0,]
pos = ax.plot(0.0, 0.0, 'ro')[0]
line = ax.plot(x_values, y_values, 'r-')[0]
fig.canvas.draw()

def plot_curr_pos(curr_pos: ev3.VehiclePosition):
    '''
    updates pos in plot
    '''
    x_values.append(curr_pos.x)
    y_values.append(curr_pos.y)
    line.set_xdata(x_values)
    line.set_ydata(y_values)
    pos.set_xdata([curr_pos.x])
    pos.set_ydata([curr_pos.y])
    fig.canvas.flush_events()
    

with ev3.TwoWheelVehicle(
    0.01518,  # radius_wheel
    0.11495,  # tread
    protocol=ev3.BLUETOOTH,
    host=MAC,
    tracking_callback=plot_curr_pos
) as vehicle:
    # Moverse en linea recta por 0.5 metros a velocidad 20%
    # Para la calibración, ajustar la velocidad si es necesario
    vehicle.drive_straight(0.5,speed=20).start(thread=False)
    plt.ioff()
    plt.pause(2)
    fig.savefig('differential-show-trajectory.png',
                dpi=300,
                bbox_inches='tight',
                transparent=False)