Add unit tests, cleaned up code, code cvrg ~90%