// File: 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
movq %rsp, %rbp
subq $0x290, %rsp            # imm = 0x290
movq %rdi, -0x10(%rbp)
movq %rsi, -0x18(%rbp)
movq -0x10(%rbp), %rdi
leaq 0xca6e(%rip), %rsi      # 0x1378c
callq 0x2230
movq %rax, -0x20(%rbp)
cmpq $0x0, -0x20(%rbp)
jne 0x6d4d
movq -0x10(%rbp), %rsi
leaq 0xc677(%rip), %rdi      # 0x133b0
movb $0x0, %al
callq 0x2050
movq $0x0, -0x8(%rbp)
jmp 0x6ff2
movq -0x20(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
movl $0x2, %edx
callq 0x2220
movq -0x20(%rbp), %rdi
callq 0x2030
movl %eax, -0x24(%rbp)
movl -0x24(%rbp), %eax
movl %eax, %ecx
addl $0x3, %ecx
testl %eax, %eax
cmovsl %ecx, %eax
sarl $0x2, %eax
cltq
shrq $0x2, %rax
movl %eax, -0x28(%rbp)
movl $0x78, %edi
callq 0x2200
movq %rax, %rdi
movq %rdi, %rax
movq %rax, -0x290(%rbp)
movl -0x28(%rbp), %esi
callq 0x41e0
jmp 0x6da5
movq -0x290(%rbp), %rax
movq %rax, -0x30(%rbp)
movq -0x20(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
xorl %edx, %edx
callq 0x2220
movq -0x30(%rbp), %rax
movq 0x58(%rax), %rdi
movl -0x28(%rbp), %eax
shll $0x2, %eax
movslq %eax, %rdx
movq -0x20(%rbp), %rcx
movl $0x4, %esi
callq 0x22a0
movl $0x0, -0x40(%rbp)
movl -0x40(%rbp), %eax
cmpl -0x28(%rbp), %eax
jge 0x6e3b
movq -0x30(%rbp), %rax
movq 0x58(%rax), %rax
movl -0x40(%rbp), %ecx
shll $0x2, %ecx
addl $0x3, %ecx
movslq %ecx, %rcx
movss 0xc56b(%rip), %xmm0     # 0x13374
movss %xmm0, (%rax,%rcx,4)
movl -0x40(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x40(%rbp)
jmp 0x6de5
movq -0x290(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x38(%rbp)
movl %eax, -0x3c(%rbp)
movl $0x78, %esi
callq 0x2210
jmp 0x6fff
movq -0x20(%rbp), %rdi
callq 0x21f0
cmpq $0x0, -0x18(%rbp)
jne 0x6e58
movq -0x30(%rbp), %rax
movq %rax, -0x8(%rbp)
jmp 0x6ff2
movq -0x18(%rbp), %rdi
leaq 0xc929(%rip), %rsi      # 0x1378c
callq 0x2230
movq %rax, -0x20(%rbp)
cmpq $0x0, -0x20(%rbp)
jne 0x6e92
movq -0x10(%rbp), %rsi
leaq 0xc532(%rip), %rdi      # 0x133b0
movb $0x0, %al
callq 0x2050
movq -0x30(%rbp), %rax
movq %rax, -0x8(%rbp)
jmp 0x6ff2
leaq -0x240(%rbp), %rdi
movq -0x20(%rbp), %rdx
movl $0x200, %esi            # imm = 0x200
callq 0x2300
leaq -0x240(%rbp), %rax
movq %rax, -0x248(%rbp)
movq -0x248(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x2150
movsd %xmm0, -0x250(%rbp)
movq -0x248(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x2150
movsd %xmm0, -0x258(%rbp)
movq -0x248(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x2150
movsd %xmm0, -0x260(%rbp)
movq -0x248(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x2150
movsd %xmm0, -0x268(%rbp)
movq -0x248(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x2150
movsd %xmm0, -0x270(%rbp)
movq -0x248(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x2150
movsd %xmm0, -0x278(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x280(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x288(%rbp)
movsd -0x250(%rbp), %xmm0
movsd -0x258(%rbp), %xmm1
leaq -0x280(%rbp), %rdi
leaq -0x288(%rbp), %rsi
callq 0x3760
movq -0x30(%rbp), %rdi
movsd -0x280(%rbp), %xmm0
movsd 0xc3a4(%rip), %xmm1     # 0x13348
subsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movsd -0x288(%rbp), %xmm1
movsd 0xc384(%rip), %xmm2     # 0x13340
subsd %xmm2, %xmm1
cvtsd2ss %xmm1, %xmm1
movsd -0x260(%rbp), %xmm2
movsd 0xc364(%rip), %xmm3     # 0x13338
subsd %xmm3, %xmm2
cvtsd2ss %xmm2, %xmm2
callq 0x7010
movq -0x20(%rbp), %rdi
callq 0x21f0
movq -0x30(%rbp), %rax
movq %rax, -0x8(%rbp)
movq -0x8(%rbp), %rax
addq $0x290, %rsp            # imm = 0x290
popq %rbp
retq
movq -0x38(%rbp), %rdi
callq 0x2380
nopl (%rax,%rax)