I think the Add function is correct. 

Sub is only correct if Add is correct. 
This is draft 1
We should really move this out into its own function, since this is the same code as above.