Implements vectorized version, Tests pass
Makes code more in line with version at master