Reduced repeated code
Corrected conic calculation, fixed NPE and added tests