// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/GSV/GSVRun.cpp
// Function: gaps::GSVRun::ReadLaserPoseFile(int, gaps::RNArray<gaps::GSVScanline*> const&, gaps::RNArray<gaps::GSVScan*>&)
int GSVRun::
ReadLaserPoseFile(int laser_index, const RNArray<GSVScanline *>& scanlines, RNArray<GSVScan *>& scanline_scans)
{
  // Create filename
  char filename[4096];
  sprintf(filename, "%s/%s/laser_pose_%d.txt", scene->RawDataDirectoryName(), Name(), laser_index+1);

  // Open file
  FILE *fp = fopen(filename, "r");
  if (!fp) {
    RNFail("Unable to open laser pose file: %s\n", filename);
    return 0;
  }

  // Read pose for each scanline
  int line_count = 0;
  const int buffer_size = 16*1024;
  char buffer[buffer_size];
  while (fgets(buffer, buffer_size, fp)) {
    // Parse line
    line_count++;
    if (buffer[0] == '\0') continue;
    if (buffer[0] == '#') continue;
    int scanline_index;
    double timestamp, position_x, position_y, position_z;
    double quaternion_x, quaternion_y, quaternion_z, quaternion_w; 
    if (sscanf(buffer, "%d%lf%lf%lf%lf%lf%lf%lf%lf", 
               &scanline_index, &timestamp,
               &position_x, &position_y, &position_z,
               &quaternion_x, &quaternion_y, &quaternion_z, &quaternion_w) != 9) {
      RNFail("Error reading line %d from %s\n", line_count, filename);
      return 0;
    }

    // Check scanline index (newer files have scanline == -1 in first line)
    if (scanline_index < 0) continue;

    // Get scan 
    GSVScan *scan = scanline_scans.Kth(scanline_index);

    // Get scanline
    GSVScanline *scanline = scanlines.Kth(scanline_index);

    // Set scanline pose
    R3Point viewpoint(position_x, position_y, position_z);
    R3Quaternion orientation(quaternion_w, quaternion_x, quaternion_y, quaternion_z);
    orientation.Rotate(R3Quaternion(R3posz_vector, -RN_PI_OVER_TWO));
    orientation.Rotate(R3Quaternion(R3posy_vector, RN_PI_OVER_TWO));
    GSVPose pose(viewpoint, orientation);
    scanline->SetPose(pose);
    
    // Set scanline timestamp
    scanline->SetTimestamp(timestamp);

#if 0
    // Check viewpoint movement
    const RNLength min_viewpoint_movement = 0.01;
    static R3Point previous_viewpoint(0, 0, 0);
    RNLength viewpoint_movement = R3Distance(viewpoint, previous_viewpoint);
    if (viewpoint_movement < min_viewpoint_movement) continue;
    previous_viewpoint = viewpoint;
#endif

    // Insert scanline into scan
    scan->InsertScanline(scanline);
  }

  // Close file
  fclose(fp);

  // Return success
  return 1;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x5108, %rsp           # imm = 0x5108
movq %rcx, 0x18(%rsp)
movq %rdx, 0x10(%rsp)
movq 0x8(%rdi), %rax
movq 0x48(%rdi), %rcx
movq 0x20(%rax), %rdx
leal 0x1(%rsi), %r8d
leaq 0xf4a06(%rip), %rsi     # 0x12be7e
xorl %ebx, %ebx
leaq 0x100(%rsp), %r14
movq %r14, %rdi
xorl %eax, %eax
callq 0x27140
leaq 0x115c10(%rip), %rsi    # 0x14d0a3
movq %r14, %rdi
callq 0x27b20
testq %rax, %rax
je 0x37674
movq %rax, %r14
leaq 0xffd5a(%rip), %rax     # 0x137208
movsd (%rax), %xmm0
movaps 0xf4807(%rip), %xmm1    # 0x12bcc0
movaps %xmm0, 0x70(%rsp)
xorps %xmm0, %xmm1
movaps %xmm1, 0x60(%rsp)
pushq $0x1
popq %rbp
leaq 0x1100(%rsp), %rbx
movq %r14, 0x8(%rsp)
movq %rbx, %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %r14, %rdx
callq 0x27fe0
testq %rax, %rax
je 0x37667
movzbl 0x1100(%rsp), %eax
cmpl $0x23, %eax
je 0x37660
testl %eax, %eax
je 0x37660
subq $0x8, %rsp
movq %rbx, %rdi
leaq 0xf48da(%rip), %rsi     # 0x12bdf0
leaq 0xc(%rsp), %rdx
leaq 0x60(%rsp), %rcx
leaq 0x58(%rsp), %r8
leaq 0x50(%rsp), %r9
xorl %eax, %eax
leaq 0x28(%rsp), %r10
pushq %r10
leaq 0x38(%rsp), %r10
pushq %r10
leaq 0x48(%rsp), %r10
pushq %r10
leaq 0x58(%rsp), %r10
pushq %r10
leaq 0x68(%rsp), %r10
pushq %r10
callq 0x27900
addq $0x30, %rsp
cmpl $0x9, %eax
jne 0x3768c
movslq 0x4(%rsp), %rax
testq %rax, %rax
js 0x37660
movq %rbx, %r14
movq 0x18(%rsp), %rcx
movq (%rcx), %rcx
movq (%rcx,%rax,8), %r13
movq 0x10(%rsp), %rcx
movq (%rcx), %rcx
movq (%rcx,%rax,8), %rbx
movsd 0x50(%rsp), %xmm0
movsd 0x48(%rsp), %xmm1
movsd 0x40(%rsp), %xmm2
leaq 0x88(%rsp), %rdi
callq 0xc0dfc
movsd 0x20(%rsp), %xmm0
movsd 0x38(%rsp), %xmm1
movsd 0x30(%rsp), %xmm2
movsd 0x28(%rsp), %xmm3
leaq 0xe0(%rsp), %r12
movq %r12, %rdi
callq 0xb0524
leaq 0xa0(%rsp), %r15
movq %r15, %rdi
leaq 0xfe715(%rip), %rsi     # 0x135cf8
movaps 0x60(%rsp), %xmm0
callq 0xb0812
movq %r12, %rdi
movq %r15, %rsi
callq 0xb1206
movq %r15, %rdi
leaq 0xfe6de(%rip), %rsi     # 0x135ce0
movaps 0x70(%rsp), %xmm0
callq 0xb0812
movq %r12, %rdi
movq %r15, %rsi
callq 0xb1206
movq %r15, %rdi
leaq 0x88(%rsp), %rsi
movq %r12, %rdx
callq 0x41d1e
movq %rbx, %rdi
movq %r15, %rsi
callq 0x3e4a6
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x78(%rbx)
movq %r13, %rdi
movq %rbx, %rsi
callq 0x3d726
leaq 0xa0(%rsp), %rdi
callq 0x41e6a
movq %r14, %rbx
movq 0x8(%rsp), %r14
incl %ebp
jmp 0x374d6
movq %r14, %rdi
callq 0x27930
pushq $0x1
popq %rbx
jmp 0x376a6
leaq 0xf47ce(%rip), %rdi     # 0x12be49
leaq 0x100(%rsp), %rsi
xorl %eax, %eax
callq 0xe434a
jmp 0x376a6
leaq 0xf4778(%rip), %rdi     # 0x12be0b
xorl %ebx, %ebx
leaq 0x100(%rsp), %rdx
movl %ebp, %esi
xorl %eax, %eax
callq 0xe434a
movl %ebx, %eax
addq $0x5108, %rsp           # imm = 0x5108
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq 0xa0(%rsp), %rdi
callq 0x41e6a
movq %rbx, %rdi
callq 0x28170