#!/usr/bin/env python

def parse_gcode(filepath):
    points = []
    with open(filepath) as infile: #TODO: set this automagically to wherever the gcode is generated
        movement = False #set to True once we find a G0 or G1
        for line in infile:
            if "G0" in line or "G1" in line:
                #Let's move
                movement = True
            if "M5" in line:
                #end
                movement = False
                break
            if movement:
                #if we're moving: two cases
                #Either a) we have Z, so this is something we should send to the hardware team somehow
                if "Z" in line:
                    pass #TODO
                #Or b) we have X and Y and we use this to build our list
                elif "X" in line or "Y" in line:
                    new_point = [0, 0]
                    found_x = False
                    res = line.split(" ")
                    first = float(res[1][1:])
                    if "X" == res[1][0]:
                        found_x = True
                        new_point[0] = first
                    elif "Y" == res[1][0]:
                        found_x = False
                        new_point[1] = first
                    if len(res) == 3:
                        second = float(res[2][1:])
                        if "X" == res[2][0]:
                            new_point[0] = second
                        elif "Y" == res[2][0]:
                            new_point[1] = second
                    else:
                        if found_x: #set second value
                            new_point[0] = points[-1][0] #set it to previous value
                        else:
                            new_point[1] = points[-1][1] #set it to previous value
                            
                    points.append(new_point)
                
    return points
        
     
     

if __name__ == "__main__":
    print parse_gcode("samples/polygons.ngc")