--X360 Controller Script for X360Lua Plugin
-- ======================================================================
-- = Dont forget to add X360_Timer:Stop() to your screen unload script  =
-- ======================================================================

--------------------
-- INITIALIZATION --
--------------------

	local inst = mc.mcGetInstance()

	local X360_Panel = wx.wxPanel(wx.NULL, wx.wxID_ANY, wx.wxDefaultPosition, wx.wxDefaultSize)
	X360_Timer = wx.wxTimer(X360_Panel)
	local X360TS_Panel = wx.wxPanel(wx.NULL, wx.wxID_ANY, wx.wxDefaultPosition, wx.wxDefaultSize)
	X360TS_Timer = wx.wxTimer(X360TS_Panel)
	local X360BTN_Panel = wx.wxPanel(wx.NULL, wx.wxID_ANY, wx.wxDefaultPosition, wx.wxDefaultSize)
	X360BTN_Timer = wx.wxTimer(X360BTN_Panel)
	local X360DPAD_Panel = wx.wxPanel(wx.NULL, wx.wxID_ANY, wx.wxDefaultPosition, wx.wxDefaultSize)
	X360DPAD_Timer = wx.wxTimer(X360DPAD_Panel)
	
	local isPressed_BTN = {false,false,false,false,false,false,false,false,false} -- Buttons 0-3 = { A,B,X,Y } Buttons 4-7 = { DPAD: UP, DOWN, LEFT, RIGHT } Button 8 = { Btn_Back }
	local isSecured = 0 
	local isProcessingDPadButtonPressed = false

-------------------
-- CONFIGURATION --
-------------------

	local DeadZone = 25        --Based on controller output value (-99 => 99)
	local MaxJogSpeed = 50     --Percentage of axix max speed
	local MaxSlowJogSpeed = 15 --Percentage of axix max speed
	local LaserEnabled = 0

	local delayProbingUntilButtonPress = false
	
	local RTHY_MapToMashineAxis = mc.AXIS2 -- Z-AXIS
	local RTHX_MapToMashineAxis = mc.AXIS3 -- A-AXIS
	local LTHY_MapToMashineAxis = mc.AXIS1 -- Y-AXIS
	local LTHX_MapToMashineAxis = mc.AXIS0 -- X-AXIS

	local thumbSticks = {{"RTH_Y_Val", 1, true, RTHY_MapToMashineAxis}, {"RTH_X_Val", 2, true, RTHX_MapToMashineAxis}, {"LTH_Y_Val", 3, true, LTHY_MapToMashineAxis}, {"LTH_X_Val", 4, true, LTHX_MapToMashineAxis}}  --RegisterName, number, isInDeadZone, MashineAxisMapping

-----------------------------
-- MACHINE STATE FUNCTIONS --
-----------------------------

function isMachineReady(finst)
	
	local notEnabledMsg = "Error: Not Enabled - Machine needs to be enabled in order to continiue"
	local notHomedMsg   = "Error: Not Ref. to Home - Machine needs to have all enabled axis ref. to home in order to continiue"
	local inCycleMsg    = "Error: Control busy - Wait for any running script or program to finish"
	local notIdleMsg	= "Error: Not in Idle state - The machine state is not Idle as reported by machine control"
	mc.mcCntlSetLastError(finst, "TESTING machine state")
	if not isMachineEnabled(finst) then
		return false, notEnabledMsg
	end
	mc.mcCntlSetLastError(finst, "TESTING isMachineEnabled -> isMachineEnabled")
	if not isAllRefToHome(finst) then
		mc.mcCntlSetLastError(finst, "TESTING isMachineEnabled -> NOT isAllRefToHome")
		return false, notHomedMsg
	end
	mc.mcCntlSetLastError(finst, "TESTING isMachineEnabled -> isAllRefToHome")
	if not isMachineIdle(finst) then 
		return false, notIdleMsg
	end
	mc.mcCntlSetLastError(finst, "TESTING isMachineEnabled -> isIdle")
	if isInCycle(finst) then
		return false, inCycleMsg
	end
	mc.mcCntlSetLastError(finst, "TESTING isMachineEnabled -> Ready")
	return true, "Ready"
end

function isMachineEnabled(ftinst)
	local machEnbld = mc.mcSignalGetState (mc.mcSignalGetHandle (ftinst, mc.OSIG_MACHINE_ENABLED))
	if (machEnbld == 0) then
		mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled FALSE")
		return false
	elseif (machEnbld == 1) then
		mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled TRUE")
		return true
	else
		mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled error, FALSE")
		return false
	end

	--mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled")
	--local machEnbld = mc.mcSignalGetState (mc.mcSignalGetHandle (inst, mc.OSIG_MACHINE_ENABLED))
	--local reg  = mc.mcSignalGetHandle(ftinst, mc.OSIG_MACHINE_ENABLED)
	--mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled got handle")
	--MachineEnabled,rcsgs=mc.mcSignalGetState(reg)
	--if (rcsgs == MERROR_NOERROR) then
		--if machEnabled then
			--mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled Got Result: Enabled ")
			--return true
		--else
			--mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled Got Result: Not enabled ")
			--return false
		--end
		
	--elseif (rcsgs == MERROR_INVALID_ARG) then
		--mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled Got ERROR: MERROR_INVALID_ARG as '" .. MachineEnabled.."'")
	--else
		--mc.mcCntlSetLastError(ftinst, "TESTING isMachineEnabled Got ERROR: Out of API Spec as '" .. MachineEnabled.."'")
	--end

end

function isMachineIdle(ftinst)
    mc.mcCntlSetLastError(ftinst, "TESTING isMachineIdle")
	--local ftinst = mc.mcGetInstance()
    local mcState = mc.mcCntlGetState(ftinst)
    if mcState==mc.MC_STATE_IDLE then
		mc.mcCntlSetLastError(ftinst, "IDLE")
        return true
    else
		mc.mcCntlSetLastError(ftinst, "X360: Error - NOT IDLE")
        return false
    end
end

function isInCycle(ftinst)
	mc.mcCntlSetLastError(ftinst, "TESTING isInCycle")
	local result = mc.mcCntlIsInCycle(ftinst)
	local inCycle
	if (result == 1) then
		return true
	elseif (result == 0) then
		return false
	else -- ( == -1)
		mc.mcCntlSetLastError(ftinst, "Could not determine machine state")
		return true -- We must assume that we are in a cycle if we can not determine we are not...
	end
end

function isAllRefToHome(ftinst)
	mc.mcCntlSetLastError(ftinst, "TESTING begin isAllRefToHome")
	
	local result = false
	local msg = ""
	--local rc = 0
	local homed = false
	mc.mcCntlSetLastError(ftinst, "TESTING before isAllRefToHome")
	--result, rc = pcall(isHomed, ftinst) --This will run the function in the defined instance and set the value of rc and msg to whatever the function returns so you can make a decision
	result = isHomed(ftinst)
	mc.mcCntlSetLastError(ftinst, "TESTING after isAllRefToHome result: " ..result)
	-- OLD if (result == 1) then
		--if (rc == 1) then
			--return true
		--elseif (rc == 0) then
			--return false
		--elseif (rc == -1) then --API ERROR
			--return false
		--end
	--end
	
	if (result == 1) then
		return true
	elseif (result == 0) then
		return false
	elseif (result == -1) then --API ERROR
		return false
	end
	
end


function isHomed(ftinst) --Check if all enabled axis have been homed
	mc.mcCntlSetLastError(ftinst, "TESTING begin isHomed")
	--ftinst = ftinst or 0
	local controlinst = ftinst --mc.mcCntlGetInstanceHandle(ftinst, "function isHomed")
	local enabled = 0
	local rc = 0
	local homed = 0
	local result = -1

	for axisNumber = 1, 6 do
		mc.mcCntlSetLastError(ftinst, "TESTING isHomed testing axis: ".. axisNumber)
		enabled, rc = mc.mcAxisIsEnabled(controlinst,(axisNumber - 1))
		
		if (rc ~= 0) then -- Setup error
			break
		elseif (enabled == 1) then -- Axis enabled
			homed, rc = mc.mcAxisIsHomed(controlinst, (axisNumber - 1))
			if (rc ~= 0) then -- Setup error
				break
			elseif (homed == 0) then --Axis not homed
				break
			end
		end
	end
	mc.mcCntlSetLastError(ftinst, "TESTING begin isHomed verify result - rc="..rc.." result="..result.." homed="..homed)
	if (rc ~= 0) then -- Setup error
		result = -1
	elseif (homed == 0) then -- At least one axis is not homed
	    result = 0
	else
		result = 1;
	end
	mc.mcCntlSetLastError(ftinst, "TESTING isHomed returns result="..result)
	return result
end
---------------------
-- X360 CONTROLLER --
---------------------

function GetX360(xinput)
    local hreg = mc.mcRegGetHandle(inst, string.format("mcX360_LUA/%s", xinput))
    return mc.mcRegGetValueString(hreg)
end

---------------------------------
-- X360 BUTTON PRESS FUNCTIONS --
---------------------------------

function EnableMashine(finst,pressed)
    if pressed then
        mc.mcSignalSetState (mc.mcSignalGetHandle (finst, mc.ISIG_EMERGENCY), 0) --I dont want to clear E-Stop on BTN_B (E-Stop) UP so instead i use X360´s MashineEnable for it... (see E-Stop function)
        wx.wxMilliSleep(100) -- You cannot enable unless E-Stop is cleared so we must wait for mcSignalSetState to complete
        mc.mcCntlEnable(finst, true)
    end
    return
end
function CycleStart(finst,pressed)
    if pressed then
        mc.mcCntlCycleStart(finst)
    end
    return
end
function EStop(finst,pressed)
    if pressed then
        mc.mcSignalSetState (mc.mcSignalGetHandle (finst, mc.ISIG_EMERGENCY), 1)
    --else
        -- Uncomment this if you want E-Stop cleared on button release
        --mc.mcSignalSetState (mc.mcSignalGetHandle (finst, mc.ISIG_EMERGENCY), 0)
    end
    return
end

------------------------------
--X360 DPAD PRESS FUNCTIONS --
------------------------------
-------------------------------------------------
function SpindleSpeedInc(finst, pressed)
    if pressed then
        local val = scr.GetProperty('slideSRO', 'Value');
        val = tonumber(val) + 5;
        local maxval = scr.GetProperty('slideSRO', 'Max Value')
        if (tonumber(val) >= tonumber(maxval)) then
         val = maxval;
        end
        scr.SetProperty('slideSRO', 'Value', tostring(val));
    end
end
function SpindleSpeedDec(finst, pressed)
    if pressed then
        local val = scr.GetProperty('slideSRO', 'Value');
        val = tonumber(val) - 5;
        local minval = scr.GetProperty('slideSRO', 'Min Value')
        if (tonumber(val) <= tonumber(minval)) then
         val = minval;
        end
        scr.SetProperty('slideSRO', 'Value', tostring(val));
    end
end
--------------------------------------------------
function FeedRateInc(finst, pressed)
    if pressed then
        local val = scr.GetProperty('slideFRO', 'Value');
        val = tonumber(val) + 10;
        local maxval = scr.GetProperty('slideFRO', 'Max Value')
        if (tonumber(val) >= tonumber(maxval)) then
         val = maxval;
        end
        scr.SetProperty('slideFRO', 'Value', tostring(val));
    end
end
function FeedRateDec(finst, pressed)
    if pressed then
        local val = scr.GetProperty('slideFRO', 'Value');
        val = tonumber(val) - 10;
        local minval = scr.GetProperty('slideFRO', 'Min Value')
        if (tonumber(val) <= tonumber(minval)) then
         val = minval;
        end
        scr.SetProperty('slideFRO', 'Value', tostring(val));
    end
end



----------------------------------------------------------
-- X360 DPAD PRESS WITH "Btn_Y" or "Btn_START" MODIFIER --
----------------------------------------------------------
function Probe(finst, pressed, axis, direction)
	if pressed and isProcessingDPadButtonPressed then
		mc.mcCntlSetLastError(finst, "X360: Debug - Already Probing...") 
		return
	elseif pressed and not isProcessingDPadButtonPressed then
		isProcessingDPadButtonPressed = true
		doProbe(finst, axis, direction)
	else
		return
	end
	
	
	isProcessingDPadButtonPressed = false
end
function doProbe(finst, axis, direction)
	
	mc.mcCntlSetLastError(finst, "X360: Debug - Enter probe function") 
	-- Variables used for probing
	ProbeMove = "G31" -- Use "G31.1" - "G31.3" to select other probes
	ProbeMoveDistance = 10 -- the distance the tool travels in the probe move
	
	
	
	--Check if machine isenabled and idle
	local ready = false
	local msg
	ready, msg = isMachineReady(finst)
	--handle machine not ready when commanded
	if not ready then
		mc.mcCntlSetLastError(finst, "X360: Machine not ready - "..msg..". \nisMachineReady() returns"..ready)
		return 
	end
	mc.mcCntlSetLastError(finst, "X360: Debug - Machine Ready")
		
	--verify settings valid
	
	--Begin Probing
    if ready then
		local dir = ""
		if direction == "-" then
			dir = "-"
		end
         
		ProbeString = "G91 " .. ProbeMove .." "..axis.. dir.. ProbeMoveDistance
		if delayProbingUntilButtonPress then
			local remainingTime = 10
			--wx.Messagebox("Press 'A' to Probe or 'B' to Cancel on your X360 no action within 10 seconds will autoabort\nPress OK will start Countdown" )
			mc.mcCntlSetLastError(finst, "Press 'A' to Probe or 'B' to Cancel on your X360 no action within 10 seconds will autoabort")
			while true do
				if (GetX360("Btn_B") == "1") then
					mc.mcCntlSetLastError(finst, "X360: Probing aborted")
					return 
				end
				if (GetX360("Btn_A") == "1") then
					mc.mcCntlSetLastError(finst, "X360: Probing "..axis.." axis in "..direction.." direction\n using string: \n" .. ProbeString)
					break
				end
				if remainingTime <= 0 then
					mc.mcCntlSetLastError(finst, "X360: Probing timed out - Probing canceled")
					return
				end
				
				local GCode = ""
				GCode = GCode ..string.format("G4 p500")
				mc.mcCntlGcodeExecuteWait (finst, GCode) --wait 0.5 second
				
				-- ' 000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
				-- ' What is set that sticks when a probe move fails (Second call to gcode execute("G31") fails)
				-- ' How can I update in mid scrit to notify the user?
				
				remainingTime = remainingTime - 0.5
				--mc.mcCntlSetLastError(finst, string.format("Press 'A' to Probe or 'B' to Cancel on your X360 no action within %.1f seconds will autoabort", remainingTime))
			end
		end
			mc.mcCntlSetLastError(finst, "Executing GCode: "..ProbeString)
		local rcgc = mc.mcCntlGcodeExecuteWait(finst, ProbeString)
		if rcgc == MERROR_NOERROR then
			mc.mcCntlSetLastError(finst, "Executing GCode Done! ToDo - Code to set selected WCS, done for now :)")
		elseif rcgc == MERROR_INVALID_INSTANCE then
			mc.mcCntlSetLastError(finst, "Executing GCode MERROR_INVALID_INSTANCE: ---DEBUG:GCodeExec result: "..rcgc)
		elseif rcgc == MERROR_NOT_NOW then
			mc.mcCntlSetLastError(finst, "Executing GCode MERROR_NOT_NOW: ---DEBUG:GCodeExec result: "..rcgc)
		elseif rcgc == MERROR_NOT_COMPILED then
			mc.mcCntlSetLastError(finst, "Executing GCode MERROR_NOT_COMPILED: ---DEBUG:GCodeExec result: "..rcgc)
		end

		--mc.mcCntlSetLastError(finst, "Executing GCode Done! ToDo - Code to set selected WCS, done for now :) ---DEBUG:GCodeExec result: "..rcgc)
    end
	
	isProcessingDPadButtonPressed = false
    return
end

--------------------------------------------
-- GENERAL BUTTON AND THUMBSTICK HANDLING --
--------------------------------------------

function HandleButtonPress(finst,btn,btnNo,func,msg,strParam1,strParam2)
    if btn == "1" and isPressed_BTN[btnNo] == false then
        func(finst,true,strParam1,strParam2)
        mc.mcCntlSetLastError(finst, msg) 
        isPressed_BTN[btnNo] = true
    elseif btn == "0" and isPressed_BTN[btnNo] == true then
        func(finst,false,strParam1,strParam2)
        isPressed_BTN[btnNo] = false
    end
    return
end

function HandleThumbInput(finst,thax,DZ, MaxJS) -- ThumbstickAxis[],DeadZone,MaxJogSpeed
	
    local th = GetX360(thax[1])
    local axis = thax[4]
    if (tonumber(th) >= (DZ*-1)) and (tonumber(th) <= DZ) and thax[2]==0 then
        if mc.mcJogGetVelocity(finst, axis) ~= 0 then
            mc.mcJogSetRate(finst, axis, 0)
            mc.mcJogVelocityStop(finst, axis);
        end
        thax[2] = 1
    else
        if tonumber(th) > tonumber(DZ) then
            mc.mcJogSetRate(finst, axis, ((((tonumber(th)-DZ)*(tonumber(th)-DZ))/((100-DZ)*(100-DZ)))*MaxJS))
            mc.mcJogVelocityStart(finst, axis, mc.MC_JOG_POS)
            thax[2] = 0
        elseif tonumber(th) < (tonumber(DZ)*-1) then
            mc.mcJogSetRate(finst, axis, ((((tonumber(th)+DZ)*(tonumber(th)+DZ))/((100-DZ)*(100-DZ)))*MaxJS))
            mc.mcJogVelocityStart(finst, axis, mc.MC_JOG_NEG)
            thax[2] = 0
        end
    end
    return
end



--------------------------
-- START X360 MONITORING --
---------------------------
if true then
X360_Timer:Start(50)
-- MAIN LOOP --
X360_Panel:Connect(wx.wxEVT_TIMER,   function (event)
-- HANDLE E-STOP ON BTN_B
    HandleButtonPress(inst,GetX360("Btn_B"),2,EStop,"X360_E-STOP")
-- SECURITY MEASURE -- Require Both triggerbuttons to be pressed...
    local deadMansGrip = (GetX360("Btn_RS") == "1" ) and (GetX360("Btn_LS") == "1" )
-- ENABLE CONTROLLER (Only X and A are configured)
    if deadMansGrip and not isSecured then
        isSecured = true
        X360TS_Timer:Start(100)
        X360BTN_Timer:Start(100)
        X360DPAD_Timer:Start(100)
        mc.mcCntlSetLastError(inst, "Secure grip! - X360 Controller enabled") 
    elseif not deadMansGrip and isSecured then -- If dead mans grip is lost then stop the machine
        isSecured = false
        X360TS_Timer:Stop()
        X360BTN_Timer:Stop()
        X360DPAD_Timer:Stop()
        for i=4, 1, -1
        do
            local axis = thumbSticks[i][4]
            --if mc.mcJogGetVelocity(inst, axis) ~= 0 then
            --mc.mcJogSetRate(inst, axis, 0)
            mc.mcJogVelocityStop(inst, axis);
            --end
            thumbSticks[i][3] = true
        end
        mc.mcCntlSetLastError(inst, "Secure grip was lost - X360 Controller disabled") 
    end
end)


-- THUMBSTICKS LOOP --
X360TS_Panel:Connect(wx.wxEVT_TIMER,   function (event)
    local BackDownMode = (GetX360("Btn_BACK") == "1" ) --Button Back is in DOWN so we alter the speed of the JOG move
-- HANDLE THUMBSTICKS JOG 
	local JS = 0
	if BackDownMode then
		JS = MaxSlowJogSpeed
	else    
		JS = MaxJogSpeed
	end
    HandleThumbInput(inst, thumbSticks[1], DeadZone, JS)
    HandleThumbInput(inst, thumbSticks[2], DeadZone, JS)
    HandleThumbInput(inst, thumbSticks[3], DeadZone, JS)
    HandleThumbInput(inst, thumbSticks[4], DeadZone, JS)
end)

-- BUTTONS LOOP --
X360BTN_Panel:Connect(wx.wxEVT_TIMER,   function (event)
-- MASHINE ENABLE ON BTN_X
    HandleButtonPress(inst,GetX360("Btn_X"),3,EnableMashine,"X360_Enabled_Machine")
-- CYCLE START ON BTN_A
    HandleButtonPress(inst,GetX360("Btn_A"),1,CycleStart,"X360_CycleStart")
-- SlowJog ON BTN_BACK
	--Moved To Thumbsticks Loop HandleButtonPress
end)

-- DPAD LOOP --
X360DPAD_Panel:Connect(wx.wxEVT_TIMER,   function (event)
    local StartDownMode = false
    local YDownMode = false
    local Both = false
    local None = false

    local StartDown = (GetX360("Btn_START") == "1" ) 
    local YDown = (GetX360("Btn_Y") == "1" ) 

-- Set Operation Mode using Modifiers
-- ** only one of Both, None,YdownMode and StartDownMode can be true at any one time

-- Set modifier variables
	if YDown and StartDown and not Both then --Set Both mode 
        Both = true
        None = false
        YDownMode = false
        StartDownMode = false
    elseif not YDown and not StartDown and not None then -- Set None mode
        None = true
        Both = false
        YDownMode = false
        StartDownMode = false
    elseif YDown and not YDownMode then -- Set Y only mode -->YDownMode	
        YDownMode = true
        None = false
        Both = false
        StartDownMode = false
    elseif StartDown and not StartDownMode then -- Set Start only mode --> StartDown
        StartDownMode = true
        FeedAndSpeedMode = false
        None = false
        Both = false
    end
	
--Handle modifier affected buttons
    if None then
    -- SpindleSpeed +
        HandleButtonPress(inst,GetX360("DPad_UP"),4,SpindleSpeedInc,"X360_SpindleSpeed +")
    -- SpindleSpeed -
        HandleButtonPress(inst,GetX360("DPad_DOWN"),5,SpindleSpeedDec,"X360_SpindleSpeed -")
    -- FeedRate +
        HandleButtonPress(inst,GetX360("DPad_RIGHT"),7,FeedRateInc,"X360_FeedRate +")
    -- FeedRate -
        HandleButtonPress(inst,GetX360("DPad_LEFT"),6,FeedRateDec,"X360_FeedRate -")
    elseif StartDownMode then
    -- PROBE DIRECTION Z+
        HandleButtonPress(inst,GetX360("DPad_UP"),4,Probe,"X360_Probe Z+","Z","+")
    -- PROBE DIRECTION Z-
        HandleButtonPress(inst,GetX360("DPad_DOWN"),5,Probe,"X360_Probe Z-","Z","-")
    -- PROBE DIRECTION A+
        HandleButtonPress(inst,GetX360("DPad_RIGHT"),7,Probe,"X360_Probe A+","A","+")
    -- PROBE DIRECTION A-
        HandleButtonPress(inst,GetX360("DPad_LEFT"),6,Probe,"X360_Probe A-","A","-")
    elseif YDownMode then
    -- PROBE DIRECTION Y+
        HandleButtonPress(inst,GetX360("DPad_UP"),4,Probe,"X360_Probe Y+","Y","+")
    -- PROBE DIRECTION Y-
        HandleButtonPress(inst,GetX360("DPad_DOWN"),5,Probe,"X360_Probe Y-","Y","-")
    -- PROBE DIRECTION X+
        HandleButtonPress(inst,GetX360("DPad_RIGHT"),7,Probe,"X360_Probe X+","X","+")
    -- PROBE DIRECTION X-
        HandleButtonPress(inst,GetX360("DPad_LEFT"),6,Probe,"X360_Probe X-","X","-")
    end
end)







end