Fixed G35 to emit the offset of the first probed point rather than the offset of the bed.

Fixed the moveTo algorithm to reliably find the next D line. Added (and confirmed) some sanity checks: the line equations of the segments match the whole, the distances of the line segments add up to the whole move length.
fixed sanity checks
added test probe file