Some refactor, fixes. Adding testcases.