Slightly optmized algorithm