// File: /workspace/github_cpp_makefile_star2_O2/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 $0x548, %rsp            # imm = 0x548
leaq 0x63f8(%rip), %rsi      # 0xa81c
callq 0x2240
testq %rax, %rax
je 0x4a29
movq %rax, %rbx
movabsq $0x300000000, %r14      # imm = 0x300000000
leaq 0x140(%rsp), %r15
movq %r15, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %rax, %rdx
callq 0x2340
movq %r15, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %rbx, 0x20(%rsp)
movq %rbx, %rdx
callq 0x2340
movq %rsp, %rbx
movq %r15, (%rbx)
pushq $0xa
popq %rbp
movq %r15, %rdi
movq %rbx, %rsi
movl %ebp, %edx
callq 0x22a0
movq %rax, %r15
movq (%rbx), %rdi
movq %rbx, %rsi
movl %ebp, %edx
callq 0x22a0
movq %rax, 0x18(%rsp)
movq %r15, %rax
imulq %r14, %rax
movq %rax, %rdi
shrq $0x1e, %rdi
testq %rax, %rax
pushq $-0x1
popq %rax
cmovsq %rax, %rdi
callq 0x2040
movq %rax, 0x30(%rsp)
testl %r15d, %r15d
movl $0x0, %eax
movq %r15, 0x28(%rsp)
cmovgl %r15d, %eax
movq %rax, 0xb0(%rsp)
imulq $0xc, %rax, %r13
leaq 0x5fcf(%rip), %r15      # 0xa4ac
leaq 0x54(%rsp), %r12
xorl %ebp, %ebp
movq 0x20(%rsp), %r14
cmpq %rbp, %r13
je 0x460b
leaq 0x140(%rsp), %rbx
movq %rbx, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r14, %rdx
callq 0x2340
movq %rbx, %rdi
movq %r15, %rsi
leaq 0xc4(%rsp), %rdx
leaq 0xc0(%rsp), %rcx
leaq 0xbc(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbx, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r14, %rdx
callq 0x2340
movq %rbx, %rdi
movq %r15, %rsi
leaq 0x70(%rsp), %rdx
leaq 0x74(%rsp), %rcx
leaq 0x78(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbx, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r14, %rdx
callq 0x2340
movq %rbx, %rdi
movq %r15, %rsi
leaq 0x7c(%rsp), %rdx
leaq 0x80(%rsp), %rcx
leaq 0x84(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbx, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r14, %rdx
callq 0x2340
movq %rbx, %rdi
movq %r15, %rsi
leaq 0x88(%rsp), %rdx
leaq 0x8c(%rsp), %rcx
leaq 0x90(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %rbx, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %r14, %rdx
callq 0x2340
movq %rbx, %rdi
movq %r15, %rsi
movq %r12, %rdx
leaq 0x58(%rsp), %rcx
leaq 0x5c(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq 0x54(%rsp), %rax
movq 0x30(%rsp), %rcx
movq %rax, (%rcx,%rbp)
movl 0x5c(%rsp), %eax
movl %eax, 0x8(%rcx,%rbp)
addq $0xc, %rbp
jmp 0x44e9
movq 0x28(%rsp), %r13
movslq %r13d, %rbx
pushq $0x18
popq %r15
movq %rbx, %rax
mulq %r15
seto %cl
addq $0x8, %rax
pushq $-0x1
popq %r12
cmovbq %r12, %rax
testb %cl, %cl
cmoveq %rax, %r12
pushq $0x78
popq %rdi
callq 0x2200
movq %rax, %r14
movq %rax, %rdi
movq 0x18(%rsp), %rsi
callq 0x3330
movq %r14, 0xa8(%rsp)
movq 0x58(%r14), %rax
movq %rax, 0x60(%rsp)
movq %r13, %r14
shlq $0x20, %r14
movq %r12, %rdi
callq 0x2040
movq %rbx, (%rax)
movq %rax, 0xa0(%rsp)
leaq 0x8(%rax), %r12
testq %r14, %r14
je 0x46a2
imulq $0x18, %rbx, %rcx
addq $-0x18, %rcx
movq %rcx, %rax
xorl %edx, %edx
divq %r15
subq %rdx, %rcx
addq $0x18, %rcx
movq %r12, %rdi
xorl %esi, %esi
movq %rcx, %rdx
callq 0x2100
movq %r12, 0x68(%rsp)
cvtsi2sd %r13d, %xmm0
movsd %xmm0, 0x28(%rsp)
xorl %ecx, %ecx
movq 0x18(%rsp), %rax
testl %eax, %eax
cmovlel %ecx, %eax
movq %rax, 0x18(%rsp)
pushq $0xa
popq %rbp
xorl %eax, %eax
movq 0x30(%rsp), %r15
cmpl 0x18(%rsp), %eax
je 0x48ce
movl %eax, 0x44(%rsp)
leaq 0x140(%rsp), %r14
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq 0x20(%rsp), %rbx
movq %rbx, %rdx
callq 0x2340
movq %r14, %rdi
leaq 0x5daa(%rip), %rsi      # 0xa4ac
leaq 0x14(%rsp), %rdx
leaq 0x10(%rsp), %rcx
leaq 0xc(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %rbx, %rdx
callq 0x2340
movq %r14, %rdi
leaq 0x618c(%rip), %rsi      # 0xa8be
leaq 0x50(%rsp), %rdx
leaq 0x4c(%rsp), %rcx
leaq 0x48(%rsp), %r8
xorl %eax, %eax
callq 0x21e0
movl 0x50(%rsp), %eax
movl %eax, 0x3c(%rsp)
movl 0x4c(%rsp), %eax
shll $0x8, %eax
movl %eax, 0x40(%rsp)
movl 0x48(%rsp), %eax
movl %eax, 0x38(%rsp)
movq %r14, %rdi
movl $0x3ff, %esi            # imm = 0x3FF
movq %rbx, %rdx
callq 0x2340
movq %r14, (%rsp)
movq %r14, %rdi
movq %rsp, %rsi
movl %ebp, %edx
callq 0x22a0
movq %rax, %rbx
testl %ebx, %ebx
movl $0x0, %eax
cmovlel %eax, %ebx
subl $0x1, %ebx
jb 0x487b
movq (%rsp), %rdi
movq %rsp, %r14
movq %r14, %rsi
movl %ebp, %edx
callq 0x22a0
movq %rax, %r13
movq (%rsp), %rdi
movq %r14, %rsi
movl %ebp, %edx
callq 0x22a0
movq (%rsp), %rdi
movq %r14, %rsi
movl %ebp, %edx
callq 0x22a0
movq (%rsp), %rdi
movq %r14, %rsi
movl %ebp, %edx
callq 0x22a0
movslq %r13d, %r14
imulq $0x18, %r14, %r12
addq 0x68(%rsp), %r12
movq %r12, %rdi
leaq 0x14(%rsp), %rsi
callq 0x6ce2
movq %r12, %rdi
leaq 0x10(%rsp), %rsi
callq 0x6ce2
movq %r12, %rdi
leaq 0xc(%rsp), %rsi
callq 0x6ce2
movabsq $0x300000000, %rax      # imm = 0x300000000
imulq %rax, %r13
sarq $0x1e, %r13
leaq (%r15,%r13), %rsi
movq %r12, %rdi
callq 0x6ce2
leaq (%r15,%r13), %rsi
addq $0x4, %rsi
movq %r12, %rdi
callq 0x6ce2
leaq (%r15,%r13), %rsi
addq $0x8, %rsi
movq %r12, %rdi
callq 0x6ce2
cvtsi2sd %r14d, %xmm0
divsd 0x28(%rsp), %xmm0
cvtsd2ss %xmm0, %xmm0
callq 0x37de
movss %xmm0, 0xc8(%rsp)
movq %r12, %rdi
leaq 0xc8(%rsp), %rsi
callq 0x6f2a
jmp 0x4791
movl 0x3c(%rsp), %ecx
shll $0x10, %ecx
movl 0x40(%rsp), %eax
orl %ecx, %eax
orl 0x38(%rsp), %eax
cvtsi2ss %eax, %xmm0
movss 0x14(%rsp), %xmm1
movq 0x60(%rsp), %rax
movss %xmm1, (%rax)
movss 0x10(%rsp), %xmm1
movss %xmm1, 0x4(%rax)
movss 0xc(%rsp), %xmm1
movss %xmm1, 0x8(%rax)
movss %xmm0, 0xc(%rax)
addq $0x10, %rax
movq %rax, 0x60(%rsp)
movl 0x44(%rsp), %eax
incl %eax
jmp 0x46cd
leaq 0xc8(%rsp), %rbx
xorl %r15d, %r15d
movq %rbx, %rdi
xorl %esi, %esi
callq 0x3330
cmpq 0xb0(%rsp), %r15
pushq $0x18
popq %rbp
je 0x4975
imulq $0x18, %r15, %rcx
movq 0x68(%rsp), %rdx
leaq (%rdx,%rcx), %r13
movq 0x8(%rdx,%rcx), %rax
subq (%rdx,%rcx), %rax
sarq $0x2, %rax
xorl %edx, %edx
pushq $0x7
popq %rcx
divq %rcx
movslq %eax, %r14
xorl %r12d, %r12d
cmpq %r14, %r12
jge 0x496d
cmpq $0xa, %r12
jae 0x496d
movq (%r13), %rax
movss -0x18(%rax,%rbp), %xmm0
movss -0x14(%rax,%rbp), %xmm1
movss -0x10(%rax,%rbp), %xmm2
movss -0xc(%rax,%rbp), %xmm3
movss -0x8(%rax,%rbp), %xmm4
movss -0x4(%rax,%rbp), %xmm5
movss (%rax,%rbp), %xmm6
movq %rbx, %rdi
movss 0x5b09(%rip), %xmm7     # 0xa468
callq 0x344e
incq %r12
addq $0x1c, %rbp
jmp 0x491c
incq %r15
jmp 0x48e3
leaq 0xc8(%rsp), %rbx
movss 0x5ae7(%rip), %xmm3     # 0xa46c
movss 0x5acf(%rip), %xmm4     # 0xa45c
xorpd %xmm0, %xmm0
xorps %xmm1, %xmm1
xorps %xmm2, %xmm2
movq %rbx, %rdi
xorl %esi, %esi
callq 0x3532
movl 0x60(%rbx), %esi
leaq 0x5ea4(%rip), %rdi      # 0xa84f
xorl %eax, %eax
callq 0x2050
leaq 0x5ea7(%rip), %rsi      # 0xa860
movq %rbx, %rdi
callq 0x38d8
movq 0x20(%rsp), %rdi
callq 0x21f0
movq 0x30(%rsp), %rdi
callq 0x2270
movq 0xa0(%rsp), %r12
movq (%r12), %rax
imulq $0x18, %rax, %rbx
testq %rax, %rax
je 0x4a03
movq %r12, %r14
addq $-0x10, %r14
movq %rbx, %r15
leaq (%r14,%r15), %rdi
callq 0x6db0
addq $-0x18, %r15
jne 0x49f4
addq $0x8, %rbx
movq %r12, %rdi
movq %rbx, %rsi
callq 0x23f0
leaq 0xc8(%rsp), %rdi
callq 0x339e
movq 0xa8(%rsp), %rax
jmp 0x4a37
leaq 0x5f2f(%rip), %rdi      # 0xa95f
callq 0x2320
xorl %eax, %eax
addq $0x548, %rsp            # imm = 0x548
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop