Feature/651

PR that rebase #651 . Should also fix #625