Added functionality, fixed numerical errors