Test and Debug

Added a main function that tests the code.
Retuned the algorithm to work for all cases.
Fixed errors in the algorithm.