Large bugfix on BTreeMap. Add unit tests and fix some bugs.