Fixed bugs, added old implementation for comparison.