Clean up algorithms and add comments