--[[
This header can not be removed

Copyright Newfangled Solutions (c) 2015 All Rights Reserved, www.machsupport.com  

THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, 
EITHER EXPRESSED OR IMPLIED , INCLUDING BUT NOT LIMITED TO THE IMPLIED WARRANTIES 
OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.

Author - Newfangled Solutions
Liscence - mit
Date - 07/08/2015
]]--
-- This is for a lathe tool changer with a output (currently assigned to 21) used to operate a solenoid that moves the tool changer away from the locked position
-- Modified on 3/25/2020 by Trevor

function m6()
	local inst = mc.mcGetInstance() --Get the instance of Mach4
	--local currentTool = mc.mcToolGetCurrent(inst) --looks for the tool currently selected
	--local selectedTool = mc.mcToolGetSelected(inst) -- looks at the tool you have selected
	local currentTool = 3				--FOR TESTING
	local selectedTool = 2				--FOR TESTING
	local currentpos = (currentTool  * 60)
	local tpos = (selectedTool * 60)
	if (selectedTool == 1) then
		mc.mcCntlGcodeExecuteWait(inst, string.format("G28.1 C1"))
		mc.mcAxisSetPos(inst, mc.C_AXIS, 60)
		return
	end
	if (selectedTool ~= currentTool) and (selectedTool ~= 1) then --Change tool: if the selected tool isnt = to the current tool, you want to change your tool
		if (selectedTool > currentTool) and (selectedTool ~= 1) then
			local movedist = (tpos - currentpos)
			mc.mcCntlSetLastError(inst, "selectedTool > currentTool")
			mc.mcAxisSetPos(inst, mc.C_AXIS, currentpos)
			mc.mcCntlGcodeExecuteWait(inst, string.format("G0 H" .. tostring(movedist))) --Reference, move to position, back up to lock position
			mc.mcCntlGcodeExecuteWait(inst, string.format("G0 H-5"))
			mc.mcToolSetCurrent(inst, selectedTool)
		elseif(selectedTool < currentTool) and (selectedTool ~= 1) then
			mc.mcCntlSetLastError(inst, "selectedTool < currentTool")
			mc.mcCntlGcodeExecuteWait(inst, string.format("G28.1 C1"))
			mc.mcAxisSetPos(inst, mc.C_AXIS, 60)
			mc.mcCntlGcodeExecuteWait(inst, string.format("G0 H" .. tostring(tpos))) --Reference, move to position, back up to lock position
			mc.mcCntlGcodeExecuteWait(inst, string.format("G0 H-5"))
			mc.mcToolSetCurrent(inst, selectedTool)
		end
	end

--This function is here to allow the debugging from the Lua editor
end
if (mc.mcInEditor() == 1) then
	m6()
end