
local function intFromPort(gate, port, len)
	local v = 0
	for i = 1, len do
		v = v + Gate.getportstate(gate, port+i-1)*math.pow(2, i-1)
	end
	return v
end

local function rotateVector(vec, rot)
	if     rot==0 then return { vec[1],  vec[2], vec[3]}
	elseif rot==1 then return { vec[2], -vec[1], vec[3]}
	elseif rot==2 then return {-vec[1], -vec[2], vec[3]}
	elseif rot==3 then return {-vec[2],  vec[1], vec[3]}
	else error("invalid rot "..rot) end
end

return function(gate)
	if not gate.waiting then
		local action = ""
		if Gate.getportstate(gate, 7)~=0 then -- remove brick
			action = action.."R\t"
		end
		if Gate.getportstate(gate, 8)~=0 then -- plant brick
			local color = intFromPort(gate, 1, 6)
			local colorfx = 0
			local shapefx = 0
			local ray = 1
			local col = 1
			local ren = 1
			action = action.."P "..color.." "..colorfx.." "..shapefx.." "..ray.." "..col.." "..ren.."\t"
		end
		if Gate.getportstate(gate, 22)~=0 then -- detect brick
			action = action.."D\t"
		end
		
		local movePos = {0, 0, 0}
		if Gate.getportstate(gate,  9)~=0 then movePos[3] = movePos[3]-0.2 end -- down
		if Gate.getportstate(gate, 10)~=0 then movePos[3] = movePos[3]+0.2 end -- up
		if Gate.getportstate(gate, 11)~=0 then movePos[1] = movePos[1]+0.5 end -- right
		if Gate.getportstate(gate, 12)~=0 then movePos[1] = movePos[1]-0.5 end -- left
		if Gate.getportstate(gate, 13)~=0 then movePos[2] = movePos[2]-0.5 end -- back
		if Gate.getportstate(gate, 14)~=0 then movePos[2] = movePos[2]+0.5 end -- forward
		local moveRotation = 0
		--if Gate.getportstate(gate, 13)~=0 then moveRotation = moveRotation+1 end -- right
		--if Gate.getportstate(gate, 14)~=0 then moveRotation = moveRotation-1 end -- left
		gate.robotdir = (gate.robotdir + moveRotation)%4
		
		if movePos[1]~=0 or movePos[2]~=0 or movePos[3]~=0 or moveRotation~=0 then
			movePos = rotateVector(movePos, gate.robotdir)
			gate.robotpos = { gate.robotpos[1]+movePos[1], gate.robotpos[2]+movePos[2], gate.robotpos[3]+movePos[3] }
			action = action.."M "..gate.robotpos[1].." "..gate.robotpos[2].." "..gate.robotpos[3].." "..gate.robotdir.."\t"
		end
		
		if action~="" then
			Gate.cb(gate, action:sub(1, #action-1))
			gate.waiting = true
		end
	end
end
