Final version

Tested with a test case. Should work out.