A* optimised by getting rid of collision check, utilizing Cspace, and initializing close_list
Depth first working
Breadth first, and Astar working with visualization