Bug fix and new algorithm