Add Test-Main, tidy up code