Fix bugs throughout algorithm implementation.