Worked out most of necessary functions and made a small test suite run

Still have most test coverage missing