// File: /workspace/github_cpp_makefile_star2_O3/jingdao[P]PointCloudApp/pcd.cpp
// Function: PCD::LoadFromBundler(char const*)
PCD* PCD::LoadFromBundler(const char* fileName) {
	FILE* f = fopen(fileName,"r");
	if (!f) {
		printf("Input file not found\n");
		return 0;
	}

	const int buffer_size = 1024;
	int num_cameras,num_points;
	char buf[buffer_size], *ptr;
	float focal_length,distortion1,distortion2;
	float rotation[9];
	float translation[3];
	float x,y,z;
	float rgb_f;
	int r,g,b,rgb;
	float* camera_positions, *camera_rotations;

	fgets(buf,buffer_size-1,f); //header
	fgets(buf,buffer_size-1,f); //num_cameras, num_points
	ptr=buf;
	num_cameras = strtol(ptr,&ptr,10);
	num_points = strtol(ptr,&ptr,10);
	camera_positions = new float[num_cameras * 3];
	camera_rotations = new float[num_cameras * 9];
	for (int i=0;i<num_cameras;i++) {
		fgets(buf,buffer_size-1,f); //read camera information
		if (!sscanf(buf, "%f %f %f",&focal_length,&distortion1,&distortion2) == 3) {
			printf("Error parsing %s\n",fileName);
			return NULL;
		}
		fgets(buf,buffer_size-1,f);
		if (!sscanf(buf, "%f %f %f",rotation,rotation+1,rotation+2) == 3) {
			printf("Error parsing %s\n",fileName);
			return NULL;
		}
		fgets(buf,buffer_size-1,f);
		if (!sscanf(buf, "%f %f %f",rotation+3,rotation+4,rotation+5) == 3) {
			printf("Error parsing %s\n",fileName);
			return NULL;
		}
		fgets(buf,buffer_size-1,f);
		if (!sscanf(buf, "%f %f %f",rotation+6,rotation+7,rotation+8) == 3) {
			printf("Error parsing %s\n",fileName);
			return NULL;
		}
		fgets(buf,buffer_size-1,f);
		if (!sscanf(buf, "%f %f %f",translation,translation+1,translation+2) == 3) {
			printf("Error parsing %s\n",fileName);
			return NULL;
		}
		memcpy(camera_positions + i * 3, translation, 3 * sizeof(float));
		memcpy(camera_rotations + i * 9, rotation, 9 * sizeof(float));
	}

	PCD* pointcloud = new PCD(num_points);
	float* data = pointcloud->float_data;
	std::vector<float> *lineInfo = new std::vector<float>[num_cameras];
	for (int i = 0; i < num_points; ++i) {
		fgets(buf,buffer_size-1,f); //read X,Y,Z position
		if (!sscanf(buf, "%f %f %f",&x,&y,&z) == 3) {
			printf("Error parsing %s\n",fileName);
			return NULL;
		}
		fgets(buf,buffer_size-1,f); //read R,G,B color
		if (!sscanf(buf, "%d %d %d",&r,&g,&b) == 3) {
			printf("Error parsing %s\n",fileName);
			return NULL;
		}
		rgb = ((r<<16)|(g<<8)|b);
		fgets(buf,buffer_size-1,f); //read view list
		ptr = buf;
		int numViews = strtol(ptr,&ptr,10);
		int cameraIndex;
//		int keyIndex;
//		float viewX,viewY;
		for (int i=0;i<numViews;i++) {
			cameraIndex = strtol(ptr,&ptr,10);
//			keyIndex = strtol(ptr,&ptr,10);
//			viewX = strtod(ptr,&ptr);
//			viewY = strtod(ptr,&ptr);
			strtol(ptr,&ptr,10);
			strtol(ptr,&ptr,10);
			strtol(ptr,&ptr,10);
			lineInfo[cameraIndex].push_back(x);
			lineInfo[cameraIndex].push_back(y);
			lineInfo[cameraIndex].push_back(z);
			lineInfo[cameraIndex].push_back(camera_positions[cameraIndex * 3]);
			lineInfo[cameraIndex].push_back(camera_positions[cameraIndex * 3 + 1]);
			lineInfo[cameraIndex].push_back(camera_positions[cameraIndex * 3 + 2]);
			lineInfo[cameraIndex].push_back(colormap(1.0*cameraIndex/num_cameras));
		}
		rgb_f = (float) rgb;
		*data++ = x;
		*data++ = y;
		*data++ = z;
		*data++ = rgb_f;
	}

	//draw lines from camera to keypoints on point cloud
	PCD lines(0);
	const float interval = 2;
	const int maxLines = 10;
	for (int j=0;j<num_cameras;j++) {
		int numLines = lineInfo[j].size() / 7;
		for (int i=0;i<numLines&&i<maxLines;i++) {
			lines.drawLine(
				lineInfo[j][i * 7],
				lineInfo[j][i * 7 + 1],
				lineInfo[j][i * 7 + 2],
				lineInfo[j][i * 7 + 3],
				lineInfo[j][i * 7 + 4],
				lineInfo[j][i * 7 + 5],
				lineInfo[j][i * 7 + 6],
				interval
			);
		}
	}
	lines.drawAxis(0,0,0,NULL,10,1);
	printf("Wrote %d points\n",lines.numPoints);
	lines.writeToPCD("p2.pcd");

	fclose(f);
	delete[] camera_positions;
	delete[] camera_rotations;
	delete[] lineInfo;
	return pointcloud;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x538, %rsp            # imm = 0x538
leaq 0x70d2(%rip), %rsi      # 0xb874
callq 0x2230
testq %rax, %rax
je 0x4fb8
movq %rax, %r13
leaq 0x130(%rsp), %r14
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %rax, %rdx
callq 0x2330
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r13, %rdx
callq 0x2330
leaq 0x8(%rsp), %rbx
movq %r14, (%rbx)
movq %r14, %rdi
movq %rbx, %rsi
movl $0xa, %edx
callq 0x2290
movq %rax, %r14
movq (%rbx), %rdi
movq %rbx, %rsi
movl $0xa, %edx
callq 0x2290
movq %rax, 0x28(%rsp)
movq %r14, %rax
shlq $0x20, %rax
movq %rax, 0x20(%rsp)
leaq (%rax,%rax,2), %rax
movq %rax, %rdi
shrq $0x1e, %rdi
testq %rax, %rax
movq $-0x1, %rax
cmovsq %rax, %rdi
callq 0x2040
movq %rax, 0x30(%rsp)
movq %r14, (%rsp)
testl %r14d, %r14d
jle 0x4970
movq (%rsp), %r15
andl $0x7fffffff, %r15d      # imm = 0x7FFFFFFF
leaq 0x130(%rsp), %rbp
leaq 0x6ca3(%rip), %rbx      # 0xb504
leaq 0x4c(%rsp), %r14
movq 0x30(%rsp), %r12
movq %rbp, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r13, %rdx
callq 0x2330
movq %rbp, %rdi
movq %rbx, %rsi
leaq 0xb4(%rsp), %rdx
leaq 0xb0(%rsp), %rcx
leaq 0xac(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbp, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r13, %rdx
callq 0x2330
movq %rbp, %rdi
movq %rbx, %rsi
leaq 0x60(%rsp), %rdx
leaq 0x64(%rsp), %rcx
leaq 0x68(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbp, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r13, %rdx
callq 0x2330
movq %rbp, %rdi
movq %rbx, %rsi
leaq 0x6c(%rsp), %rdx
leaq 0x70(%rsp), %rcx
leaq 0x74(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbp, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r13, %rdx
callq 0x2330
movq %rbp, %rdi
movq %rbx, %rsi
leaq 0x78(%rsp), %rdx
leaq 0x7c(%rsp), %rcx
leaq 0x80(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbp, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r13, %rdx
callq 0x2330
movq %rbp, %rdi
movq %rbx, %rsi
movq %r14, %rdx
leaq 0x50(%rsp), %rcx
leaq 0x54(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq 0x4c(%rsp), %rax
movq %rax, (%r12)
movl 0x54(%rsp), %eax
movl %eax, 0x8(%r12)
addq $0xc, %r12
decq %r15
jne 0x486b
movq %r13, 0x58(%rsp)
movslq (%rsp), %rbx
movl $0x18, %ecx
movq %rbx, %rax
mulq %rcx
seto %cl
addq $0x8, %rax
movq $-0x1, %rbp
cmovbq %rbp, %rax
testb %cl, %cl
cmoveq %rax, %rbp
movl $0x78, %edi
callq 0x2200
movq %rax, %r15
movl $0x4, 0x40(%rax)
xorl %r14d, %r14d
movq %r14, 0x50(%rax)
movq 0x28(%rsp), %r12
leal (,%r12,4), %eax
movslq %eax, %rdi
shlq $0x2, %rdi
callq 0x2270
movq %rax, %r13
movq %rax, 0x58(%r15)
movq %r14, 0x48(%r15)
movl %r12d, 0x60(%r15)
movl %r12d, 0x64(%r15)
movl $0x0, 0x68(%r15)
movq %r14, 0x70(%r15)
xorpd %xmm0, %xmm0
movupd %xmm0, 0x1c(%r15)
movl $0x3f800000, 0x2c(%r15) # imm = 0x3F800000
movq %r14, 0x30(%r15)
movabsq $0x3f80000000000000, %rax # imm = 0x3F80000000000000
movq %r15, 0x90(%rsp)
movq %rax, 0x38(%r15)
movq %rbp, %rdi
callq 0x2040
movq %rax, %r14
movq %rbx, (%rax)
leaq 0x8(%rax), %r12
cmpq $0x0, 0x20(%rsp)
je 0x4a68
leaq (%rbx,%rbx,2), %rax
leaq -0x18(,%rax,8), %rax
movabsq $-0x5555555555555555, %rcx # imm = 0xAAAAAAAAAAAAAAAB
mulq %rcx
shrq $0x4, %rdx
leaq (%rdx,%rdx,2), %rax
leaq 0x18(,%rax,8), %rdx
movq %r12, %rdi
xorl %esi, %esi
callq 0x2100
movq %r14, 0x88(%rsp)
cmpl $0x0, 0x28(%rsp)
movq %r12, 0x20(%rsp)
jle 0x4da0
xorps %xmm0, %xmm0
cvtsi2sdl (%rsp), %xmm0
movsd %xmm0, 0xa0(%rsp)
xorl %eax, %eax
movl %eax, 0x3c(%rsp)
movq %r13, 0x98(%rsp)
leaq 0x130(%rsp), %r14
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq 0x58(%rsp), %rbx
movq %rbx, %rdx
callq 0x2330
movq %r14, %rdi
leaq 0x6a3e(%rip), %rsi      # 0xb504
leaq 0x1c(%rsp), %rdx
leaq 0x18(%rsp), %rcx
leaq 0x14(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %rbx, %rdx
callq 0x2330
movq %r14, %rdi
leaq 0x6e20(%rip), %rsi      # 0xb916
leaq 0x48(%rsp), %rdx
leaq 0x44(%rsp), %rcx
leaq 0x40(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movl 0x48(%rsp), %eax
shll $0x10, %eax
movl 0x44(%rsp), %ecx
shll $0x8, %ecx
orl %eax, %ecx
orl 0x40(%rsp), %ecx
movl %ecx, 0x38(%rsp)
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %rbx, %rdx
callq 0x2330
movq %r14, 0x8(%rsp)
movq %r14, %rdi
leaq 0x8(%rsp), %rsi
movl $0xa, %edx
callq 0x2290
movq %rax, %r13
testl %r13d, %r13d
jle 0x4d51
movq 0x8(%rsp), %rdi
leaq 0x8(%rsp), %rbx
movq %rbx, %rsi
movl $0xa, %edx
callq 0x2290
movq %rax, %r15
movq 0x8(%rsp), %rdi
movq %rbx, %rsi
movl $0xa, %edx
callq 0x2290
movq 0x8(%rsp), %rdi
movq %rbx, %rsi
movl $0xa, %edx
callq 0x2290
movq 0x8(%rsp), %rdi
movq %rbx, %rsi
movl $0xa, %edx
callq 0x2290
movslq %r15d, %rax
leaq (%rax,%rax,2), %rax
leaq (%r12,%rax,8), %rbp
movq 0x8(%rbp), %r14
movq 0x10(%rbp), %r12
cmpq %r12, %r14
je 0x4bd4
movss 0x1c(%rsp), %xmm0
movss %xmm0, (%r14)
addq $0x4, %r14
movq %r14, 0x8(%rbp)
jmp 0x4bec
movq %rbp, %rdi
movq %r14, %rsi
leaq 0x1c(%rsp), %rdx
callq 0x7c96
movq 0x8(%rbp), %r14
movq 0x10(%rbp), %r12
cmpq %r12, %r14
je 0x4c06
movss 0x18(%rsp), %xmm0
movss %xmm0, (%r14)
addq $0x4, %r14
movq %r14, 0x8(%rbp)
jmp 0x4c1e
movq %rbp, %rdi
movq %r14, %rsi
leaq 0x18(%rsp), %rdx
callq 0x7c96
movq 0x8(%rbp), %r14
movq 0x10(%rbp), %r12
cmpq %r12, %r14
je 0x4c38
movss 0x14(%rsp), %xmm0
movss %xmm0, (%r14)
addq $0x4, %r14
movq %r14, 0x8(%rbp)
jmp 0x4c50
movq %rbp, %rdi
movq %r14, %rsi
leaq 0x14(%rsp), %rdx
callq 0x7c96
movq 0x8(%rbp), %r14
movq 0x10(%rbp), %r12
movq %r15, %rbx
movabsq $0x300000000, %rax      # imm = 0x300000000
imulq %rax, %rbx
sarq $0x1e, %rbx
addq 0x30(%rsp), %rbx
cmpq %r12, %r14
je 0x4c82
movss (%rbx), %xmm0
movss %xmm0, (%r14)
addq $0x4, %r14
movq %r14, 0x8(%rbp)
jmp 0x4c98
movq %rbp, %rdi
movq %r14, %rsi
movq %rbx, %rdx
callq 0x7c96
movq 0x8(%rbp), %r14
movq 0x10(%rbp), %r12
leaq 0x4(%rbx), %rdx
cmpq %r12, %r14
je 0x4cb4
movss (%rdx), %xmm0
movss %xmm0, (%r14)
addq $0x4, %r14
movq %r14, 0x8(%rbp)
jmp 0x4cc7
movq %rbp, %rdi
movq %r14, %rsi
callq 0x7c96
movq 0x8(%rbp), %r14
movq 0x10(%rbp), %r12
addq $0x8, %rbx
cmpq %r12, %r14
je 0x4ce3
movss (%rbx), %xmm0
movss %xmm0, (%r14)
addq $0x4, %r14
movq %r14, 0x8(%rbp)
jmp 0x4cf9
movq %rbp, %rdi
movq %r14, %rsi
movq %rbx, %rdx
callq 0x7c96
movq 0x8(%rbp), %r14
movq 0x10(%rbp), %r12
xorps %xmm0, %xmm0
cvtsi2sd %r15d, %xmm0
divsd 0xa0(%rsp), %xmm0
cvtsd2ss %xmm0, %xmm0
callq 0x3918
movss %xmm0, 0xb8(%rsp)
cmpq %r12, %r14
je 0x4d30
movss %xmm0, (%r14)
addq $0x4, %r14
movq %r14, 0x8(%rbp)
jmp 0x4d43
movq %rbp, %rdi
movq %r14, %rsi
leaq 0xb8(%rsp), %rdx
callq 0x7dd6
movq 0x20(%rsp), %r12
decl %r13d
jne 0x4b57
xorps %xmm0, %xmm0
cvtsi2ssl 0x38(%rsp), %xmm0
movss 0x1c(%rsp), %xmm1
movq 0x98(%rsp), %r13
movss %xmm1, (%r13)
movss 0x18(%rsp), %xmm1
movss %xmm1, 0x4(%r13)
movss 0x14(%rsp), %xmm1
movss %xmm1, 0x8(%r13)
movss %xmm0, 0xc(%r13)
addq $0x10, %r13
movl 0x3c(%rsp), %eax
incl %eax
cmpl 0x28(%rsp), %eax
jne 0x4a93
movl $0x4, 0xf8(%rsp)
xorl %ebx, %ebx
movq %rbx, 0x108(%rsp)
xorl %edi, %edi
callq 0x2270
movq %rax, 0x110(%rsp)
movq %rbx, 0x100(%rsp)
movq %rbx, 0x118(%rsp)
movl $0x0, 0x120(%rsp)
movq %rbx, 0x128(%rsp)
xorpd %xmm0, %xmm0
movupd %xmm0, 0xd4(%rsp)
movl $0x3f800000, 0xe4(%rsp) # imm = 0x3F800000
movq %rbx, 0xe8(%rsp)
movabsq $0x3f80000000000000, %rax # imm = 0x3F80000000000000
movq %rax, 0xf0(%rsp)
cmpl $0x0, (%rsp)
jle 0x4ef7
movq (%rsp), %rax
andl $0x7fffffff, %eax       # imm = 0x7FFFFFFF
movq %rax, (%rsp)
xorl %r14d, %r14d
movabsq $0x2492492492492493, %r15 # imm = 0x2492492492492493
leaq 0xb8(%rsp), %rbx
leaq (%r14,%r14,2), %rsi
movq 0x8(%r12,%rsi,8), %rcx
subq (%r12,%rsi,8), %rcx
sarq $0x2, %rcx
movq %rcx, %rax
mulq %r15
subq %rdx, %rcx
shrq %rcx
addq %rdx, %rcx
shrq $0x2, %rcx
testl %ecx, %ecx
jle 0x4ee5
movq 0x20(%rsp), %rax
leaq (%rax,%rsi,8), %r13
decl %ecx
cmpq $0x9, %rcx
movl $0x9, %eax
cmovaeq %rax, %rcx
leaq (%rcx,%rcx,8), %rax
leaq (%rax,%rax,2), %rax
leaq (%rcx,%rax), %rbp
addq $0x1c, %rbp
xorl %r12d, %r12d
movq (%r13), %rax
movss (%rax,%r12), %xmm0
movss 0x4(%rax,%r12), %xmm1
movss 0x8(%rax,%r12), %xmm2
movss 0xc(%rax,%r12), %xmm3
movss 0x10(%rax,%r12), %xmm4
movss 0x14(%rax,%r12), %xmm5
movss 0x18(%rax,%r12), %xmm6
movq %rbx, %rdi
movss 0x65e1(%rip), %xmm7     # 0xb4b8
callq 0x3552
addq $0x1c, %r12
cmpq %r12, %rbp
jne 0x4e98
incq %r14
cmpq (%rsp), %r14
movq 0x20(%rsp), %r12
jne 0x4e45
leaq 0xb8(%rsp), %rbx
movss 0x65b5(%rip), %xmm3     # 0xb4bc
movss 0x659d(%rip), %xmm4     # 0xb4ac
xorpd %xmm0, %xmm0
xorps %xmm1, %xmm1
xorps %xmm2, %xmm2
movq %rbx, %rdi
xorl %esi, %esi
callq 0x363c
movl 0x60(%rbx), %esi
leaq 0x697a(%rip), %rdi      # 0xb8a7
xorl %eax, %eax
callq 0x2050
leaq 0x697d(%rip), %rsi      # 0xb8b8
movq %rbx, %rdi
callq 0x3a1a
movq 0x58(%rsp), %rdi
callq 0x21f0
movq 0x30(%rsp), %rdi
callq 0x2260
movq 0x88(%rsp), %r15
movq (%r15), %rax
leaq (,%rax,8), %rcx
leaq (%rcx,%rcx,2), %rbx
testq %rax, %rax
je 0x4f92
movq %rbx, %r14
movq -0x10(%r15,%r14), %rdi
testq %rdi, %rdi
je 0x4f8c
movq (%r15,%r14), %rsi
subq %rdi, %rsi
callq 0x2210
addq $-0x18, %r14
jne 0x4f76
addq $0x8, %rbx
movq %r15, %rdi
movq %rbx, %rsi
callq 0x23e0
leaq 0xb8(%rsp), %rdi
callq 0x3494
movq 0x90(%rsp), %rax
jmp 0x4fc6
leaq 0x69f8(%rip), %rdi      # 0xb9b7
callq 0x2310
xorl %eax, %eax
addq $0x538, %rsp            # imm = 0x538
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq