Fixed bug
Try to account for numerical errors
Small refactoring
Fixed some inconsistencies