Fixed to use lazy ST monads and cleaned up