* progress, passes tests, needs more optimization * passes tests * minor * cleanup set diagonal elements of matrix * replace numeric consts with enum --------- Co-authored-by: ptosco <paolo.tosco@novartis.com>