Add arg-sample code

@mjhubisz

This PR addresses issue #9 by merging the code needed for `arg-sample` into master.


Probably want to pass by reference here and with const.  You could have a big speed up since this function is called so often.