Fixed bugs from last commit, and improved implementation