--XBox 360 Module
--Using plugin for LUA by Dazthegas (Mach support forums)
--Created by: Tom Lamontagne, Element Machine Tools
--Created date: 02-05-2017

xb = {}

--Set vars
if (inst == nil) then
	inst = mc.mcGetInstance()
end

xb.MaxJog = 50 --% of max machine velocity
xb.JogType = -1 -- 1 for continuous 0 for incremental -1 to disable

function xb.SetJogType(t)
	xb.JogType = t
	mc.mcCntlSetLastError(inst, "Settting jog type: " .. tostring(t))
end

function xb.GetJogType()
	local t = xb.JogType
	mc.mcCntlSetLastError(inst, "Returned jog type: " .. tostring(t))
	return t
end

function xb.SetMaxJog(r)
	xb.MaxJog = r
	mc.mcCntlSetLastError(inst, "Settting max jog: " .. tostring(r))
end

function xb.GetMaxJog()
	local r = xb.MaxJog
	mc.mcCntlSetLastError(inst, "Returned max jog: " .. tostring(r))
	return r
end

function xb.GetRegister(regname, num)
	local rc = mc.MERROR_NOERROR
	local val = nil
	local hreg, rc = mc.mcRegGetHandle(inst, string.format("mcX360_LUA/%s", regname))
	if (rc == mc.MERROR_NOERROR) then
		val = mc.mcRegGetValueString(hreg)
		if (num == 1) then
			val = tonumber(val)
		end
	end
	return val, rc
end

function xb.SetRegister(regname, val)
	local hreg = mc.mcRegGetHandle(inst, string.format("mcX360_LUA/%s", regname))
	mc.mcRegSetValueString(hreg, tostring(val))
end

function xb.SetLastReg(key, val)
	xb.RegTable_Last[key] = val
	--mc.mcCntlSetLastError(inst, "Set last " .. tostring(key) .. " to " .. tostring(val))
end

function xb.GetLastReg(key)
	local val = xb.RegTable_Last[key]
	--mc.mcCntlSetLastError(inst, "Last " .. tostring(key) .. " is " .. tostring(val))
	return val
end

function xb.Jog(axis, dir, state)
	if (dir == mc.MC_JOG_STOP) then
		if (xb.GetJogType() ~= 0) then
			mc.mcJogVelocityStop(inst, axis)
		end
	else
		if (xb.GetJogType() == 0) and (math.abs(state) > 75) then
			local inc = mc.mcJogGetInc(inst, axis)
			inc = inc * dir
			mc.mcJogIncStart(inst, axis, inc)
		elseif (xb.GetJogType() == 1) then
			--mc.mcCntlSetLastError(inst, "Jogging continuous")
			mc.mcJogVelocityStart(inst, axis, dir)
		end
	end
end

function xb.SetJogRate(axis, p)
	--Set jog rate
	mc.mcJogSetRate(inst, axis, p)
	--mc.mcCntlSetLastError(inst, "Jog rate set to " .. tostring(p))
end

xb.RegTable = {
	["Btn_A"] = function(state)
		if (state == 1) then
			mc.mcCntlEnable(inst, true)
			--mc.mcCntlSetLastError(inst, "A button pressed")
		else
			--mc.mcCntlSetLastError(inst, "A button released")
		end
	end,
	["Btn_B"] = function(state)
		if (state == 1) then
			mc.mcCntlEStop(inst)
			--mc.mcCntlSetLastError(inst, "B button pressed")
		else
			--mc.mcCntlSetLastError(inst, "B button released")
		end
	end,
	["Btn_X"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "X button pressed")
		else
			mc.mcCntlSetLastError(inst, "X button released")
		end
	end,
	["Btn_Y"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "Y button pressed")
		else
			mc.mcCntlSetLastError(inst, "Y button released")
		end
	end,
	["Btn_BACK"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "Back button pressed")
		else
			mc.mcCntlSetLastError(inst, "Back button released")
		end
	end,
	["Btn_START"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "Start button pressed")
		else
			mc.mcCntlSetLastError(inst, "Start button released")
		end
	end,
	["Btn_LS"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "Continuous Jog Enabled")
			xb.SetJogType(1)
		else
			mc.mcCntlSetLastError(inst, "Continous Jog Disabled")
			xb.SetJogType(-1)
		end
	end,
	["Btn_RS"] = function(state)
		local leftState = xb.GetRegister("Btn_LS", 1)
		if (leftState == 0) then
			if (state == 1) then
				mc.mcCntlSetLastError(inst, "Incremental Jog Enabled")
				xb.SetJogType(0)
			else
				mc.mcCntlSetLastError(inst, "Incremental Jog Disabled")
				xb.SetJogType(-1)
			end
		end
	end,
	["LTR_Val"] = function(state)
		mc.mcCntlSetLastError(inst, "Left trigger val: " .. tostring(state))
	end,
	["RTR_Val"] = function(state)
		mc.mcCntlSetLastError(inst, "Right trigger val: " .. tostring(state))
	end,
	["LTH_Y_Val"] = function(state)
		local maxjog = xb.GetMaxJog()
		local cmd = ((math.abs(state) - 20)/(99-20)) * maxjog
		local jograte = (cmd * cmd) / maxjog
		--mc.mcCntlSetLastError(inst, "State = " .. tostring(state) .. "\nMax jog = " .. tostring(maxjog) .. "\nCmd = " .. tostring(cmd) .. "\nJog rate = " .. tostring(jograte))
		xb.SetJogRate(mc.Y_AXIS, jograte)
		if (state < -20) then
			xb.Jog(mc.Y_AXIS, mc.MC_JOG_NEG, state)
		elseif (state > 20) then
			xb.Jog(mc.Y_AXIS, mc.MC_JOG_POS, state)
		else
			xb.Jog(mc.Y_AXIS, mc.MC_JOG_STOP, state)
		end
		--mc.mcCntlSetLastError(inst, "Left stick Y val: " .. tostring(state))
	end,
	["LTH_X_Val"] = function(state)
		local maxjog = xb.GetMaxJog()
		local cmd = ((math.abs(state) - 20)/(99-20)) * maxjog
		local jograte = (cmd * cmd) / maxjog
		--mc.mcCntlSetLastError(inst, "State = " .. tostring(state) .. "\nMax jog = " .. tostring(maxjog) .. "\nCmd = " .. tostring(cmd) .. "\nJog rate = " .. tostring(jograte))
		xb.SetJogRate(mc.X_AXIS, jograte)
		if (state < -20) then
			xb.Jog(mc.X_AXIS, mc.MC_JOG_NEG, state)
		elseif (state > 20) then
			xb.Jog(mc.X_AXIS, mc.MC_JOG_POS, state)
		else
			xb.Jog(mc.X_AXIS, mc.MC_JOG_STOP, state)
		end
		--mc.mcCntlSetLastError(inst, "Left stick X val: " .. tostring(state))
	end,
	["RTH_Y_Val"] = function(state)
		local maxjog = xb.GetMaxJog()
		local cmd = ((math.abs(state) - 20)/(99-20)) * maxjog
		local jograte = (cmd * cmd) / maxjog
		--mc.mcCntlSetLastError(inst, "State = " .. tostring(state) .. "\nMax jog = " .. tostring(maxjog) .. "\nCmd = " .. tostring(cmd) .. "\nJog rate = " .. tostring(jograte))
		xb.SetJogRate(mc.Z_AXIS, jograte)
		if (state < -20) then
			xb.Jog(mc.Z_AXIS, mc.MC_JOG_NEG, state)
		elseif (state > 20) then
			xb.Jog(mc.Z_AXIS, mc.MC_JOG_POS, state)
		else
			xb.Jog(mc.Z_AXIS, mc.MC_JOG_STOP, state)
		end
		--mc.mcCntlSetLastError(inst, "Right stick Y val: " .. tostring(state))
	end,
	["RTH_X_Val"] = function(state)
		--mc.mcCntlSetLastError(inst, "Right stick X val: " .. tostring(state))
	end,
	["Btn_LTH"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "Left stick button pressed")
		else
			mc.mcCntlSetLastError(inst, "Left stick button released")
		end
	end,
	["Btn_RTH"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "Right stick button pressed")
		else
			mc.mcCntlSetLastError(inst, "Right stick button released")
		end
	end,
	["DPad_UP"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "DPad up button pressed")
		else
			mc.mcCntlSetLastError(inst, "DPad up button released")
		end
	end,
	["DPad_DOWN"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "DPad down button pressed")
		else
			mc.mcCntlSetLastError(inst, "DPad down button released")
		end
	end,
	["DPad_LEFT"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "DPad left button pressed")
		else
			mc.mcCntlSetLastError(inst, "DPad left button released")
		end
	end,
	["DPad_RIGHT"] = function(state)
		if (state == 1) then
			mc.mcCntlSetLastError(inst, "DPad right button pressed")
		else
			mc.mcCntlSetLastError(inst, "DPad right button released")
		end
	end
}

xb.RegTable_Last = {
	["Btn_A"] = 0,
	["Btn_B"] = 0,
	["Btn_X"] = 0,
	["Btn_Y"] = 0,
	["Btn_BACK"] = 0,
	["Btn_START"] = 0,
	["Btn_LS"] = 0,
	["Btn_RS"] = 0,
	["LTR_Val"] = 0,
	["RTR_Val"] = 0,
	["LTH_Y_Val"] = 0,
	["LTH_X_Val"] = 0,
	["RTH_Y_Val"] = 0,
	["RTH_X_Val"] = 0,
	["Btn_LTH"] = 0,
	["Btn_RTH"] = 0,
	["DPad_UP"] = 0,
	["DPad_DOWN"] = 0,
	["DPad_LEFT"] = 0,
	["DPad_RIGHT"] = 0
}

function xb.PLC()
	for k,v in pairs(xb.RegTable) do
		local state = xb.GetRegister(k, 1)
		local lastState = xb.GetLastReg(k)
		if (state ~= lastState) then
			xb.RegTable[k](state)
			xb.SetLastReg(k, state)
		end
	end
end

return xb