Add replacement tree building functionality
Add and organize unit tests