# import linuxcnc
# import hal

# def tool_remap(self, **words):
    # command = linuxcnc.command()
    # stat = linuxcnc.stat()
    # stat.poll()

    # tool_number = int(words["T"])
    
    # if tool_number == 1:
        # z_position = 0  # Set Z1 position
    # elif tool_number == 2:
        # z_position = -50  # Set Z2 position (adjust this)
    # else:
        # return
    
    # # Move the Z-axis based on the selected tool
    # command.mode(linuxcnc.MODE_MDI)
    # command.mdi(f"G53 G0 Z{z_position}")  # Move Z to desired position
    # command.wait_complete()

# import linuxcnc
# import hal

# def remap_tool(self, **words):
    # tool_number = int(words['T'])

    # if tool_number == 1:
        # print("Switching to Z1 (Tool 1)")
        # hal.set_p("motion.analog-out-03", 0)
    # elif tool_number == 2:
        # print("Switching to Z2 (Tool 2)")
        # hal.set_p("motion.analog-out-03", 1)
    # else:
        # print("Unknown tool: ", tool_number)

    # return INTERP_OK


import linuxcnc

# Define the remap function
def remap_toolchange(self, *args, **kwargs):
    tool_number = self.params["tool_number"]
    
    if tool_number == 1:
        # Activate Z1
        self.execute("M428")  # Call M428 to switch to Z1
    elif tool_number == 2:
        # Activate Z2
        self.execute("M429")  # Call M429 to switch to Z2
    else:
        raise ValueError(f"Unsupported tool number: {tool_number}")

    # Call the original M6 command
    self.execute("M6")

# Register the remap function
linuxcnc.remap_command("M6", remap_toolchange)