Algorithm is now correct. Termination is no very nice but it works.