Finished search algorithm, still debugging