Cleaned up and tweaked code.
Also added comments (who wants those anyway though)
Maybe this time I'll stop and do my discrete homework.