#!/usr/bin/env python

import sys
import serial

if len(sys.argv) < 3:
    print("Usage:")
    print("    " + sys.argv[0] + " /dev/serial/port input.gcode")
    sys.exit(1)

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

port = serial.Serial()
port.port = port_file
port.baudrate = 115200
#port.timeout = 0.0

max_cmd_buffer = 5
counter = 0

def connect_serial():
    try:
        port.open()
        if port.is_open:
            print("connected to: " + port_file)
        else:
            print("error connecting to " + port_file)
            sys.exit(1)
    except serial.serialutil.SerialException:
        print("error connecting to " + port_file)
        sys.exit(1)

def wait_for_text(s):
    while True:
        response = port.read_until().decode().strip()
        print("rx w: " + response)
        if response.startswith(s):
            break
        elif response.startswith("Error"):
            print(response)
            print("CNC returned error. aborting.")
            abort_print(False)
            port.close()
            sys.exit(1)

def abort_print(wait_for_oks = True):
    global counter

    print("tx: M5")
    port.write("M5\n".encode())
    counter += 1

    print("tx: G0 X0 Y0 F1000")
    port.write("G0 X0 Y0 F1000\n".encode())
    counter += 1

    if wait_for_oks:
        while counter > 0:
            wait_for_text("ok")
            counter -= 1

def send_file(f):
    global counter

    for line in f:
        if ";" in line:
            line = line.split(";")[0]

        line = line.strip()

        if len(line) <= 0:
            print("skipping empty line")
            continue

        print("tx: " + line)
        tx = line.encode() + b'\n'
        port.write(tx)

        counter += 1
        print("cnt=" + str(counter))

        while counter >= max_cmd_buffer:
            response = port.read_until().decode().strip()
            #print("rx: " + response)
            if response.startswith("ok"):
                counter -= 1
                print("cnt=" + str(counter))
            elif response.startswith("Error"):
                print(response)
                print("CNC returned error. aborting.")
                abort_print(False)
                port.close()
                sys.exit(1)

def main():
    connect_serial()

    print("waiting for CNC to reset")
    wait_for_text("start")

    print("auto-homing after reset")
    print("tx: G28")
    port.write("G28\n".encode())
    wait_for_text("ok")

    try:
        with open(in_file, 'r') as f:
            send_file(f)

        while counter > 0:
            wait_for_text("ok")
            counter -= 1

        print("workaround. kill when done.")
        while True:
            pass
    except KeyboardInterrupt:
        print("user interrupt. aborting.")
        abort_print(True)

    port.close()

if __name__ == '__main__':
    main()