Fixed some TODOs and added unit tests for organisation