Added in a rough debug mode

Debug mode displays the grids. Still trying to see how I can colour the
grids according to the path.