local inst = mc.mcGetInstance()

package.path = wx.wxGetCwd() .. "\\Profiles\\Mach4Mill\\Modules\\?.lua;"
if(package.loaded.ToolChangePositions == nil) then
tcp = require "ToolChangePositions"
end



function m6()
------ Compara utensile attuale con utensile selezionato ------

--if (SelectedTool == 0) then
--mc.mcCntlMdiExecute(inst, "m630")
--end





local SelectedTool = mc.mcToolGetSelected(inst)
local CurrentTool = mc.mcToolGetCurrent(inst)
if (SelectedTool == CurrentTool) then
mc.mcCntlSetLastError(inst, "Prossimo Utensile = Utensile Attuale")



---------------------




do return end
end
------ Acquisisci lo stato attuale ------
local CurFeed = mc.mcCntlGetPoundVar(inst, 2134)
local CurFeedMode = mc.mcCntlGetPoundVar(inst, 4001)
local CurAbsMode = mc.mcCntlGetPoundVar(inst, 4003)

------ Acquisisci posizione utensile attuale ------
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, "ERRORE: Utensile selezionato non esiste!")
do return end
end
------ Acquisisci posizione prossimo utensile ------
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, "ERRORE: Utensile selezionato non esiste!")
do return end
end


------- Controllo Mandrino fermo -------
rc = mc.mcSignalGetState(inst, mc.ISIG_INPUT15)
if (rc == 0) then
    rc = mc.mcSignalWait(inst, mc.ISIG_INPUT15, mc.WAIT_MODE_HIGH, 5)
        if (rc ~= 0) then
            wx.wxMessageBox("Mandrino acceso, cambio utensile non possibile")
            mc.mcCntlCycleStop(inst) 
        end
end





------ Vai alla posizione di sgancio utensile attuale ------
local GCode = ""
GCode = GCode .. "G00 G90 G53 Z0.0\n"                                                  ---- Z zero
GCode = GCode .. "G00 G90 G53 Y-100.0\n"                                               ---- Y sicurezza
mc.mcCntlGcodeExecuteWait(inst, GCode)

local inst = mc.mcGetInstance() 
	local hsig, rc = mc.mcSignalGetHandle(inst, mc.OSIG_OUTPUT22)
	mc.mcSignalSetState(hsig, true); 

------- Slitta avanti -----

local GCode = ""
GCode = GCode .. string.format("G00 G90 G53 X%.4f \n", XPos1)                        ---- Pos. X Y sicurezza
GCode = GCode .. string.format("G00 G90 G53 X%.4f Y%.4f \n", XPos1, YPos1 - 50 )     ---- Pos. X Y sicurezza
GCode = GCode .. string.format("G00 G90 G53 Z%.4f \n", ZPos1+1)                      ---- Avvicinamento quota Z
GCode = GCode .. string.format("G01 G90 G53 Z%.4f F100\n", ZPos1)                    ---- Posizione Z cambio utensile
GCode = GCode .. string.format("G01 G90 G53 Y%.4f F1000.0\n", YPos1)                 ---- Posizione Y cambio utensile

mc.mcCntlGcodeExecuteWait(inst, GCode)


------ Sbloccaggio Utensile ------
local DrawBarOut = mc.OSIG_OUTPUT20
local hsig = mc.mcSignalGetHandle(inst, DrawBarOut)
mc.mcSignalSetState(hsig, 1)

-----  Controllo utensile sganciato -----
rc = mc.mcSignalWait(inst, mc.ISIG_INPUT13, mc.WAIT_MODE_HIGH, 5)
if (rc ~= 0) then
    wx.wxMessageBox("Pinza non aperta, cambio utensile interrotto")
   mc.mcCntlCycleStop(inst)
end

 






------ Movimento in sicurezza ------
GCode = ""
GCode = GCode .. string.format("G4 P100\n G90" )                                    ------ Pausa 
GCode = GCode .. string.format("G00 G90 G53 Z%.4f\n ", ZPos2 + 30.0)                ------ Quota chiusura aria
mc.mcCntlGcodeExecuteWait(inst, GCode)                          

------ Bloccaggio pinza ------

local DrawBarOut = mc.OSIG_OUTPUT20
local hsig = mc.mcSignalGetHandle(inst, DrawBarOut)
mc.mcSignalSetState(hsig, 0)

------ Controllo pinza chiusa --------

rc = mc.mcSignalWait(inst, mc.ISIG_INPUT13, mc.WAIT_MODE_LOW, 5)
if (rc ~= 0) then
    wx.wxMessageBox("Pinza non chiusa, cambio utensile interrotto")
   mc.mcCntlCycleStop(inst)
end


----  Controllo utensile non presente  ---------

rc = mc.mcSignalWait(inst, mc.ISIG_INPUT18, mc.WAIT_MODE_HIGH, 5)
if (rc ~= 0) then
    wx.wxMessageBox("Utensile non sganciato, cambio utensile interrotto")
   mc.mcCntlCycleStop(inst) 
end


---------- Movimento alla posizione successiva    ---------
GCode = ""
GCode = GCode .. "G00 G90 G53 Z0.0\n"                                               ---- Z zero                            
GCode = GCode .. string.format("G00 G90 G53 X%.4f Y%.4f\n", XPos2, YPos2)           ---- quota posizione selezionata
GCode = GCode .. string.format("G00 G90 G53 Z%.4f\n", ZPos2 + 30.0)                 ---- Avvicinamento in Z
mc.mcCntlGcodeExecuteWait(inst, GCode)




--------- Apertura pinza  ----------

local DrawBarOut = mc.OSIG_OUTPUT20
local hsig = mc.mcSignalGetHandle(inst, DrawBarOut)
mc.mcSignalSetState(hsig, 1)


rc = mc.mcSignalWait(inst, mc.ISIG_INPUT13, mc.WAIT_MODE_HIGH, 5)
if (rc ~= 0) then
    wx.wxMessageBox("Pinza non Aperta, cambio utensile interrotto")
   mc.mcCntlCycleStop(inst)
end


-------- Aggancio nuovo utensile   -------------


GCode = ""
GCode = GCode .. string.format("G4 P100\n G90", ZPos2)                             ---- attesa apertura pinza
GCode = GCode .. string.format("G01 G90 G53 Z%.4f F1000.0\n", ZPos2+5)               ---- Posizione aggancio Z
GCode = GCode .. string.format("G01 G90 G53 Z%.4f F200.0\n", ZPos2)                 ---- Posizione aggancio Z
GCode = GCode .. string.format("G4 P100\n G90", ZPos2)                             ---- Pausa stabilizzazione             
mc.mcCntlGcodeExecuteWait(inst, GCode)


------ Bloccaggio utensile ------
local DrawBarOut = mc.OSIG_OUTPUT20
local hsig = mc.mcSignalGetHandle(inst, DrawBarOut)
mc.mcSignalSetState(hsig, 0)


------- Controllo pinza chiusa ------------

rc = mc.mcSignalGetState(inst, mc.ISIG_INPUT13)
if (rc ~= 0) then
    wx.wxMessageBox("Pinza non chiusa, cambio utensile interrotto")
   mc.mcCntlCycleStop(inst)
end


----- Controllo Utensile agganciato correttamente  -----------

rc = mc.mcSignalGetState(inst, mc.ISIG_INPUT14)
if (rc ~= 1) then
rc = mc.mcSignalWait(inst, mc.ISIG_INPUT14, mc.WAIT_MODE_HIGH, 2)
if (rc ~= 0) then
    wx.wxMessageBox("Utensile non Agganciato correttamente, cambio utensile interrotto")
   mc.mcCntlCycleStop(inst)
end
   
end

---------- Movimento svincolo

GCode = ""
GCode = GCode .. string.format("G4 P100\n")                        
GCode = GCode .. string.format("G01 G90 G53 Y%.4f F1000.0\n", YPos2 -50)
GCode = GCode .. string.format("G01 G90 G53 Z%.4f F1000.0\n", ZPos2 +20)                       
mc.mcCntlGcodeExecuteWait(inst, GCode)

--------- Richiamo slitta -------

local inst = mc.mcGetInstance() 
	local hsig, rc = mc.mcSignalGetHandle(inst, mc.OSIG_OUTPUT22)
	mc.mcSignalSetState(hsig, false); 


--------  Muovi a posizione di sicurezza -----

GCode = ""
GCode = GCode .. string.format("G00 G90 G53 Z0 \n")                       
mc.mcCntlGcodeExecuteWait(inst, GCode)


-------- Verifica che l'utensile sia presente -----------

rc = mc.mcSignalWait(inst, mc.ISIG_INPUT18, mc.WAIT_MODE_LOW, 3)
if (rc ~= 0) then
    wx.wxMessageBox("Utensile non Presente, cambio utensile interrotto")
   mc.mcCntlCycleStop(inst) 
end




------ Reset state ------
mc.mcCntlSetPoundVar(inst, 2134, CurFeed)
mc.mcCntlSetPoundVar(inst, 4001, CurFeedMode)
mc.mcCntlSetPoundVar(inst, 4003, CurAbsMode)
------ Set new tool ------
mc.mcToolSetCurrent(inst, SelectedTool)
mc.mcCntlSetLastError(inst, string.format("Tool change - Tool: %.0f", SelectedTool))

---------- imposta compensazione lunghezza utensile -------

GCode = ""
GCode = GCode .. string.format("G43 H%.0f", SelectedTool)
mc.mcCntlGcodeExecuteWait(inst, GCode)

end

if (mc.mcInEditor() == 1) then
m6()
end