Revert "Removed dead code."

This reverts commit bbed8a6d49061bcc7eb06ea4c5439fb5ff2fb164.
Fixed error in tests. Still need to check outputs for correct data shape.
Added shape check to tests. Not only the right data, but also the right
shape should be returned.