Fix bug. Can now pass unit test.
Fix header include.
Add new test.