Fixed segfault and algorithmic bug