Hi,
I use mach4 and an ESS and have done for seven years, and the combination will drive your four axis machine no trouble.
Mach4 has a surface mapping module called mcSurface. Its really meant to probe the nominally flat bed of your machine for small deviations from 'flat' and then to process
that data such that the machine automatically compensates for the bed not being perfectly flat. This may not be what you had in mind.
I use a software (freeware) utility called Autoleveller that was intended to probe PCB blanks and compensate (via Gdcode edit) for a not perfectly flat PCB blank.
This software can be used in creative ways and may be adapted to your requirement. The software is open source so you could modify it or rewrite it at will.
There is another freeware software solution, the name of which escapes me at the moment, that uses similar ideas to Autoleveller that may be even better suited to
adaption to a particular purpose.
Craig