// File: /workspace/github_cpp_makefile_star2_O1/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, 0x30(%rsp)
movq %rdx, 0x28(%rsp)
movq 0x8(%rdi), %rax
movq 0x48(%rdi), %rcx
movq 0x20(%rax), %rdx
leal 0x1(%rsi), %r8d
leaq 0x1157d9(%rip), %rsi    # 0x14ee7d
xorl %ebx, %ebx
leaq 0x100(%rsp), %r14
movq %r14, %rdi
xorl %eax, %eax
callq 0x27140
leaq 0x136d24(%rip), %rsi    # 0x1703e3
movq %r14, %rdi
callq 0x27b30
testq %rax, %rax
je 0x398f0
movq %rax, %r15
leaq 0x1100(%rsp), %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %rax, %rdx
callq 0x27fd0
testq %rax, %rax
je 0x398e1
leaq 0x120bd0(%rip), %rax    # 0x15a2c8
movsd (%rax), %xmm0
movaps 0x1155ad(%rip), %xmm1   # 0x14ecb0
movaps %xmm0, 0x70(%rsp)
xorps %xmm0, %xmm1
movaps %xmm1, 0x60(%rsp)
movl $0x1, %ebp
leaq 0x1100(%rsp), %r12
leaq 0x10(%rsp), %rbx
leaq 0x18(%rsp), %r14
leaq 0x20(%rsp), %r13
movzbl 0x1100(%rsp), %eax
testl %eax, %eax
je 0x398c6
cmpl $0x23, %eax
je 0x398c6
subq $0x8, %rsp
movq %r12, %rdi
leaq 0x11569c(%rip), %rsi    # 0x14edef
leaq 0x14(%rsp), %rdx
leaq 0x60(%rsp), %rcx
leaq 0x58(%rsp), %r8
leaq 0x50(%rsp), %r9
xorl %eax, %eax
pushq %rbx
pushq %r14
pushq %r13
leaq 0x58(%rsp), %r10
pushq %r10
leaq 0x68(%rsp), %r10
pushq %r10
callq 0x27900
addq $0x30, %rsp
cmpl $0x9, %eax
jne 0x3989e
movslq 0xc(%rsp), %rax
testq %rax, %rax
js 0x398bd
movq 0x30(%rsp), %rcx
movq (%rcx), %rcx
movq (%rcx,%rax,8), %rbx
movq 0x28(%rsp), %rcx
movq (%rcx), %rcx
movq (%rcx,%rax,8), %r14
movsd 0x50(%rsp), %xmm0
movsd 0x48(%rsp), %xmm1
movsd 0x40(%rsp), %xmm2
leaq 0x88(%rsp), %rdi
callq 0xd5e2e
movsd 0x10(%rsp), %xmm0
movsd 0x38(%rsp), %xmm1
movsd 0x20(%rsp), %xmm2
movsd 0x18(%rsp), %xmm3
leaq 0xe0(%rsp), %r12
movq %r12, %rdi
callq 0xc2ebe
leaq 0xa0(%rsp), %r13
movq %r13, %rdi
leaq 0x11f593(%rip), %rsi    # 0x158da0
movaps 0x60(%rsp), %xmm0
callq 0xc3182
movq %r12, %rdi
movq %r13, %rsi
callq 0xc3c7c
movq %r13, %rdi
leaq 0x11f55c(%rip), %rsi    # 0x158d88
movaps 0x70(%rsp), %xmm0
callq 0xc3182
movq %r12, %rdi
movq %r13, %rsi
callq 0xc3c7c
movq %r13, %rdi
leaq 0x88(%rsp), %rsi
movq %r12, %rdx
callq 0x4546e
movq %r14, %rdi
movq %r13, %rsi
callq 0x41686
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x78(%r14)
movq %rbx, %rdi
movq %r14, %rsi
callq 0x4071c
leaq 0xa0(%rsp), %rdi
callq 0x455ba
xorl %eax, %eax
leaq 0x10(%rsp), %rbx
leaq 0x18(%rsp), %r14
leaq 0x1100(%rsp), %r12
leaq 0x20(%rsp), %r13
jmp 0x398c2
leaq 0x115565(%rip), %rdi    # 0x14ee0a
movl %ebp, %esi
leaq 0x100(%rsp), %rdx
xorl %eax, %eax
callq 0xfd64a
movl $0x1, %eax
jmp 0x398c2
movl $0x2, %eax
testb $0x1, %al
jne 0x3991a
movq %r12, %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %r15, %rdx
callq 0x27fd0
incl %ebp
testq %rax, %rax
jne 0x3972c
movq %r15, %rdi
callq 0x27920
movl $0x1, %ebx
jmp 0x39906
leaq 0x115551(%rip), %rdi    # 0x14ee48
leaq 0x100(%rsp), %rsi
xorl %eax, %eax
callq 0xfd64a
movl %ebx, %eax
addq $0x5108, %rsp           # imm = 0x5108
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
xorl %ebx, %ebx
jmp 0x39906
movq %rax, %rbx
leaq 0xa0(%rsp), %rdi
callq 0x455ba
movq %rbx, %rdi
callq 0x28160