Change algorithm more robustly and faster, previous way was buggy