#include <stdio.h>

FILE *in,*out;

struct mypoint {
	double x,y,z;
	int r,g,b;
} p;

int numPoints=0;

int rgba(int r, int g, int b) {
	return b + 256*g +256*256*r;
}

void doHeader(FILE *h, int nP) {
	
  fprintf(h,"# .PCD v.7 - Point Cloud Data file format\n");
		fprintf(h,"FIELDS x y z rgb\n");
		fprintf(h,"SIZE 4 4 4 4\n");
                fprintf(h,"TYPE F F F U\n");
		fprintf(h,"WIDTH %d\n",nP);
		fprintf(h,"HEIGHT 1\n");
		fprintf(h,"POINTS %d\n",nP);
		fprintf(h,"DATA ascii\n");


}

int main(int argc, char **argv) {
  int u,v,d;
	if (argc!=3) {
		 printf("command line arguments infile, outfile missing\n");
		 return(0);
	}
    printf("Translating point cloud format in %s to %s.\n",argv[1],argv[2]);
	in = fopen(argv[1],"r");
	out=fopen(argv[2],"w");

	

	while (fscanf(in,"%lf, %lf, %lf, %d, %d, %d, %d, %d, %d\n",
			&p.x, &p.y, &p.z, &u, &v, &d, &p.r, &p.g, &p.b)!=EOF /*&& k<320*240*/) {
				numPoints++;
	}
	fclose(in);
	printf("%d points to be converted\n",numPoints);

 	in = fopen(argv[1],"r"); 
	doHeader(out,numPoints);

     while (fscanf(in,"%lf, %lf, %lf, %d, %d, %d, %d, %d, %d\n",
			&p.x, &p.y, &p.z, &u, &v, &d, &p.r, &p.g, &p.b)!=EOF /*&& k<320*240*/) {
				fprintf(out,"%lf %lf %lf %d\n", p.x, p.y, p.z, rgba(p.r, p.g, p.b));
	}
	fclose(in);
	fclose(out);
        printf("conversion done\n");

}