# V0.002 pass2 for plasmac - slowdown for holes and other small (<= minDiameter) profiles
#  long non-recursive version with queue to fake look ahead (no rewinding input with tell/seek due to exising file mode)
#  Notes: Python queue library qsize() returns incorrect queue size. LNct is used at some point to determine remaining queue entries.
import re, queue, math

LINESTRING = '{0}'
HOLESTRING = '{0} ({1})'
PROFILEQUEUESIZE = 1024 #probably excessive (DRAM?)
ZCOMMENTS = True # add or append removed Z move to line's comment

#precision at which traverse around profile closes to be considered "closed" profile (bad CAM or intended undercut)
#overcut coasting must continue along current/last segment and (if necessary) onto (the first) next segment
KP_PROFILEPRECISION = 0.001 #get from plasmac or LinuxCNC?

#remove
KP_KEEPEMPTYLINES = False #don't supress empty lines
KP_ADDEMPTYLINES = True #empty line after every STOPCUT
KP_PIERCE1 = 1 # CIP pierce
KP_PIERCE0 = 0 # normal pierce
#remove

# gcodes
STARTCUT = 'm3'
STOPCUT = 'm5'
ENDJOB1 = 'm30'
ENDJOB2 = 'm2'
ENDJOB3 = '%'
METRICUNITS = 'g21'
IMPERIALUNITS = 'g20'
G00MOVE = 'g0'
G01MOVE = 'g1'
G02MOVE = 'g2'
G03MOVE = 'g3'
TEMPHOLESTART ='M67 E3 Q60'
HOLEEND = 'M67 E3 Q0'

PC_IMPERIAL = 1
PC_METRIC  = 2
PC_DEFAULTMATERIAL = 0
PC_HOLES0 = 0 # plasmac hole - normal, as coded
PC_HOLES1 = 1 # plasmac hole - auto slowdown
PC_HOLES2 = 2 # plasmac hole - auto slowdown and overcut (coast)
PC_HOLEMODE = '#<holes>'
PC_OVERCUT = '#<oclength>'
PC_MDIA = '#<m_diameter>'
PC_IDIA = '#<i_diameter>'
#remove
KP_PIERCE = "#<pierce>"
KP_POST = "#<post>" #direcly modify CONSTANTS for easier/faster testing
#remove
DEBUG = True #verbose debugging text (as comments)

measuremode = PC_IMPERIAL
minDiameter = 1.25 #from plasmac
precision = 6 #from plasmac - check this in original code
scale = 0
holeEnable = overCut = False
ocLength = 0.157
line = rawline = commenttext =''
lastG = G00MOVE #fix this!!!! and ismove() - first line should be rapid, but maybe not - catch CAM modal errors
lastXmove = lastYmove = 0

firstmaterial = PC_DEFAULTMATERIAL
infile = 'input2.ngc' #exising RO file in plasmac
fRead = open(infile, 'r') #open input RO
line = fRead.readline() #read first line
pierceonlyComments = False # comment lines unnecessary for piercing that normally don't appear
Zcomments = False
pierceonly = True
pierceMode = KP_PIERCE0

def errorout(ErrorOut):
    return

def removeZword(line): #move Z axis motion to (a new) comment
    line = line.strip()
    if 'z' in line:
        try: zword = re.search(r'(([Zz]) *(-?\d+.?\d*))',line).group(1)
        except: return line
        line = re.sub(zword,'',line)
        if (ZCOMMENTS):
            if ((')' in line) and (' (' in line)):
                line = line[:line.index(')')] + ' *Z removed: ' + zword + '*' + line[line.index(')'):]
            else:
                line = line + ' (*Z removed ' + zword + '*)'
            line = line.strip()
    return line

def removeNword(line): #remove line number if present
    line = line.strip()
    if (line[0:1] == 'n'):
       nword = re.search(r'(^([Nn]\d+) *)',line).group(1)
       line = re.sub(nword,'',line).strip()
    return line

def cleanupGandMwords(line): #remove leading zeros in G and M
    if ('g0' in line):
        line = re.sub(r'([g]0?[4])','g4',line) #fix this!!!! general regex for all Gx Mx etc.
        line = re.sub(r'([g]0?[3])','g3',line)
        line = re.sub(r'([g]0?[2])','g2',line)
        line = re.sub(r'([g]0?[1])','g1',line)
        line = re.sub(r'([g]0?[0])','g0',line)
    if ('m0' in line):
        line = re.sub(r'([m]0?[2])','m2',line)
        line = re.sub(r'([m]0?[3])','m3',line)
        line = re.sub(r'([m]0?[5])','m5',line)
    return line

def iscomment(line): #find comment - assumes it begins the line #fix this!!!!
    line = line.strip()
    try:
        if (line.index('(') < 2): #weird python error suddenly appeared with valid commented gcode line...
            return True
    except:
        return False
    return False

def apxSize(minX,maxX,minY,maxY): #pick or add method for profile vs. minDiameter
    #return math.sqrt((maxX - minX)**2 + (maxY - minY)**2) #diagonal line across extents of 'hole' profile
    return (max(maxX-minX,maxY-minY)) #longest side of 'hole' profile

def ismove(lastG): # if modal motion append it to the last G0x
    global line
    if (('x' in line) or ('y' in line)) and (not iscomment(line) and (not '#<' in line)): # fix a modal line first (global result)
        if (len(lastG) > 0):
            if((not G00MOVE in line) and (not G01MOVE in line) and (not G02MOVE in line) and (not G03MOVE in line)):
                line = lastG + line
        else:
            return False #fix this!!!! should result in plasmac error
    if((G00MOVE in line) or (G01MOVE in line) or (G02MOVE in line) or (G03MOVE in line)): # then test for motion
        return True
    return False

def getminmaxXY(minX, maxX, minY, maxY, firstX, firstY):
    global line, lastG
    x, y, lastG = getXY(line)
    if ((x != 0) and (y != 0)):
        maxX = max(x,maxX); maxY = max(y,maxY)
        minX = min(x,minX); minY = min(y,minY)
        if (firstX == 9999): firstX = x
        if (firstY == 9999): firstY = y
    return minX, maxX, minY, maxY, firstX, firstY

def getXY(line): #get G0x, X, and Y from motion line
    global lastG

    def getXYval(selection, line): #eliminate a couple of individual fuctions with similar standalone regex
        if ((selection == 'x') and (selection in line)):
            try: x = float(re.search(r'(x) *(-?\d+.?\d*)',line).group(2))
            except: return 0
            return x
        if ((selection == 'y') and (selection in line)):
            try: y = float(re.search(r'(y) *(-?\d+.?\d*)',line).group(2))
            except: return 0
            return y
        return 0

    x = y = 0
    if ismove(lastG):
        x = getXYval('x',line)
        y = getXYval('y',line)
        if ('g' in line):
            try: lastG = re.search(r'([Gg]0?[01]?)',line).group(1) # return any form of the Gxx motion command
            except: x = y = 0
        if ((x+y) == 0): #fix this - add error/null checks
            return x,y,''
    return x, y, lastG

#def getXYZIJval(selection, line):
#    if (selection in line):
#        return float(re.search(r"(' + selection + ') *(-?\d+.?\d*)",line).group(2))

def getI(line):
    if (('i') in line):
        I = float(re.search(r'(i) *(-?\d+.?\d*)',line).group(2))
    return I

def getJ(line):
    if (('j') in line):
        J = float(re.search(r'(j) *(-?\d+.?\d*)',line).group(2))
    return J

def getX(line):
    x , y, lastG = getXY(line)
    return x

def getY(line):
    x , y, lastG = getXY(line)
    return y

def identifyProfiles(G0ct,G1ct,G2ct,G3ct,LNct): #tests and exceptions for profile pre/post processing will go here
    global commenttext
    commenttext = 'Start Profile Process'
    if ((G0ct < 1) and (G1ct+G2ct+G3ct+LNct > 0)): #success if legal cut motion was counted
        return True
    return False

def doPierce(x,y,pierceMode):
    if pierceMode == (KP_PIERCE0): #fixed pierce (normal)
        if (DEBUG): print('( pierce only at X{0} Y{1} )'.format(x,y))
        print('m3 $0 s1')
        print('g91')
        print('g1 X.000001')
        print('g90')
        print('m5')
        if (KP_ADDEMPTYLINES):
            print('')
#remove
    elif pierceMode == (KP_PIERCE1): #CIP pierce
        return
#remove
    return

def commentoutline(line): #make line a comment
    if (('(' in line) and (')' in line) and len(line.strip()) > 2 ):
        line = line.replace('(','') #fix this!!!! - regex
        line = line.replace(')','')
    if (len(line.strip()) > 0):
        line = '( ' + line.strip() + ' )'
        return line
    else: # empty line
        return ''

def processProfile(lastXmove, lastYmove):
    global lastG, line, rawline, pierceonly, pierceonlyComments, pierceMode
    linequeue = queue.Queue(PROFILEQUEUESIZE) #FIFO
    G0ct = G1ct = G2ct = G3ct = 0
    LNct = 1
    LNclose = 0
    ishole = closedprofile = False
    maxX = maxY = -9999
    minX = minY = firstX = firstY = 9999
    firstLNct = 0
    dummy1 = '' #nothing
    firstline = secondline = lastline = closingline= ''

    while ((line) and (not ENDJOB1 in line) and (not ENDJOB2 in line) and (not ENDJOB3 in line)):
        if ismove(lastG): #also inserts lastG before modal move
            minX, maxX, minY, maxY, firstX, firstY = getminmaxXY(minX, maxX, minY, maxY, firstX, firstY) # update position from current move
        if (STOPCUT in line):
            linequeue.put(line) #push last (M05) line
            if (pierceonly):
                doPierce(lastXmove, lastYmove, pierceMode) #pierce routine
                while(not linequeue.empty()): #empty the queue
                    line = linequeue.get(0)
                    if (pierceonlyComments): print(LINESTRING.format(commentoutline(line)))
            else: # Normal cut mode
                #if inside profle processing
#remove
                if (KP_ADDEMPTYLINES):
                    print('')
#remove
                ishole = identifyProfiles(G0ct,G1ct,G2ct,G3ct,LNct) #doesn do anything yet except check for some cutting action
                if ((ishole and (apxSize(minX,maxX,minY,maxY) <= minDiameter)) and holeEnable and closedprofile): #start inside profile processing
                    print(HOLESTRING.format(TEMPHOLESTART,commenttext))
                else:
                    ishole = False
                firstLNct = LNct
                while(not linequeue.empty()): #sequentially pop all saved lines
                    line = linequeue.get(0); LNct -= 1
                    if (LNct == (firstLNct -1)): # profile's first move after M03 - add tests and processing here
                        firstline = line
                    if (LNct == (firstLNct -2)): # profile's second move after M03 - in event first was a lead
                        secondline = line
                    if ((LNct == 1) and ishole and holeEnable): # profile's last move before M05 - add tests and processing here
                        lastline = line
                    print(LINESTRING.format(line))
                if (overCut and holeEnable and closedprofile and (ocLength > 0)): # do overcut along first line
                    line = line
                if (ishole and holeEnable and closedprofile): #end inside profile processing
                    print(HOLESTRING.format(HOLEEND,'End Profile Process'))
#remove
                    if (KP_ADDEMPTYLINES):
                        print('')
#remove
        if (LNct > 2): #check for closed profile - based on destination of all moves - which won't detect a profile (loop)/CAM errors
            testX, testY, dummy1 = getXY(line)
            if ((abs(float(testX-firstX)) <= KP_PROFILEPRECISION) and (abs(float(testY - firstY)) <= KP_PROFILEPRECISION)):
                closedprofile = True #above tests every line for profile closure, which won't detect CAM error (profile/lead-out)
                closingline = line # move that closed profile (should be first line, excluding lead)
                LNclose = LNct #move detected to have closed profile

        # check for funky gcode here - fix before queue to enable queuing fixes in sequence
        if (False): # 360 degree G02 and G03 arcs and split for dequeue processing
            line = line # fix this!!!!

        linequeue.put(line) #push current line
        #count moves in lines to guess profile type latter - save last move type to address modal lines
        if (G00MOVE in line): G0ct += 1; lastG = G00MOVE #detect any G00 move(s) in cut
        if (G01MOVE in line): G1ct += 1; lastG = G01MOVE
        if (G02MOVE in line): G2ct += 1; lastG = G02MOVE
        if (G03MOVE in line): G3ct += 1; lastG = G03MOVE
        LNct += 1
        line = fRead.readline() #get next line
        line = preprocessline(line)

def ProcessOtherLines(line): #deal with anything that will not appear in cut, while tracking any g00/G01 motion
    global lastG, lastXmove, lastYmove, measuremode, holeEnable  #function modifies lines (adds comments)
    global overCut, ocLength, firstmaterial, pierceMode, pierceonly
    global ZCOMMENTS, KP_ADDEMPTYLINES, KP_KEEPEMPTYLINES
    if ('g91' in line):
        errorout('G91')
    if ('g90.1' in line):
        errorout('g91.1')
    if ('m190' in line): # material
        try: material = re.match(r'm190 *p *(\d*)',line).group(1)
        except: material = PC_DEFAULTMATERIAL
        if (firstmaterial < 1):
            firstmaterial = material
            line = line + '( ' + 'get material name from plasmac and put here' + ' )'
            #Popen('halcmd setp plasmac_run.first-material {}'.format(material), stdout = PIPE, shell = True)
    if (IMPERIALUNITS in line): #process cut-time directives - plasmac's must begin on first line column
        measuremode = PC_IMPERIAL #freedom units mode
    if (METRICUNITS in line):
        measuremode = PC_METRIC #metric units mode
    if (PC_HOLEMODE in line): #<holes> = n
        try: holeval = int(re.search(r'^\#\< *holes *\> *\= *(\d*)',line).group(1))
        except: holeval = PC_HOLES0
        if (holeval == PC_HOLES0):
            line = line + ' (disable hole sensing)'
            holeEnable = overCut = False
        if (holeval == PC_HOLES1):
            line = line + ' (hole sensing - velocity reduction)'
            holeEnable = True ; overCut = False
        if (holeval == PC_HOLES2):
            if (ocLength > 0):
                holeEnable = overCut = True
                line = line + ' (hole sensing - velocity reduction - overcut)'
            else:
                holeEnable = True ; overCut = False
                line = line + ' (hole sensing - velocity reduction - overcut length = 0)'
        if (holeval > PC_HOLES2):
            line = line + ' (no hole sensing - unknown hole mode: ' + str(holeval) + ')'
            holeEnable = overCut = False
    if (PC_OVERCUT in line):  #fix this!!!! scale
        try: ocLength = float(re.search(r'^\#\< *oclength *\> *\= *(\d*\.\d*)',line).group(1))
        except: ocLength = 0
        if (ocLength <= 0):
            line = line + ' (overcut value for hole mode 2 must be larger than zero)'
            ocLength = 0 ; overCut = False
        else: line = line + ' (overcut value - active with hole mode 2 only)'
    if (PC_MDIA in line):  #fix this!!!! scale
        try: minDiameter = float(re.search(r'^\#\< *m_diameter *\> *\= *(\d*\.\d*)',line).group(1))
        except: minDiameter = 0
        if (minDiameter <= 0):
            line = line + ' (metric diameter = 0 - no hole sensing)'
            minDiameter = 0 ; holeEnable = overCut = False
        else: line = line + ' (metric diameter set for hole sensing)'
    if (PC_IDIA in line):  #fix this!!!! scale
        try: minDiameter = float(re.search(r'^\#\< *i_diameter *\> *\= *(\d*\.\d*)',line).group(1))
        except: minDiameter = 0
        if (minDiameter <= 0):
            line = line + ' (imperial diameter = 0 - no hole sensing)'
            minDiameter = 0 ; holeEnable = overCut = False
        else: line = line + ' (imperial diameter set for hole sensing)'

#remove
    if (KP_PIERCE in line): #<pierce> = n
        try: pierceMode = int(re.search(r'^\#\< *pierce *\> *\= *(\d*)',line).group(1))
        except: pierceMode = KP_PIERCE0
        if (pierceMode == KP_PIERCE0): line = line + ' (standard pierce)'
        if (pierceMode == KP_PIERCE1): line = line + ' (dynamic cip pierce)'

    if (KP_POST in line): #proecess post directives - modify constants
        if ('zcomments off' in line): ZCOMMENTS = False
        if ('zcomments on' in line): ZCOMMENTS = True

        if ('keepemptylines off' in line): KP_KEEPEMPTYLINES = False
        if ('keepemptylines on' in line): KP_KEEPEMPTYLINES = True

        if ('addemptylines off' in line): KP_ADDEMPTYLINES = False
        if ('addemptylines on' in line): KP_ADDEMPTYLINES = True

        if ('pierce only off' in line): pierceonly = False
        if ('pierce only on' in line): pierceonly = True
#remove

    if ismove(lastG): lastXmove, lastYmove, lastG = getXY(line) #track position (ie: rapid moves)
    return line

def preprocessline(line):
    global rawline
    rawline = line #keep original available JIC
    line = line.lower().strip()
    line = removeNword(line)
    line = removeZword(line)
    line = cleanupGandMwords(line)
    return line

while line:
    line = preprocessline(line) #modifies line (moves Z, removes n-word, etc.)
    if (STARTCUT in line): #process M03 to M05
        #select the default material here if no material so kerf value is available before processing (first) profile
        processProfile(lastXmove, lastYmove)
    else: #process everything else
        line = ProcessOtherLines(line) #modifies line (adds comments)
#remove
        if ((len(line) > 0) or (KP_KEEPEMPTYLINES)):
#remove
            print(LINESTRING.format(line)) #output all non-cut lines
    line = fRead.readline() #get next line
fRead.close()