#! /usr/bin/python2
import sys, os
import gettext
import time
BASE = os.path.abspath(os.path.join(os.path.dirname(sys.argv[0]), ".."))
gettext.install("linuxcnc", localedir=os.path.join(BASE, "share", "locale"), unicode=True)

import linuxcnc, hal

_after = None
def hal_in_background():
    global _after
    _after = None
    if not h.change:
        app.tk.call("set", "::tkPriv(button)", -1)
        return

    if (h.change_button) == 1 and ngc_running == 0:
        change_complete()
        app.update()
        app.tk.call("set", "::tkPriv(button)", -1)
        stop_polling_hal_in_background()
        return

    _after = app.after(100, hal_in_background)

def poll_hal_in_background():
    global _after
    _after = app.after(100, hal_in_background)

def stop_polling_hal_in_background():
    global _after
    if _after: app.after_cancel(_after)
    _after = None

def do_change(n):
    if n:
        message = _("Insert tool %d and click continue when ready") % n
    else:
        message = _("Remove the tool and click continue when ready")
    app.wm_withdraw()
    app.update()
    poll_hal_in_background()
    try:
        r = app.tk.call("nf_dialog", ".tool_change",
            _("Tool change"), message, "info", 0, _("Continue"))
    finally:
        stop_polling_hal_in_background()
    if isinstance(r, str): r = int(r)
    if r == 0:
        change_complete()
    app.update()

def do_remove(n):
    if n:
        message = _("Insert tool %d and click continue when ready") % n
    else:
        message = _("Remove the tool and click continue when ready")
    app.wm_withdraw()
    app.update()
    poll_hal_in_background()
    try:
        r = app.tk.call("nf_dialog", ".tool_change",
            _("Tool change"), message, "info", 0, _("Continue"))
    finally:
        stop_polling_hal_in_background()
    if isinstance(r, str): r = int(r)
    if r == 0:
        tool_removed = 1
    app.update()    

def ngc_routine():
    
    #If tool should be returned. Write return, level and pocket coordinates to HAL Else write failsafe numbers.
    if current_tool_return == 1: 
        h.current_tool_return = True
        h.current_tool_maglevel = current_tool_magazine_level
        h.current_pocket_x = current_pocket_x
        h.current_pocket_y = current_pocket_y
        h.current_pocket_z = current_pocket_z
    else:
        h.current_tool_return = False
        h.current_tool_maglevel = -1
        h.current_pocket_x = -1
        h.current_pocket_y = -1
        h.current_pocket_z = 1          

    #If new tool should be collected. Write collect, level and pocket coordinates to HAL Else write failsafe numbers.
    if new_tool_collect == 1: 
        h.new_tool_collect = True
        h.new_tool_maglevel = new_tool_magazine_level
        h.new_pocket_x = new_pocket_x
        h.new_pocket_y = new_pocket_y
        h.new_pocket_z = new_pocket_z
    else:
        h.new_tool_collect = False
        h.new_tool_maglevel = -1
        h.new_pocket_x = -1
        h.new_pocket_y = -1
        h.new_pocket_z = 1          

    # Call the NGC routine
    linuxcnc.command("toolchange_tester.ngc")

    try:
        while 1:
            if current_tool_return == 1 and h.tool_returned == 1 and new_tool_collect == 1 and h.tool_installed == 1:
                break
            elif current_tool_return == 1 and h.tool_returned == 1 and new_tool_collect == 0 and h.tool_installed == 0:
                break
            elif current_tool_return == 0 and h.tool_returned == 0 and new_tool_collect == 1 and h.tool_installed == 1:
                break
            time.sleep(0.1)
    except KeyboardInterrupt:
        pass    

def change_complete():
    #Write new tool to var file for storage
    with open('/home/cnc/linuxcnc/configs/FagganCNC/currenttool.var', 'w') as currenttool:
        currenttool.write("currenttool: "+str(h.number))

    #Send change complete to controller to resume program
    h.changed = True    

def setup():
    #Lookup what the current tool is
    with open('/home/cnc/linuxcnc/configs/FagganCNC/currenttool.var', 'r') as currenttool:
        for line in currenttool:
            if line.startswith('currenttool: '):
                current_line_return = str(line.split('currenttool: ')[1].rstrip(''))
                current_tool = int(current_line_return.rstrip('')[0])
                break

    #Look for current tools pocket
    with open('/home/cnc/linuxcnc/configs/FagganCNC/tool.tbl', 'r') as tooltable:
        for line in tooltable:
            if line.startswith('T'+str(current_tool)):
                current_line_return = str(line.split('P')[1].rstrip(''))
                current_pocket = int(current_line_return.rsplit(' ')[0])
                break


    #Check where current tool is getting stored and where new tool is located in regards to manual vs automatic tool change
    if current_pocket in range(1,45) and h.pocket in range(1,45):
        current_tool_return = 1
        new_tool_collect = 1
        ngc_running = 0

    elif current_pocket in range(1,45) and h.pocket not in range(1,45):
        current_tool_return = 1
        new_tool_collect = 0
        ngc_running = 0


    elif current_pocket not in range(1,45) and h.pocket in range(1,45):
        current_tool_return = 0
        new_tool_collect = 1
        ngc_running = 0

    else:
        current_tool_return = 0
        new_tool_collect = 0
        ngc_running = 0

    #If current tool is stored in magazine, look for XYZ Coordinates for the pocket and magazine level
    if current_tool_return == 1:
        #Look for current tool pocket x coordinate
        with open('/home/cnc/linuxcnc/configs/FagganCNC/toolchanger.var', 'r') as toolchanger:
            for line in toolchanger:
                if line.startswith('P'+str(current_pocket)):
                    current_line_return = str(line.split('X')[1].rstrip(''))
                    current_pocket_x = int(current_line_return.rsplit(' ')[0])
                    break

        #Look for current tool pocket y coordinate
        with open('/home/cnc/linuxcnc/configs/FagganCNC/toolchanger.var', 'r') as toolchanger:
            for line in toolchanger:
                if line.startswith('P'+str(current_pocket)):
                    current_line_return = str(line.split('Y')[1].rstrip(''))
                    current_pocket_y = int(current_line_return.rsplit(' ')[0])
                    break

        #Look for current tools pocket z coordinate
        with open('/home/cnc/linuxcnc/configs/FagganCNC/toolchanger.var', 'r') as toolchanger:
            for line in toolchanger:
                if line.startswith('P'+str(current_pocket)):
                    current_line_return = str(line.split('Z')[1].rstrip(''))
                    current_pocket_z = int(current_line_return.rsplit(' ')[0])
                    break
        
        #Check what magazine level current tool goes in
        if current_pocket in range(1,31):
            current_tool_magazine_level = 0
        else:    
            current_tool_magazine_level = 1
      
            
    #If new tool is located in magazine, look for XYZ coordinates for the pocket and magazine level
    if new_tool_collect == 1:
        #Look for new tool pocket x coordinate
        with open('/home/cnc/linuxcnc/configs/FagganCNC/toolchanger.var', 'r') as toolchanger:
            for line in toolchanger:
                if line.startswith('P'+str(h.pocket)):
                    new_line_return = str(line.split('X')[1].rstrip(''))
                    new_pocket_x = int(new_line_return.rsplit(' ')[0])
                    break

        #Look for new tool pocket y coordinate
        with open('/home/cnc/linuxcnc/configs/FagganCNC/toolchanger.var', 'r') as toolchanger:
            for line in toolchanger:
                if line.startswith('P'+str(h.pocket)):
                    new_line_return = str(line.split('Y')[1].rstrip(''))
                    new_pocket_y = int(new_line_return.rsplit(' ')[0])
                    break

        #Look for new tools pocket z coordinate
        with open('/home/cnc/linuxcnc/configs/FagganCNC/toolchanger.var', 'r') as toolchanger:
            for line in toolchanger:
                if line.startswith('P'+str(h.pocket)):
                    new_line_return = str(line.split('Z')[1].rstrip(''))
                    new_pocket_z = int(new_line_return.rsplit(' ')[0])
                    break

        #Check what magazine level new tool is in
        if h.pocket in range(1,31):
            new_tool_magazine_level = 0
        else:    
            new_tool_magazine_level = 1  

def fullchange():
    #Current tool belongs in magazine and new tool is collected from magazine
        ngc_running = 1
        ngc_routine()

        if h.tool_returned == 1 and h.tool_installed == 1:
            ngc_running = 0
            stop_polling_hal_in_background()
            change_complete()

def autoreturn():
    #Current tool belongs in magazine but new tool needs manual loading
    #store current tool in magazine
    ngc_running = 1
    ngc_routine()

    #Promt user to manually load tool    
    if h.tool_returned == 1:
        ngc_running = 0
        do_change(h.number)


def autoload():
    #Current tool does not belong in magazine but new tool is stored in magazine

        #Promt user to manually unload tool
        tool_removed = 0        
        do_remove()

        #Collect new tool from magazine pocket.
        if tool_removed == 1:
            ngc_running = 1
            ngc_routine()
            if h.tool_installed == 1:
                ngc_running = 0
                stop_polling_hal_in_background()
                change_complete()

h = hal.component("hal_manualtoolchange")
h.newpin("number", hal.HAL_S32, hal.HAL_IN)
h.newpin("change", hal.HAL_BIT, hal.HAL_IN)
h.newpin("change_button", hal.HAL_BIT, hal.HAL_IN)
h.newpin("changed", hal.HAL_BIT, hal.HAL_OUT)
h.newpin("pocket", hal.HAL_S32, hal.HAL_IN)
h.newpin("tool_returned", hal.HAL_BIT, hal.HAL_IN)
h.newpin("tool_installed", hal.HAL_BIT, hal.HAL_IN)
h.newpin("current_tool_return", hal.HAL_BIT, hal.HAL_OUT)
h.newpin("new_tool_collect", hal.HAL_BIT, hal.HAL_OUT)
h.newpin("current_pocket_x", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("current_pocket_y", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("current_pocket_z", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("current_tool_maglevel", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("new_pocket_x", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("new_pocket_y", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("new_pocket_z", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("new_tool_maglevel", hal.HAL_FLOAT, hal.HAL_OUT)
h.ready()

import Tkinter, nf, rs274.options

app = Tkinter.Tk(className="AxisToolChanger")
app.wm_geometry("-60-60")
app.wm_title(_("AXIS Manual Toolchanger"))
rs274.options.install(app)
nf.start(app); nf.makecommand(app, "_", _)
app.wm_protocol("WM_DELETE_WINDOW", app.wm_withdraw)
lab = Tkinter.Message(app, aspect=500, text = _("\
This window is part of the AXIS manual toolchanger.  It is safe to close \
or iconify this window, or it will close automatically after a few seconds."))
lab.pack()

def withdraw():
    app.wm_withdraw()
    app.bind("<Expose>", lambda event: app.wm_withdraw())

app.after(10 * 1000, withdraw)

#Loop looking for tool change signal from controller. 
try:
    while 1:
        change = h.change
        if change and not h.changed:
            setup()

            if current_tool_return == 1 and new_tool_collect == 1:
                fullchange()

            elif current_tool_return == 1 and new_tool_collect == 0:
                autoreturn()
                
            elif current_tool_return == 0 and new_tool_collect == 1:
                autoload()

            elif current_tool_return == 0 and new_tool_collect == 0:
                ngc_running = 0
                do_change(h.number)

        elif not change:
            h.changed = False
        app.after(100)
        app.update()
except KeyboardInterrupt:
    pass