Implement actual rootfinder iteration loop
Same for determining the other coordinate first (radius, not height)
Some cleanup
Test output of both calculation variants to compare Python and C++
Testing stuff