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

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))

    ser = serial.serial_for_url("hwgrep://", baudrate=115200, parity=serial.PARITY_NONE,
                                stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
                                timeout=2.0)
    with ser:
        timestr = datetime.datetime.now().strftime("%Y-%m-%d %H-%M-%S")
        fname = "UGVDATA_{}.csv".format(timestr)
        with open(fname, "w") as 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)
                        continue
                    matches = line_re.match(line)
                    if not matches:
                        print("line did not match: ", line)
                        continue
                    nums = [str(numstr) for numstr in matches.groups()]
                    if len(nums) != 9:
                        continue
                    f.write(",".join(nums))
                    f.write("\n")
            except KeyboardInterrupt:
                print("interrupt")
                f.flush()
                return

if __name__ == "__main__":
    capture_data()