// File: /workspace/github_cpp_makefile_star2_O1/jingdao[P]PointCloudApp/pcd.cpp
// Function: PCD::LoadFromCameraParameter(char const*)
PCD* PCD::LoadFromCameraParameter(const char* dir) {

	DIR* d = opendir(dir);
	if (!d) {
		printf("Invalid directory %s\n",dir);
		return NULL;
	}
	struct dirent *dt;
	int numRead=0; 
	PCD* cloud = new PCD(0);
	char buffer[1024];

	int ndir = strlen(dir);
	strncpy(buffer,dir,ndir);
	char* buffer_c = buffer + ndir;
	*buffer_c++ = '/';

	double focal_length = 600, w = 640, h = 480;
//	double focal_length = 1801, w = 1824, h =2736;
	w = (1 - w) / 2;
	h = (1 - h) / 2;

	while ((dt = readdir(d))) {
		int l = strlen(dt->d_name);
		if (strncmp(dt->d_name + l - 4,".txt",4) == 0) {
			strcpy(buffer_c,dt->d_name);
			FILE* f = fopen(buffer,"r");
			if (f) {
				char line[256];
				double projection[12], *p = projection, translation[3];
				float x,y,z,tmp,rotation[9];

				fgets(line,255,f);
				if (strncmp(line,"CONTOUR",7)!=0) 
					break;
				while (fgets(line,255,f)) {
					if (sscanf(line,"%lf %lf %lf %lf",p,p+1,p+2,p+3) == 4) {
						p += 4;
					}
				}
				translation[2] = -projection[11];
				translation[0] = (projection[3] - translation[2] * w) / focal_length;
				translation[1] = (translation[2] * h - projection[7]) / focal_length;
				rotation[6] = -projection[8];
				rotation[7] = -projection[9];
				rotation[8] = -projection[10];
				rotation[0] = (projection[0] - rotation[6] * w) / focal_length;
				rotation[1] = (projection[1] - rotation[7] * w) / focal_length;
				rotation[2] = (projection[2] - rotation[8] * w) / focal_length;
				rotation[3] = (rotation[6] * h - projection[4]) / focal_length;
				rotation[4] = (rotation[7] * h - projection[5]) / focal_length;
				rotation[5] = (rotation[8] * h - projection[6]) / focal_length;
				//transpose R
				tmp = rotation[1]; rotation[1] = rotation[3]; rotation[3] = tmp;
				tmp = rotation[2]; rotation[2] = rotation[6]; rotation[6] = tmp;
				tmp = rotation[5]; rotation[5] = rotation[7]; rotation[7] = tmp;
				// C = - R' * T
				x = - (rotation[0]*translation[0]+rotation[1]*translation[1]+rotation[2]*translation[2]);
				y = - (rotation[3]*translation[0]+rotation[4]*translation[1]+rotation[5]*translation[2]);
				z = - (rotation[6]*translation[0]+rotation[7]*translation[1]+rotation[8]*translation[2]);
				cloud->drawAxis(x,y,z,rotation,3,1);
				numRead++;
				fclose(f);
			}
		}
	}
	printf("Loaded %d camera parameters\n",numRead);
	closedir(d);

	cloud->drawAxis(0,0,0,NULL,10,1);
	return cloud;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x5b8, %rsp            # imm = 0x5B8
movq %rdi, %r15
callq 0x22d0
testq %rax, %rax
je 0x56ee
movq %rax, %r14
movl $0x78, %edi
callq 0x2200
movq %rax, %r12
movl $0x4, 0x40(%rax)
xorl %ebx, %ebx
movq %rbx, 0x50(%rax)
xorl %edi, %edi
callq 0x2270
movq %rax, 0x58(%r12)
movq %rbx, 0x48(%r12)
movq %rbx, 0x60(%r12)
movl $0x0, 0x68(%r12)
movq %rbx, 0x70(%r12)
xorps %xmm0, %xmm0
movups %xmm0, 0x1c(%r12)
movl $0x3f800000, 0x2c(%r12) # imm = 0x3F800000
movq %rbx, 0x30(%r12)
movabsq $0x3f80000000000000, %rax # imm = 0x3F80000000000000
movq %r12, 0x40(%rsp)
movq %rax, 0x38(%r12)
movq %r15, %rdi
callq 0x20c0
movslq %eax, %r12
leaq 0x1b0(%rsp), %rdi
movq %r15, %rsi
movq %r12, %rdx
callq 0x20e0
leaq (%rsp,%r12), %rax
addq $0x1b1, %rax            # imm = 0x1B1
movq %rax, 0x48(%rsp)
movb $0x2f, -0x1(%rax)
leaq 0xb0(%rsp), %rbp
leaq 0x64ab(%rip), %r15      # 0xb80d
movl $0x0, 0xc(%rsp)
movq %r14, %rdi
callq 0x22f0
testq %rax, %rax
je 0x56aa
movq %rax, %r13
leaq 0x13(%rax), %r12
movq %r12, %rdi
callq 0x20c0
cltq
leaq (%rax,%r13), %rdi
addq $0xf, %rdi
movl $0x4, %edx
leaq 0x64b9(%rip), %rsi      # 0xb859
callq 0x20d0
movb $0x1, %bl
testl %eax, %eax
je 0x53b4
testb %bl, %bl
jne 0x536a
jmp 0x56aa
movq 0x48(%rsp), %rdi
movq %r12, %rsi
callq 0x21c0
leaq 0x1b0(%rsp), %rdi
leaq 0x63d4(%rip), %rsi      # 0xb7a4
callq 0x2230
testq %rax, %rax
je 0x53ab
movq %rax, %r12
movq %rbp, %rdi
movl $0xff, %esi
movq %rax, %rdx
callq 0x2330
movl 0xb0(%rsp), %eax
movl $0x544e4f43, %ecx       # imm = 0x544E4F43
xorl %ecx, %eax
movl 0xb3(%rsp), %ecx
movl $0x52554f54, %edx       # imm = 0x52554F54
xorl %edx, %ecx
orl %eax, %ecx
sete %bl
jne 0x53ab
movq %rbp, %rdi
movl $0xff, %esi
movq %r12, %rdx
callq 0x2330
testq %rax, %rax
je 0x5469
leaq 0x50(%rsp), %r13
leaq 0x8(%r13), %rcx
leaq 0x10(%r13), %r8
leaq 0x18(%r13), %r9
movq %rbp, %rdi
movq %r15, %rsi
movq %r13, %rdx
xorl %eax, %eax
callq 0x21e0
xorl %ecx, %ecx
cmpl $0x4, %eax
sete %cl
shll $0x5, %ecx
addq %rcx, %r13
movq %rbp, %rdi
movl $0xff, %esi
movq %r12, %rdx
callq 0x2330
testq %rax, %rax
jne 0x542a
movsd 0xa8(%rsp), %xmm0
cvtsd2ss 0x90(%rsp), %xmm8
movaps 0x5f2c(%rip), %xmm14    # 0xb3b0
xorps %xmm14, %xmm8
movss %xmm8, 0x28(%rsp)
cvtsd2ss 0x98(%rsp), %xmm10
xorps %xmm14, %xmm10
cvtsd2ss 0xa0(%rsp), %xmm2
movss %xmm10, 0x2c(%rsp)
xorps %xmm14, %xmm2
movss %xmm2, 0x30(%rsp)
cvtss2sd %xmm8, %xmm1
movaps %xmm1, %xmm3
movsd 0x5ea0(%rip), %xmm11    # 0xb368
mulsd %xmm11, %xmm3
movsd 0x50(%rsp), %xmm4
subsd %xmm3, %xmm4
movsd 0x58(%rsp), %xmm5
movsd 0x5dea(%rip), %xmm12    # 0xb2d0
divsd %xmm12, %xmm4
cvtsd2ss %xmm4, %xmm4
movss %xmm4, 0x10(%rsp)
xorps %xmm3, %xmm3
cvtss2sd %xmm10, %xmm3
movaps %xmm3, %xmm6
mulsd %xmm11, %xmm6
subsd %xmm6, %xmm5
divsd %xmm12, %xmm5
xorps %xmm6, %xmm6
cvtsd2ss %xmm5, %xmm6
movsd 0x60(%rsp), %xmm5
cvtss2sd %xmm2, %xmm2
movaps %xmm2, %xmm7
mulsd %xmm11, %xmm7
subsd %xmm7, %xmm5
divsd %xmm12, %xmm5
cvtsd2ss %xmm5, %xmm5
movaps %xmm1, %xmm7
movsd 0x5e30(%rip), %xmm13    # 0xb370
mulsd %xmm13, %xmm7
subsd 0x70(%rsp), %xmm7
movss %xmm6, 0x14(%rsp)
divsd %xmm12, %xmm7
cvtsd2ss %xmm7, %xmm7
movaps %xmm3, %xmm9
mulsd %xmm13, %xmm9
subsd 0x78(%rsp), %xmm9
movss %xmm5, 0x18(%rsp)
divsd %xmm12, %xmm9
cvtsd2ss %xmm9, %xmm9
movss %xmm7, 0x1c(%rsp)
movss %xmm9, 0x20(%rsp)
movss %xmm7, 0x14(%rsp)
movss %xmm6, 0x1c(%rsp)
movss %xmm8, 0x18(%rsp)
movapd %xmm0, %xmm8
movss %xmm5, 0x28(%rsp)
movss %xmm10, 0x24(%rsp)
movaps %xmm2, %xmm10
mulsd %xmm0, %xmm1
mulsd %xmm0, %xmm3
mulsd %xmm0, %xmm2
mulsd %xmm11, %xmm8
addsd 0x68(%rsp), %xmm8
xorpd 0x5a40(%rip), %xmm0     # 0xb010
mulsd %xmm13, %xmm0
subsd 0x88(%rsp), %xmm0
divsd %xmm12, %xmm8
divsd %xmm12, %xmm0
cvtss2sd %xmm4, %xmm4
cvtss2sd %xmm7, %xmm7
mulsd %xmm0, %xmm7
mulsd %xmm8, %xmm4
addsd %xmm7, %xmm4
subsd %xmm1, %xmm4
xorps %xmm1, %xmm1
cvtss2sd %xmm6, %xmm1
xorps %xmm6, %xmm6
cvtss2sd %xmm9, %xmm6
mulsd %xmm0, %xmm6
mulsd %xmm8, %xmm1
addsd %xmm6, %xmm1
mulsd %xmm13, %xmm10
subsd 0x80(%rsp), %xmm10
subsd %xmm3, %xmm1
divsd %xmm12, %xmm10
xorps %xmm3, %xmm3
cvtsd2ss %xmm10, %xmm3
movss %xmm3, 0x2c(%rsp)
cvtss2sd %xmm3, %xmm3
mulsd %xmm0, %xmm3
cvtss2sd %xmm5, %xmm5
mulsd %xmm8, %xmm5
addsd %xmm3, %xmm5
xorps %xmm0, %xmm0
cvtsd2ss %xmm4, %xmm0
subsd %xmm2, %xmm5
xorps %xmm14, %xmm0
cvtsd2ss %xmm1, %xmm1
xorps %xmm14, %xmm1
xorps %xmm2, %xmm2
cvtsd2ss %xmm5, %xmm2
xorps %xmm14, %xmm2
movq 0x40(%rsp), %rdi
leaq 0x10(%rsp), %rsi
movss 0x5d64(%rip), %xmm3     # 0xb3f0
movss 0x5d48(%rip), %xmm4     # 0xb3dc
callq 0x3664
incl 0xc(%rsp)
movq %r12, %rdi
callq 0x21f0
jmp 0x53ab
leaq 0x616c(%rip), %rdi      # 0xb81d
movl 0xc(%rsp), %esi
xorl %eax, %eax
callq 0x2050
movq %r14, %rdi
callq 0x2350
movss 0x5d20(%rip), %xmm3     # 0xb3ec
movss 0x5d08(%rip), %xmm4     # 0xb3dc
xorps %xmm0, %xmm0
xorps %xmm1, %xmm1
xorps %xmm2, %xmm2
movq 0x40(%rsp), %rbx
movq %rbx, %rdi
xorl %esi, %esi
callq 0x3664
jmp 0x5701
leaq 0x60fa(%rip), %rdi      # 0xb7ef
xorl %ebx, %ebx
movq %r15, %rsi
xorl %eax, %eax
callq 0x2050
movq %rbx, %rax
addq $0x5b8, %rsp            # imm = 0x5B8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq