Some clean up. TODO: Add if then else, better tests