Correct segfault and optimize code