final version of algorithm, also improved performance