Algorithm cleanup and improvements