Fix #168, Fix #1506

This should Fix #1506 and partially Fix #168 but depends on the reviewers. I think that if test pass it should be ready to merge.