Changed code to be closer to D. Wilson's original article; Added C implementation