local inst = mc.mcGetInstance()
package.path = wx.wxGetCwd() .. "\\Profiles\\AXYZ\\Modules\\?.lua;" 
if(package.loaded.ToolChangePositions == nil) then 
    tcp = require "ToolChangePositions" 
end

function  m6()
    mc.mcCntlSetLastError(inst, "Tool Change Started")

    ------ Get and compare next and current tools to see if we need to change tools------ 
    local SelectedTool = mc.mcToolGetSelected(inst) 
    local CurrentTool = mc.mcToolGetCurrent(inst) 
    mc.mcCntlSetLastError(inst, string.format("Tool change - From: %.0f To %.0f", CurrentTool, SelectedTool))
    if (SelectedTool == CurrentTool) then 
        mc.mcCntlSetLastError(inst, "Next tool = Current tool") 
        do return end 
    end

    ------ Get current state ------ 
    local CurFeed = mc.mcCntlGetPoundVar(inst, 2134) 
    local CurFeedMode = mc.mcCntlGetPoundVar(inst, 4001) 
    local CurAbsMode = mc.mcCntlGetPoundVar(inst, 4003)

    ------ Get position data for current tool ------
    ToolData = tcp.GetToolData(CurrentTool) 
    if (ToolData ~= nil) then 
        Num1 = ToolData.Tool_Number 
        XPos1 = ToolData.X_Position 
        YPos1 = ToolData.Y_Position 
        ZPos1 = ToolData.Z_Position 
    else 
        mc.mcCntlEStop(inst) 
        mc.mcCntlSetLastError(inst, "ERROR: Tool number out of range!") 
        do return end 
    end

    ------ Get position data for next tool ------ 
    ToolData = tcp.GetToolData(SelectedTool) 
    if (ToolData ~= nil) then 
        Num2 = ToolData.Tool_Number 
        XPos2 = ToolData.X_Position 
        YPos2 = ToolData.Y_Position 
        ZPos2 = ToolData.Z_Position 
    else 
        mc.mcCntlEStop(inst) 
        mc.mcCntlSetLastError(inst, "ERROR: Tool number out of range!") 
        do return end 
    end

    ------ Move to current tool change position ------ 
    local GCode = "" 
    GCode = GCode .. "G00 G90 G53 Z-1.0\n" 
    GCode = GCode .. string.format("G00 G90 G53 X%.4f Y%.4f\n", XPos1, YPos1) 
    GCode = GCode .. string.format("G00 G90 G53 Z%.4f\n", ZPos1) 
    --GCode = GCode .. string.format("G01 G90 G53 Z%.4f F15.0\n", ZPos1) 
    mc.mcCntlGcodeExecuteWait(inst, GCode)

    ------ Release the current tool ------
    ReleaseTool()

    ------ Move to next tool change position ------ 
    GCode = "" 
    ---GCode = GCode .. string.format("G01 G90 G53 Z%.4f\n F15.0", ZPos1 + 1.0) 
    --GCode = GCode .. "G00 G90 G53 Z-1.0\n" 
    GCode = GCode .. string.format("G00 G90 G53 X%.4f Y%.4f\n", XPos2, YPos2) 
    --GCode = GCode .. string.format("G00 G90 G53 Z%.4f\n", ZPos2 + 1.0) 
    --GCode = GCode .. string.format("G01 G90 G53 Z%.4f F15.0\n", ZPos2) 
    mc.mcCntlGcodeExecuteWait(inst, GCode)

    ------ Get the new tool ------
    GetTool()

    ------ Move Z to home position ------
    mc.mcCntlGcodeExecuteWait(inst, "G00 G90 G53 Z-0.50\n")

    ------ Reset state ------ 
    mc.mcCntlSetPoundVar(inst, 2134, CurFeed) 
    mc.mcCntlSetPoundVar(inst, 4001, CurFeedMode) 
    mc.mcCntlSetPoundVar(inst, 4003, CurAbsMode) 

    ------ Set new tool ------ 
    mc.mcToolSetCurrent(inst, SelectedTool) 
    CurrentTool = mc.mcToolGetCurrent(inst) 
    mc.mcCntlSetLastError(inst, string.format("Tool change - Tool: %.0f", CurrentTool))
end

function ReleaseTool()
    ------ Lower to drop off position ------
    local GCode = ""
    GCode = GCode .. "G00 G91 Z-3.25\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)

    ----- Open Drawbar ------
    local DrawbarOPEN = mc.mcSignalGetHandle(inst, mc.OSIG_OUTPUT1)
    mc.mcSignalSetState(DrawbarOPEN, 1)

    ------ Move to Clear position turn off drawbar------
    GCode = ""
    GCode = GCode .. "G00 G91 Z.75\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)
    mc.mcSignalSetState(DrawbarOPEN, 0)
    
    ------ Close Drawbar ------
    local DrawbarCLOSE = mc.mcSignalGetHandle(inst, mc.OSIG_OUTPUT2)
    mc.mcSignalSetState(DrawbarCLOSE, 1)
    mc.mcSignalSetState(DrawbarCLOSE, 0)

    ------ Move to clear tool position ------
    GCode = ""
    GCode = GCode .. "G00 G91 Z2.5\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)
end

function GetTool()
    local inst = 0;    
    ------ Lower to blow off start position ------
    local GCode = ""
    GCode = GCode .. "G00 G91 Z-2.0\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)
    
    ------ Turn on Blow off  ------
    local BlowOff = mc.mcSignalGetHandle(inst, mc.OSIG_OUTPUT0)
    mc.mcSignalSetState(BlowOff, 1)

    ------ Lower to pick up start position shut off blow off, turn on drawbar------
    local GCode = ""
    GCode = GCode .. "G00 G91 Z-.25\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)
    mc.mcSignalSetState(BlowOff, 0)
    local DrawbarOPEN = mc.mcSignalGetHandle(inst, mc.OSIG_OUTPUT1)
    mc.mcSignalSetState(DrawbarOPEN, 1)

    ------ Move to pickup position position ------
    GCode = ""
    GCode = GCode .. "G00 G91 Z-1\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)

    ------ Turn off  open drawbar ------
    mc.mcSignalSetState(DrawbarOPEN, 0)

    ------ Close Drawbar ------
    local DrawbarCLOSE = mc.mcSignalGetHandle(inst, mc.OSIG_OUTPUT2)
    mc.mcSignalSetState(DrawbarCLOSE, 1)
    GCode = ""
    GCode = GCode .. "G00 G91 Z.25\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)

    ------ Stop Closing drawbar ------
    mc.mcSignalSetState(DrawbarCLOSE, 0)

    ------ Move to final tool change position ------
    GCode = ""
    GCode = GCode .. "G00 G91 Z3\n"
    mc.mcCntlGcodeExecuteWait(0, GCode)
end

if(mc.mcInEditor() == 1) then
    -- If you are in the mcLua editor, this will cause your script to execute if you Run or Debug 
    -- Make sure your Capitalization and numbers match that of the function you are calling 
    m6()
end