Updated the algorithm, the first one was wrong, but I think this one is better