#!/usr/bin/env python

import sys

def add_spaces(s):
    prev = ' '
    r = ""
    for c in s:
        if str(prev).isdigit() and str(c).isalpha():
            r = r + " "
        r = r + c
        prev = c
    return r

def convert_gcode(fi, fo):
    power = ""
    speed = ""
    mode_g0 = True

    for line in fi:
        if line.startswith("S"):
            power = " " + line.rstrip()
        elif line.startswith("F"):
            speed = " " + line.rstrip()
        elif line.startswith("M3 S0"):
            fo.write(line)
            fo.write("G0 X0 Y0 S0 F1000\n")
            fo.write("G1 X1 Y0 S1 F1000\n")
            fo.write("G0 X0 Y0 S0 F1000\n")
            fo.write("G1 X0 Y1 S1 F1000\n")
            fo.write("G0 X0 Y0 S0 F1000\n")
        else:
            s = add_spaces(line.rstrip())
            if line.startswith("G1"):
                mode_g0 = False
                s += power
                s += speed
            elif line.startswith("G0"):
                mode_g0 = True
                s += power
                if (power != " S0") and (power != ""):
                    print("Warning: G0 move with power not zero!" + power)
            elif line.startswith("X") or line.startswith("Y"):
                if mode_g0:
                    s = "G0 " + s
                    s += power
                    if (power != " S0") and (power != ""):
                        print("Warning: G0 move with power not zero!" + power)
                else:
                    s = "G1 " + s
                    s += power
                    s += speed
            s += "\n"
            fo.write(s)

def main():
    if len(sys.argv) < 3:
        print("Usage:")
        print("    " + sys.argv[0] + " input.nc output.gcode")
        sys.exit(1)

    in_file = sys.argv[1]
    out_file = sys.argv[2]

    with open(in_file, 'r') as fi, open(out_file, 'w') as fo:
        convert_gcode(fi, fo)

if __name__ == '__main__':
    main()