#!/usr/bin/env python3
import serial
import re
import datetime
import sys

def capture_data():
    num_re = r"([\-0-9\.]+)"
    line_re = re.compile(r".*inputs: acc=\({0}, {0}, {0}\) gyro=\({0}, {0}, {0}\) mag=\({0}, {0}, {0}\)".format(num_re))

    if len(sys.argv) >= 2:
        ser_url = sys.argv[1]
    else:
        ser_url = "hwgrep://"
    if len(sys.argv) >= 3:
        fname = sys.argv[2]
        f = open(fname, "w")
    else:
        f = sys.stdout
        # timestr = datetime.datetime.now().strftime("%Y-%m-%d %H-%M-%S")
        # fname = "UGVDATA_{}.csv".format(timestr)
    ser = serial.serial_for_url(ser_url, baudrate=115200, parity=serial.PARITY_NONE,
                                stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS)
    with ser, f:
        try:
            f.write("AX,AY,AZ,GX,GY,GZ,MX,MY,MZ\n")
            while True:
                try:
                    line = ser.read_until().decode("utf-8")
                except Exception as e:
                    print("line decode error: ", e, file=sys.stderr)
                    continue
                matches = line_re.match(line)
                if not matches:
                    print("line did not match: ", line, file=sys.stderr)
                    continue
                nums = [str(numstr) for numstr in matches.groups()]
                if len(nums) != 9:
                    continue
                f.write(",".join(nums))
                f.write("\n")
                f.flush()
        except KeyboardInterrupt:
            print("interrupt", file=sys.stderr)
        finally:
            f.flush()

if __name__ == "__main__":
    capture_data()