Added path finder helper, path finding not working yet