// File: /workspace/github_cpp_makefile_star2_O3/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 0x578a
movq %rax, %r14
movl $0x78, %edi
callq 0x2200
movq %rax, %rbx
movl $0x4, 0x40(%rax)
xorl %r12d, %r12d
movq %r12, 0x50(%rax)
xorl %edi, %edi
callq 0x2270
movq %rax, 0x58(%rbx)
movq %r12, 0x48(%rbx)
movq %r12, 0x60(%rbx)
movl $0x0, 0x68(%rbx)
movq %r12, 0x70(%rbx)
xorps %xmm0, %xmm0
movups %xmm0, 0x1c(%rbx)
movl $0x3f800000, 0x2c(%rbx) # imm = 0x3F800000
movq %r12, 0x30(%rbx)
movabsq $0x3f80000000000000, %rax # imm = 0x3F80000000000000
movq %rax, 0x38(%rbx)
movq %r15, %rdi
callq 0x20c0
movslq %eax, %r12
leaq 0x1b0(%rsp), %rdi
movq %r15, %rsi
movq %r12, %rdx
callq 0x20e0
addq %rsp, %r12
addq $0x1b1, %r12            # imm = 0x1B1
movb $0x2f, -0x1(%r12)
leaq 0x6504(%rip), %r13      # 0xb929
xorl %ebp, %ebp
movl %ebp, 0xc(%rsp)
movq %r14, %rdi
callq 0x22f0
testq %rax, %rax
je 0x57a4
movq %rax, %rbp
leaq 0x13(%rbp), %r15
movq %r15, %rdi
callq 0x20c0
cltq
leaq (%rax,%rbp), %rdi
addq $0xf, %rdi
movl $0x4, %edx
movq %r13, %rsi
callq 0x20d0
testl %eax, %eax
jne 0x548a
movq %r12, %rdi
movq %r15, %rsi
callq 0x21c0
leaq 0x1b0(%rsp), %rdi
leaq 0x63f4(%rip), %rsi      # 0xb874
callq 0x2230
testq %rax, %rax
jne 0x549f
movq %r14, %rdi
callq 0x22f0
movq %rax, %rbp
testq %rax, %rax
jne 0x543f
jmp 0x57a4
movq %rax, %r15
movq %rbx, 0x10(%rsp)
leaq 0xb0(%rsp), %rbx
movq %rbx, %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
setne %al
jne 0x579f
movb %al, 0xb(%rsp)
movq %r12, 0x18(%rsp)
movq %rbx, %rdi
movl $0xff, %esi
movq %r15, %rdx
callq 0x2330
testq %rax, %rax
leaq 0x63d4(%rip), %r12      # 0xb8dd
je 0x554f
leaq 0x50(%rsp), %rbp
leaq 0x8(%rbp), %rcx
leaq 0x10(%rbp), %r8
leaq 0x18(%rbp), %r9
movq %rbx, %rdi
movq %r12, %rsi
movq %rbp, %rdx
xorl %eax, %eax
callq 0x21e0
xorl %ecx, %ecx
cmpl $0x4, %eax
sete %cl
shll $0x5, %ecx
addq %rcx, %rbp
movq %rbx, %rdi
movl $0xff, %esi
movq %r15, %rdx
callq 0x2330
testq %rax, %rax
jne 0x5510
xorps %xmm3, %xmm3
cvtsd2ss 0x90(%rsp), %xmm3
movaps 0x5e9d(%rip), %xmm12    # 0xb400
xorps %xmm12, %xmm3
cvtsd2ss 0x98(%rsp), %xmm7
xorps %xmm12, %xmm7
cvtsd2ss 0xa0(%rsp), %xmm9
movaps 0x50(%rsp), %xmm6
cvtss2sd %xmm3, %xmm0
movaps %xmm0, %xmm4
movaps 0x5a7e(%rip), %xmm11    # 0xb010
xorps %xmm11, %xmm4
movups 0x58(%rsp), %xmm5
movaps 0x60(%rsp), %xmm1
movsd 0x70(%rsp), %xmm2
xorps %xmm11, %xmm2
movlhps %xmm2, %xmm6            # xmm6 = xmm6[0],xmm2[0]
movsd 0x78(%rsp), %xmm2
xorps %xmm11, %xmm2
movlhps %xmm2, %xmm5            # xmm5 = xmm5[0],xmm2[0]
movsd 0x80(%rsp), %xmm2
xorps %xmm11, %xmm2
movlhps %xmm2, %xmm1            # xmm1 = xmm1[0],xmm2[0]
xorps %xmm2, %xmm2
cvtss2sd %xmm7, %xmm2
movlhps %xmm0, %xmm4            # xmm4 = xmm4[0],xmm0[0]
movapd 0x5e53(%rip), %xmm13    # 0xb430
mulpd %xmm13, %xmm4
addpd %xmm6, %xmm4
movaps %xmm2, %xmm10
xorps %xmm11, %xmm10
movlhps %xmm2, %xmm10           # xmm10 = xmm10[0],xmm2[0]
mulpd %xmm13, %xmm10
addpd %xmm5, %xmm10
movsd 0xa8(%rsp), %xmm8
xorps %xmm12, %xmm9
movss %xmm9, 0x40(%rsp)
movapd 0x5e26(%rip), %xmm14    # 0xb440
divpd %xmm14, %xmm4
cvtpd2ps %xmm4, %xmm6
movlpd %xmm6, 0x20(%rsp)
divpd %xmm14, %xmm10
cvtpd2ps %xmm10, %xmm5
movlpd %xmm5, 0x2c(%rsp)
movss %xmm3, 0x28(%rsp)
movapd %xmm8, %xmm4
xorps %xmm3, %xmm3
cvtss2sd %xmm9, %xmm3
movss %xmm7, 0x34(%rsp)
movaps %xmm3, %xmm7
xorps %xmm11, %xmm7
movlhps %xmm3, %xmm7            # xmm7 = xmm7[0],xmm3[0]
mulsd %xmm8, %xmm0
mulsd %xmm8, %xmm2
mulsd %xmm8, %xmm3
xorpd %xmm11, %xmm8
mulsd 0x5cc0(%rip), %xmm4     # 0xb338
addsd 0x68(%rsp), %xmm4
movsd 0x5c49(%rip), %xmm9     # 0xb2d0
divsd %xmm9, %xmm4
mulsd 0x5cab(%rip), %xmm8     # 0xb340
subsd 0x88(%rsp), %xmm8
divsd %xmm9, %xmm8
mulpd %xmm13, %xmm7
addpd %xmm1, %xmm7
xorps %xmm1, %xmm1
cvtss2sd %xmm6, %xmm1
shufps $0x55, %xmm6, %xmm6     # xmm6 = xmm6[1,1,1,1]
cvtss2sd %xmm6, %xmm6
mulsd %xmm8, %xmm6
mulsd %xmm4, %xmm1
addsd %xmm6, %xmm1
divpd %xmm14, %xmm7
cvtpd2ps %xmm7, %xmm6
subsd %xmm0, %xmm1
xorps %xmm7, %xmm7
cvtss2sd %xmm5, %xmm7
shufps $0x55, %xmm5, %xmm5     # xmm5 = xmm5[1,1,1,1]
xorps %xmm0, %xmm0
cvtss2sd %xmm5, %xmm0
mulsd %xmm8, %xmm0
mulsd %xmm4, %xmm7
addsd %xmm0, %xmm7
movlpd %xmm6, 0x38(%rsp)
subsd %xmm2, %xmm7
xorps %xmm2, %xmm2
cvtss2sd %xmm6, %xmm2
shufps $0x55, %xmm6, %xmm6     # xmm6 = xmm6[1,1,1,1]
xorps %xmm0, %xmm0
cvtss2sd %xmm6, %xmm0
mulsd %xmm8, %xmm0
mulsd %xmm4, %xmm2
addsd %xmm0, %xmm2
subsd %xmm3, %xmm2
xorps %xmm0, %xmm0
cvtsd2ss %xmm1, %xmm0
xorl %r12d, %r12d
movb 0xb(%rsp), %al
movb %al, %r12b
xorps %xmm12, %xmm0
xorps %xmm1, %xmm1
cvtsd2ss %xmm7, %xmm1
xorps %xmm12, %xmm1
cvtsd2ss %xmm2, %xmm2
xorps %xmm12, %xmm2
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
leaq 0x20(%rsp), %rsi
movss 0x5d61(%rip), %xmm3     # 0xb4c0
movss 0x5d45(%rip), %xmm4     # 0xb4ac
callq 0x363c
movl 0xc(%rsp), %ebp
incl %ebp
movq %r15, %rdi
callq 0x21f0
testl %r12d, %r12d
movq 0x18(%rsp), %r12
je 0x5427
jmp 0x57a8
leaq 0x612e(%rip), %rdi      # 0xb8bf
xorl %ebx, %ebx
movq %r15, %rsi
xorl %eax, %eax
callq 0x2050
jmp 0x57e3
movq 0x10(%rsp), %rbx
movl 0xc(%rsp), %ebp
leaq 0x613e(%rip), %rdi      # 0xb8ed
movl %ebp, %esi
xorl %eax, %eax
callq 0x2050
movq %r14, %rdi
callq 0x2350
movss 0x5cf4(%rip), %xmm3     # 0xb4bc
movss 0x5cdc(%rip), %xmm4     # 0xb4ac
xorps %xmm0, %xmm0
xorps %xmm1, %xmm1
xorps %xmm2, %xmm2
movq %rbx, %rdi
xorl %esi, %esi
callq 0x363c
movq %rbx, %rax
addq $0x5b8, %rsp            # imm = 0x5B8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq