#!/usr/bin/python

import hal, linuxcnc, sys, time, os

def writeTool(x):
    f = open("./current_tool.var", "w")
    f.write(str(x))
    f.close()
    return

def readTool():
    with open("./current_tool.var", "r") as f:
        for line in f:
            if line and line[0].isdigit():
                #print(int,line.rstrip().split())
                return sum(map(int,line.split()))
            else:
                return -1

def checkFile():
    if os.path.isfile("./current_tool.var") == False:
        print("file not found, creating file")
        writeTool(0)
        return


try:
    checkFile() #need to see if we even have a file
    s = linuxcnc.stat()  # connect to linuxcnc status channel
    s.poll()
    c = hal.component("ToolTracker")
    c.ready()
    #construct CMD
    cmd = linuxcnc.command()
    cmd.state(linuxcnc.STATE_ON)
    time.sleep(5)
    old_tool = 0
    old_tool = readTool()
    print("Tool at Shutdown was", old_tool)
   
    #first time we set it returns 0
    cmd.tool_offset(old_tool,
                    s.tool_table[old_tool].zoffset,
                    s.tool_table[old_tool].xoffset,
                    s.tool_table[old_tool].diameter,
                    s.tool_table[old_tool].frontangle,
                    s.tool_table[old_tool].backangle,
                    s.tool_table[old_tool].orientation)

    #print("tool ",old_tool)
    #print("zoff ",s.tool_table[old_tool].zoffset)
    #print("diam ",s.tool_table[old_tool].diameter)
    time.sleep(2) #wait a second
    s.poll() #poll status again
    #this time we get values.
    cmd.tool_offset(old_tool,
                    s.tool_table[old_tool].zoffset,
                    s.tool_table[old_tool].xoffset,
                    s.tool_table[old_tool].diameter,
                    s.tool_table[old_tool].frontangle,
                    s.tool_table[old_tool].backangle,
                    s.tool_table[old_tool].orientation)

    #print("tool ",old_tool)
    #print("zoff ",s.tool_table[old_tool].zoffset)
    #print("diam ",s.tool_table[old_tool].diameter)


    
    #cmd.mode(linuxcnc.MODE_MDI)
    #cmd.mdi("M61 Q" + str(old_tool))
    time.sleep(2)
    cmd.mode(linuxcnc.MODE_AUTO)
    
    while 1:
        s.poll()
        print("tool in spindle ", s.tool_in_spindle)

        if old_tool != s.tool_in_spindle:
            #print("Tool Changed")
            writeTool(s.tool_in_spindle)        

        time.sleep(20)



except KeyboardInterrupt:
    pass
'''
finally:
    print ('Tool Monitor has Crashed')
    raise SystemExit
'''