Started working on step 5b of path planning.

Also updated variable names and step 4 logic to follow more closely the
algorithm laid out in the Shortest Path Planning research paper.