Changed algorithm, added optimizations