Done recreating my work. Also added another A* that avoids blockades
effectively