Finished implementation, just fixing bugs.
Need to fix test case 5