Grid and tracking works.

Still need to define better end condition and search