Fixed many bugs, added unit tests