Melhorias diversas

Fiz com que ele compile usando C, mas como não tem testes unitários, não tem como saber se está tudo certo!
Aproveitei e coloquei o algoritmo em um só lugar