Binary tree algorithm with some tests