Properly free resources
Restructure a bit
Optimise termination a little
Use one's complement to perform subtraction