from TelloModule import TelloModule
import time
import matplotlib.pyplot as plt
import matplotlib.animation as animation


mod = TelloModule()
fig = plt.figure()
plt.ion()

x = 100
y = 150
line1 = plt.plot([x, 0],[0, y], c = 'b')
line2 = plt.plot([-x, 0],[0, y], c = 'b')
line3 = plt.plot([-x, 0],[0, -y], c = 'b')
line4 = plt.plot([x, 0],[0, -y], c = 'b')
line5 = plt.plot([-x/3, x/3], [y-y/3, y-y/3], c = 'b')

mod.module_init()

point = [None, None, None, None]
while 1:
    if (point[0] != None): 
        for p in point:
            p.remove()
    point[0] = plt.scatter(0, mod.get_forward_dist(), c = 'r')
    point[1] = plt.scatter(mod.get_right_dist(), 0, c = 'r')
    point[2] = plt.scatter(-mod.get_left_dist(), 0, c = 'r')
    point[3] = plt.scatter(0, -mod.get_back_dist(), c = 'r')

    plt.pause(0.1)

plt.ioff()    
plt.show()
mod.module_deinit()