Stage 1 of Path finding complete - some testing still required