Made the maths for angle calculation
Edited the math