Add angle calculation and its test