Working version

All test case ran successfully
Need to remove debugging printf
May restructure code if time permits