getting further in EstDim C++, but still has bugs