import os
import sys

# add python path of microwave to sys.path
microwave_path = os.path.join(__file__, *(['..'] * 3))
microwave_path = os.path.abspath(microwave_path)
sys.path.insert(0, microwave_path)

from microwave.parsers import MicroWaveParser

if __name__ == '__main__':
    if len(sys.argv) != 2:
        print('Usage: python MicroWave2Ply.py [filename.bin]')
        exit(1)

    filename = sys.argv[1]
    (fn, ext) = os.path.splitext(filename)
    with open(filename, "rb") as udp_file:
        buf = udp_file.read()
        point_cloud = MicroWaveParser.ParserFrame(buf)
        with open(fn + ".ply", "w") as f:
            f.write("ply\n")
            f.write("format ascii 1.0\n")
            f.write("comment MicroWave2Ply generated\n")
            f.write("element vertex %d\n" % len(point_cloud))
            f.write("property float x\n")
            f.write("property float y\n")
            f.write("property float z\n")
            f.write("element face 0\n")
            f.write("property list uchar int vertex_indices\n")
            f.write("end_header\n")
            for p in point_cloud:
                x, y, z = p
                f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z))
            print("Done!")