This is essentially a copypaste from the Gcode project, although I've made a couple changes and will probably make more.