Simplify argument substitution

Get rid of the helper functions, and use a single big pattern matching
case.
Add missing tests for substitution