Fix issues in offset calculation with margin

An experimental fix for margin offset calculation issue.
Closes #7