Fix algo + add debug mode