Fix small bugs (leading to a whole rewrite of the algorithm ^^)