Uniform cost search

Not tested, but here is my code.