Cleaned up code. Address TODOs and Testing.