Updated Path finding

Minor fix, but lots of commenting