#!/usr/bin/env python3

import sys
import subprocess
import time
import re
import struct

class MSPDebug:
    INFO = "\x1b[94m"
    ERROR = "\x1b[93m"
    CLEAR = "\x1b[0m"

    def __init__(self):
        self.process = subprocess.Popen([
            "mspdebug",
            "-q",
            "--embedded",
            "rf2500"
        ], stdin=subprocess.PIPE, stdout=subprocess.PIPE)

        self.inp = self.process.stdin
        self.out = self.process.stdout
    
    def communicate(self, command):
        print(command)
        self.write(command)
        return self.read()

    def write(self, command):
        print(command)
        self.inp.write(command.encode("utf8") + b"\n")

    def read(self):
        lines = []

        while True:
            line = self.out.readline().rstrip().decode("utf8")
            print(line)

            if line == "" or line == "\\busy":
                continue

            if line == "\\ready":
                return lines

            if line.startswith(":"):
                print(MSPDebug.INFO + line[1:] + MSPDebug.CLEAR)
            elif line.startswith("!"):
                print(MSPDebug.ERROR + line[1:] + MSPDebug.CLEAR)

            lines.append(line)
    
    def run(self):
        time.sleep(1)
        self.write("run")
        lines = self.read()

        if any(x for x in lines if x == ":dconsole_breakpoint:"):
            self.console_read()

    def console_read(self):
        lines = self.communicate("md dconsole_buffer 128")

        buf = b""
        for line in lines:
            buf += bytes.fromhex(line[12:59])

        if buf[0] == 0x01:
            sys.stdout.write(str(int(struct.unpack_from("<B", buf, 1)[0])))
        elif buf[0] == 0x02:
            sys.stdout.write(str(int(struct.unpack_from("<H", buf, 1)[0])))
        elif buf[0] and 0x80:
            sys.stdout.write(buf[1:(buf[0] & 0x7F) + 1].decode("latin-1"))

    def close(self):
        self.process.send_signal(2) # SIGINT

msp = MSPDebug()

for cmd in sys.argv[1:]:
    msp.write(cmd)
    msp.read()

try:
    #msp.write("setbreak dconsole_breakpoint 0")
    
    while True:
        msp.run()
except KeyboardInterrupt:
    print("\nExiting...")
finally:
    msp.write("run")
    time.sleep(0.1)
    msp.close()