Improve algorithm and stability