Added tests for flyToPosition
Some code cleanup