// File: /workspace/github_cpp_makefile_star2_O2/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 $0x248, %rsp            # imm = 0x248
movq %rsi, %rbx
movq %rdi, %r14
leaq 0x5db4(%rip), %rsi      # 0xa81c
callq 0x2240
testq %rax, %rax
je 0x4c18
movq %rax, %r12
movq %rbx, 0x8(%rsp)
xorl %ebx, %ebx
pushq $0x2
popq %rdx
movq %rax, %rdi
xorl %esi, %esi
callq 0x2230
movq %r12, %rdi
callq 0x2030
pushq $0x4
popq %rcx
cltd
idivl %ecx
movl %eax, %r15d
movl %eax, %r13d
sarl $0x2, %r13d
pushq $0x78
popq %rdi
callq 0x2200
movq %rax, %rbp
movq %rax, %rdi
movl %r13d, %esi
callq 0x3330
movq %r12, %rdi
xorl %esi, %esi
xorl %edx, %edx
callq 0x2230
movq %rbp, 0x10(%rsp)
movq 0x58(%rbp), %rbp
andl $-0x4, %r15d
movslq %r15d, %rdx
pushq $0x4
popq %rsi
movq %rbp, %rdi
movq %r12, %rcx
callq 0x22d0
testl %r13d, %r13d
cmovlel %ebx, %r13d
shlq $0x4, %r13
cmpq %rbx, %r13
je 0x4b03
movl $0x4b7f0000, 0xc(%rbp,%rbx) # imm = 0x4B7F0000
addq $0x10, %rbx
jmp 0x4af0
movq %r12, %rdi
callq 0x21f0
movq 0x8(%rsp), %rdi
testq %rdi, %rdi
je 0x4c23
leaq 0x5cfc(%rip), %rsi      # 0xa81c
callq 0x2240
testq %rax, %rax
movq 0x10(%rsp), %rbx
je 0x4c2a
movq %rax, %r15
leaq 0x40(%rsp), %r12
movq %r12, %rdi
movl $0x200, %esi            # imm = 0x200
movq %rax, %rdx
callq 0x2340
leaq 0x38(%rsp), %r14
movq %r12, (%r14)
movq %r12, %rdi
movq %r14, %rsi
callq 0x2150
movsd %xmm0, 0x20(%rsp)
movq (%r14), %rdi
movq %r14, %rsi
callq 0x2150
movsd %xmm0, 0x18(%rsp)
movq (%r14), %rdi
movq %r14, %rsi
callq 0x2150
movsd %xmm0, 0x8(%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
leaq 0x30(%rsp), %r14
andq $0x0, (%r14)
leaq 0x28(%rsp), %r12
andq $0x0, (%r12)
movsd 0x20(%rsp), %xmm0
movsd 0x18(%rsp), %xmm1
movq %r14, %rdi
movq %r12, %rsi
callq 0x2d98
movsd (%r14), %xmm0
addsd 0x5752(%rip), %xmm0     # 0xa330
cvtsd2ss %xmm0, %xmm0
movsd (%r12), %xmm1
addsd 0x5748(%rip), %xmm1     # 0xa338
cvtsd2ss %xmm1, %xmm1
movsd 0x8(%rsp), %xmm2
addsd 0x573e(%rip), %xmm2     # 0xa340
cvtsd2ss %xmm2, %xmm2
movq %rbx, %rdi
callq 0x4c50
movq %r15, %rdi
callq 0x21f0
jmp 0x4c3b
leaq 0x5859(%rip), %rdi      # 0xa478
xorl %ebx, %ebx
jmp 0x4c31
movq 0x10(%rsp), %rbx
jmp 0x4c3b
leaq 0x5847(%rip), %rdi      # 0xa478
movq %r14, %rsi
xorl %eax, %eax
callq 0x2050
movq %rbx, %rax
addq $0x248, %rsp            # imm = 0x248
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq