// File: /workspace/github_cpp_makefile_star2_O1/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 0x67ce(%rip), %rsi      # 0xb7a4
callq 0x2230
testq %rax, %rax
je 0x5213
movq %rax, %r12
movq %rbx, 0x8(%rsp)
movq %r14, (%rsp)
xorl %r14d, %r14d
movq %rax, %rdi
xorl %esi, %esi
movl $0x2, %edx
callq 0x2220
movq %r12, %rdi
callq 0x2030
leal 0x3(%rax), %r15d
testl %eax, %eax
cmovnsl %eax, %r15d
movl %r15d, %ebp
sarl $0x2, %ebp
sarl $0x4, %r15d
movl $0x78, %edi
callq 0x2200
movq %rax, %rbx
movl $0x4, 0x40(%rax)
movq %r14, 0x50(%rax)
andl $-0x4, %ebp
movslq %ebp, %r13
leaq (,%r13,4), %rdi
callq 0x2270
movq %rax, %rbp
movq %rax, 0x58(%rbx)
movq %r14, 0x48(%rbx)
movl %r15d, 0x60(%rbx)
movl %r15d, 0x64(%rbx)
movl $0x0, 0x68(%rbx)
movq %r14, 0x70(%rbx)
xorps %xmm0, %xmm0
movups %xmm0, 0x1c(%rbx)
movl $0x3f800000, 0x2c(%rbx) # imm = 0x3F800000
movq %r14, 0x30(%rbx)
movabsq $0x3f80000000000000, %rax # imm = 0x3F80000000000000
movq %rax, 0x38(%rbx)
movq %r12, %rdi
xorl %esi, %esi
xorl %edx, %edx
callq 0x2220
movl $0x4, %esi
movq %rbp, %rdi
movq %r13, %rdx
movq %r12, %rcx
callq 0x22c0
testl %r15d, %r15d
jle 0x50c9
movq 0x58(%rbx), %rax
movl %r15d, %ecx
shlq $0x4, %rcx
xorl %edx, %edx
movl $0x4b7f0000, 0xc(%rax,%rdx) # imm = 0x4B7F0000
addq $0x10, %rdx
cmpq %rdx, %rcx
jne 0x50b8
movq %r12, %rdi
callq 0x21f0
movq 0x8(%rsp), %rdi
testq %rdi, %rdi
je 0x5233
leaq 0x66be(%rip), %rsi      # 0xb7a4
callq 0x2230
testq %rax, %rax
je 0x5221
movq %rax, %r15
leaq 0x30(%rsp), %r12
movq %r12, %rdi
movl $0x200, %esi            # imm = 0x200
movq %rax, %rdx
callq 0x2330
leaq 0x28(%rsp), %r14
movq %r12, (%r14)
movq %r12, %rdi
movq %r14, %rsi
callq 0x2150
movsd %xmm0, 0x8(%rsp)
movq (%r14), %rdi
movq %r14, %rsi
callq 0x2150
movsd %xmm0, (%rsp)
movq (%r14), %rdi
movq %r14, %rsi
callq 0x2150
movsd %xmm0, 0x10(%rsp)
movq (%r14), %rdi
movq %r14, %rsi
callq 0x2150
movq (%r14), %rdi
movq %r14, %rsi
callq 0x2150
movq (%r14), %rdi
movq %r14, %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 0x2e2c
movslq 0x60(%rbx), %rax
testq %rax, %rax
jle 0x5209
movsd 0x20(%rsp), %xmm0
addsd 0x61b0(%rip), %xmm0     # 0xb350
cvtsd2ss %xmm0, %xmm0
movsd 0x18(%rsp), %xmm1
addsd 0x61a6(%rip), %xmm1     # 0xb358
cvtsd2ss %xmm1, %xmm1
movsd 0x10(%rsp), %xmm2
addsd 0x619c(%rip), %xmm2     # 0xb360
cvtsd2ss %xmm2, %xmm2
movq 0x58(%rbx), %rcx
shlq $0x4, %rax
xorl %edx, %edx
movss (%rcx,%rdx), %xmm3
addss %xmm0, %xmm3
movss %xmm3, (%rcx,%rdx)
movss 0x4(%rcx,%rdx), %xmm3
addss %xmm1, %xmm3
movss %xmm3, 0x4(%rcx,%rdx)
movss 0x8(%rcx,%rdx), %xmm3
addss %xmm2, %xmm3
movss %xmm3, 0x8(%rcx,%rdx)
addq $0x10, %rdx
cmpq %rdx, %rax
jne 0x51d2
movq %r15, %rdi
callq 0x21f0
jmp 0x5233
leaq 0x61e6(%rip), %rdi      # 0xb400
xorl %ebx, %ebx
movq %r14, %rsi
jmp 0x522c
leaq 0x61d8(%rip), %rdi      # 0xb400
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