Improved _pack by removing hstack and adding preallocated theta as parameter
 adding a preallocated theta as input and