M-step fixed, some refactoring