Lots of improvements, added test project, fixed code