Starting to add Template Method Pattern in add algorithm.
Need to implement iterator method.
Also started to fix AddNodeTest, but not done