// File: /workspace/github_cpp_makefile_star2_O3/jingdao[P]PointCloudApp/pcd.cpp
// Function: PCD::LoadFromKITTI(char const*, char const*)
PCD* PCD::LoadFromKITTI(const char* fileName, const char* oxts) {
	FILE* f = fopen(fileName,"r");
	if (!f) {
		printf("File not found: %s\n", fileName);
		return NULL;
	}

	fseek (f, 0, SEEK_END);   // non-portable
	int size_bytes=ftell(f);
	int size = size_bytes / 4 / sizeof(float);

	PCD* p = new PCD(size);
	fseek(f,0,SEEK_SET);
	fread(p->float_data,sizeof(float),size*4,f);
	
	//map reflectance to color
	for (int i=0;i<size;i++) {
//		p->float_data[i * 4 + 3] = colormap(p->float_data[i * 4 + 3]);
		p->float_data[i * 4 + 3] = 255 << 16;
//		p->float_data[i * 4 + 3] = (int) (255 * p->float_data[i * 4 + 3]) << 16;
	}
	fclose (f);
	if (!oxts) return p;

	f = fopen(oxts,"r");
	if (!f) {
		printf("File not found: %s\n", fileName);
		return p;
	}
	char buf[512];
	fgets(buf,512,f);
	char* c = buf;

	double latitude = strtod(c,&c);
	double longitude = strtod(c,&c);
	double altitude = strtod(c,&c);
	double roll = strtod(c,&c);
	double pitch = strtod(c,&c);
	double yaw = strtod(c,&c);
	double E=0,N=0;
	wgs2utm(latitude,longitude,&E,&N);
	p->translate(E - 457840.938302, N - 5428721.722951, altitude - 113.966202);

	fclose(f);
	return p;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x238, %rsp            # imm = 0x238
movq %rsi, %rbx
movq %rdi, %r14
leaq 0x687e(%rip), %rsi      # 0xb874
callq 0x2230
testq %rax, %rax
je 0x5301
movq %rax, %r13
movq %r14, (%rsp)
movq %rbx, 0x8(%rsp)
xorl %r15d, %r15d
movq %rax, %rdi
xorl %esi, %esi
movl $0x2, %edx
callq 0x2220
movq %r13, %rdi
callq 0x2030
leal 0x3(%rax), %r12d
testl %eax, %eax
cmovnsl %eax, %r12d
movl %r12d, %ebp
sarl $0x2, %ebp
sarl $0x4, %r12d
movl $0x78, %edi
callq 0x2200
movq %rax, %rbx
movl $0x4, 0x40(%rax)
movq %r15, 0x50(%rax)
andl $-0x4, %ebp
movslq %ebp, %rbp
leaq (,%rbp,4), %rdi
callq 0x2270
movq %rax, %r14
movq %rax, 0x58(%rbx)
movq %r15, 0x48(%rbx)
movl %r12d, 0x60(%rbx)
movl %r12d, 0x64(%rbx)
movl $0x0, 0x68(%rbx)
movq %r15, 0x70(%rbx)
pxor %xmm0, %xmm0
movdqu %xmm0, 0x1c(%rbx)
movl $0x3f800000, 0x2c(%rbx) # imm = 0x3F800000
movq %r15, 0x30(%rbx)
movq 0x8(%rsp), %r15
movabsq $0x3f80000000000000, %rax # imm = 0x3F80000000000000
movq %rax, 0x38(%rbx)
movq %r13, %rdi
xorl %esi, %esi
xorl %edx, %edx
callq 0x2220
movl $0x4, %esi
movq %r14, %rdi
movq %rbp, %rdx
movq %r13, %rcx
callq 0x22c0
testl %r12d, %r12d
jle 0x51d8
movl %r12d, %ecx
leal 0x3(%r12), %eax
decq %rcx
movq %rcx, %xmm0
pshufd $0x44, %xmm0, %xmm0     # xmm0 = xmm0[0,1,0,1]
shrl $0x2, %eax
shlq $0x6, %rax
movdqa 0x62e6(%rip), %xmm1     # 0xb3e0
movdqa 0x62ee(%rip), %xmm2     # 0xb3f0
xorl %ecx, %ecx
movdqa 0x62f4(%rip), %xmm3     # 0xb400
pxor %xmm3, %xmm0
pcmpeqd %xmm4, %xmm4
movdqa 0x62f4(%rip), %xmm5     # 0xb410
movdqa %xmm2, %xmm6
pxor %xmm3, %xmm6
movdqa %xmm6, %xmm7
pcmpgtd %xmm0, %xmm7
pcmpeqd %xmm0, %xmm6
pshufd $0xf5, %xmm6, %xmm8     # xmm8 = xmm6[1,1,3,3]
pand %xmm7, %xmm8
pshufd $0xf5, %xmm7, %xmm6     # xmm6 = xmm7[1,1,3,3]
por %xmm8, %xmm6
movd %xmm6, %edx
notl %edx
testb $0x1, %dl
je 0x5159
movl $0x4b7f0000, 0xc(%r14,%rcx) # imm = 0x4B7F0000
pxor %xmm4, %xmm6
pextrw $0x4, %xmm6, %edx
testb $0x1, %dl
je 0x5170
movl $0x4b7f0000, 0x1c(%r14,%rcx) # imm = 0x4B7F0000
movdqa %xmm1, %xmm6
pxor %xmm3, %xmm6
movdqa %xmm6, %xmm7
pcmpgtd %xmm0, %xmm7
pcmpeqd %xmm0, %xmm6
pshufd $0xf5, %xmm6, %xmm8     # xmm8 = xmm6[1,1,3,3]
pand %xmm7, %xmm8
pshufd $0xf5, %xmm7, %xmm6     # xmm6 = xmm7[1,1,3,3]
por %xmm8, %xmm6
pxor %xmm4, %xmm6
pextrw $0x0, %xmm6, %edx
testb $0x1, %dl
je 0x51b0
movl $0x4b7f0000, 0x2c(%r14,%rcx) # imm = 0x4B7F0000
pextrw $0x4, %xmm6, %edx
testb $0x1, %dl
je 0x51c3
movl $0x4b7f0000, 0x3c(%r14,%rcx) # imm = 0x4B7F0000
paddq %xmm5, %xmm2
paddq %xmm5, %xmm1
addq $0x40, %rcx
cmpq %rcx, %rax
jne 0x511c
movq %r13, %rdi
callq 0x21f0
testq %r15, %r15
je 0x5321
leaq 0x6684(%rip), %rsi      # 0xb874
movq %r15, %rdi
callq 0x2230
testq %rax, %rax
je 0x530f
movq %rax, %rbp
leaq 0x30(%rsp), %r13
movq %r13, %rdi
movl $0x200, %esi            # imm = 0x200
movq %rax, %rdx
callq 0x2330
leaq 0x28(%rsp), %r15
movq %r13, (%r15)
movq %r13, %rdi
movq %r15, %rsi
callq 0x2150
movq %xmm0, 0x8(%rsp)
movq (%r15), %rdi
movq %r15, %rsi
callq 0x2150
movq %xmm0, (%rsp)
movq (%r15), %rdi
movq %r15, %rsi
callq 0x2150
movq %xmm0, 0x10(%rsp)
movq (%r15), %rdi
movq %r15, %rsi
callq 0x2150
movq (%r15), %rdi
movq %r15, %rsi
callq 0x2150
movq (%r15), %rdi
movq %r15, %rsi
callq 0x2150
xorl %eax, %eax
leaq 0x20(%rsp), %rdi
movq %rax, (%rdi)
leaq 0x18(%rsp), %rsi
movq %rax, (%rsi)
movsd 0x8(%rsp), %xmm0
movsd (%rsp), %xmm1
callq 0x2e28
testl %r12d, %r12d
jle 0x52f7
movsd 0x20(%rsp), %xmm0
movhpd 0x18(%rsp), %xmm0       # xmm0 = xmm0[0],mem[0]
addpd 0x6171(%rip), %xmm0     # 0xb420
cvtpd2ps %xmm0, %xmm0
movsd 0x10(%rsp), %xmm1
addsd 0x606f(%rip), %xmm1     # 0xb330
cvtsd2ss %xmm1, %xmm1
movl %r12d, %eax
shlq $0x4, %rax
xorl %ecx, %ecx
movsd (%r14,%rcx), %xmm2
addps %xmm0, %xmm2
movlps %xmm2, (%r14,%rcx)
movss 0x8(%r14,%rcx), %xmm2
addss %xmm1, %xmm2
movss %xmm2, 0x8(%r14,%rcx)
addq $0x10, %rcx
cmpq %rcx, %rax
jne 0x52ce
movq %rbp, %rdi
callq 0x21f0
jmp 0x5321
leaq 0x61c8(%rip), %rdi      # 0xb4d0
xorl %ebx, %ebx
movq %r14, %rsi
jmp 0x531a
leaq 0x61ba(%rip), %rdi      # 0xb4d0
movq (%rsp), %rsi
xorl %eax, %eax
callq 0x2050
movq %rbx, %rax
addq $0x238, %rsp            # imm = 0x238
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq