I think the Add function is correct. 

Sub is only correct if Add is correct. 
This is draft 1