added debug prints, fixed bug with angle calculation