Add tests and new function
Add reference poly implementation