Refactored, added path costs to trees

Refactored some things to avoid duplication, added the path costs onto
the tree. They are only drawn if the path costs list is not empty (i.e
user has inputted values to use). Added tests for new search algorithms.