Maze improvements, KDTrees, implementation of other stuff for future paper