Updated notation and started implementing A*.

A shortest path algorithm is required for step 5bi