Hi!

There was some of your code I did not understand.

This line of code:
theta = Sgn(dy) * ArkTangent(dy, dx) + phi

What sort of a function is ArkTangent?
It is not anywhere in the code you posted