"""
Animates distances and angle of lidar
Uses model-free algorithms to track grouping of points (objects/groups)
"""
from tracking.mock_lidar import MockLidar
import matplotlib.pyplot as plt
import numpy as np
import matplotlib.animation as animation
import tracking.lidar_loader as loader
import tracking.algorithms as alg


PORT_NAME = '/dev/ttyUSB0'
DMAX = 4000
IMIN = 0
IMAX = 50


def update_line(num, iterator, line):
    scan = next(iterator)
    # Now update the groups, and then update the maps with different colours for different groups.
    offsets = np.array([(np.radians(meas[1]), meas[2]) for meas in scan])
    line.set_offsets(offsets)
    intens = np.array([meas[0] for meas in scan])
    line.set_array(intens)
    return line,


def run():
    lidar = MockLidar(loader.load_scans_bytes_file("tracking/out.pickle"))
    fig = plt.figure()
    ax = plt.subplot(111, projection='polar')
    line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
                      cmap=plt.cm.Greys_r, lw=0)
    ax.set_rmax(DMAX)
    ax.grid(True)

    iterator = lidar.iter_scans()
    ani = animation.FuncAnimation(fig, update_line,
                                  fargs=(iterator, line), interval=50)
    plt.show()
    lidar.stop()
    lidar.disconnect()


if __name__ == '__main__':
    run()