Adicionando a fonte de onde utilizei o algoritmo
Corrigindo ciclo e algumas falhas no algoritmo