Got an rrt to visualize - should clean up the code