import time
import samplePump

def read_until(uart, token, timeout_ms=3000):
    start = time.ticks_ms()
    buf = b""

    while time.ticks_diff(time.ticks_ms(), start) < timeout_ms:
        if uart.any():
            data = uart.read()
            if data:
                buf += data
                if token in buf:
                    return buf
        time.sleep_ms(5)

    return buf


def get_samplePump_uart():
    # samplepump.init() must have already run.
    if not hasattr(samplePump, "samplePumpUART"):
        raise RuntimeError("samplepump.samplePumpUART does not exist. Has samplepump.init() run?")

    return samplePump.samplePumpUART


def upload_roboteq_hex(filename="/flash/roboteq.hex"):
    uart = get_samplePump_uart()

    # Clear any stale bytes before entering loader mode.
    while uart.any():
        uart.read()

    uart.write(b"%SLD 321654987\r")

    resp = read_until(uart, b"HLD", 3000)
    if b"HLD" not in resp:
        raise RuntimeError("Roboteq did not enter HLD mode: %r" % resp)

    with open(filename, "rb") as f:
        line_num = 0

        for raw in f:
            line_num += 1
            line = raw.strip()

            if not line:
                continue

            uart.write(line + b"\r")

            ack = read_until(uart, b"+", 1000)
            if b"+" not in ack:
                raise RuntimeError("No + ack at line %d: %r" % (line_num, ack))

    return True