Fixed some code.

Removed unnecessary loop.
Added debug toggle.
Used vector magnitude as go-signal basis instead of x-axis.