NOTE:
I can save you some time, been there done that.........
You currently have two varibles: GetCurrentTool() and GetSelectedTool()
When you come to a M6, mach stores the current tool in current tool, and the new tool in selected tool.
Unfortunatly, (at current this current mach time), when you initially Parse the Gcode to build you Tool path there is now way to store a list and order of use of needed tools.
(Yes, I have asked Brian for a way, when I started mine).
For me, I put my current tool and next tool both before the M6, that way I can stay ahead of the game.
scott