Fix algo.
Merge pull request #2 from nouiz/GSoC2014_part2

Fix algo. I added a todo. I'll make other comments in the PR.