#!/usr/bin/python
import argparse
import subprocess

'''
Make sure pcl is installed!
'''

def write_ply_header(out_file, nr_of_points):
    out_file.write('''ply
format ascii 1.0
element vertex %d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
    ''' %(nr_of_points))


def main():
    parser = argparse.ArgumentParser(description='Convert xyz with colors to ply file format')
    parser.add_argument('xyz_file', type=str,
                        help='xyz input file')
    parser.add_argument('output_file', type=str,
                        help='output ply input file')
    results = parser.parse_args()
    xyz_file = results.xyz_file
    ply_file = results.output_file
    print(xyz_file)
    print(ply_file)
    nr_of_points = sum(1 for line in open(xyz_file))
    with open(xyz_file, 'r') as in_file, open(ply_file, 'w') as out_file:
        write_ply_header(out_file, nr_of_points)
        for line in in_file:
            out_file.write(line)

    # HACK get ply binary format by converting to pcd and back to ply
    pcd_file = ply_file + ".pcd"
    print("converting to {} and back to ply to make it in binary format".format(pcd_file))
    subprocess.check_output(["pcl_ply2pcd", ply_file, pcd_file])
    subprocess.check_output(["pcl_pcd2ply", pcd_file, ply_file])


if __name__ == "__main__":
    main()