optimize Algorithm and fix bug