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

if __name__ == "__main__":
    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:
            f.write("AX,AY,AZ,GX,GY,GZ,MX,MY,MZ\n")
            while True:
                line = ser.read_until()
                matches = line_re.match(line)
                if not matches:
                    continue
                nums = [str(numstr) for numstr in matches.groups()]
                if len(nums) != 9:
                    continue
                f.write(",".join(nums))
                f.write("\n")