Hi Henrik. I've readed that you are working in some software to do the autoleveling in two steps, first by doing the measures and then by modifying the G-Code.
I'm Working exactly in the same thing and using the same aproach. For now I have ready the interpolation algorithm, as well as the measuring code. "All" I have left is to re-level the G-Code.
The first approach will be to adjust the Z coordinate for all the g-codes to take into account the interpolated level for that point, even if the gcode command doesn't have a Z coordinate for that point.(By saving the last Z coordinate seen in the code)
For a seccond step, I would like to detect large moves and split them to try to keep the leveling all along the segment.
And for a third step, I would like to process G2 and G3 command, so the arcs segments could be also leveled.
Regards!