Fixing #856 master

This is basically #1040 for master and revert of #25.