Working algorithm, cleaned up