Initial functionality for diag_add_margin.py
Initial functionality for diag_add_margin.py, continued