Cleaned up the algorithm