Found bug that made path shorter but not shortest. Fixed it