// File: 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 %r14
pushq %rbx
subq $0x51c8, %rsp           # imm = 0x51C8
movq %rdi, 0x51b8(%rsp)
movl %esi, 0x51b4(%rsp)
movq %rdx, 0x51a8(%rsp)
movq %rcx, 0x51a0(%rsp)
movq 0x51b8(%rsp), %rax
movq %rax, 0x58(%rsp)
leaq 0x41a0(%rsp), %rcx
movq %rcx, 0x60(%rsp)
movq 0x8(%rax), %rdi
callq 0x47cf0
movq 0x58(%rsp), %rdi
movq %rax, 0x68(%rsp)
callq 0x44310
movq 0x60(%rsp), %rdi
movq 0x68(%rsp), %rdx
movq %rax, %rcx
movl 0x51b4(%rsp), %r8d
addl $0x1, %r8d
leaq 0x21f722(%rip), %rsi    # 0x266f7d
movb $0x0, %al
callq 0x2a280
leaq 0x41a0(%rsp), %rdi
leaq 0x23bb82(%rip), %rsi    # 0x2833f3
callq 0x2a520
movq %rax, 0x4198(%rsp)
cmpq $0x0, 0x4198(%rsp)
jne 0x478af
leaq 0x41a0(%rsp), %rsi
leaq 0x21f6b0(%rip), %rdi    # 0x266f48
movb $0x0, %al
callq 0x1cbff0
movl $0x0, 0x51c4(%rsp)
jmp 0x47b84
movl $0x0, 0x4194(%rsp)
movl $0x4000, 0x4190(%rsp) # imm = 0x4000
leaq 0x190(%rsp), %rdi
movq 0x4198(%rsp), %rdx
movl $0x4000, %esi           # imm = 0x4000
callq 0x29bd0
cmpq $0x0, %rax
je 0x47b6c
movl 0x4194(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x4194(%rsp)
movsbl 0x190(%rsp), %eax
cmpl $0x0, %eax
jne 0x47909
jmp 0x478c5
movsbl 0x190(%rsp), %eax
cmpl $0x23, %eax
jne 0x47918
jmp 0x478c5
leaq 0x190(%rsp), %rdi
leaq 0x21f5c8(%rip), %rsi    # 0x266eef
leaq 0x18c(%rsp), %rdx
leaq 0x180(%rsp), %rcx
leaq 0x178(%rsp), %r8
leaq 0x170(%rsp), %r9
leaq 0x168(%rsp), %r14
leaq 0x160(%rsp), %rbx
leaq 0x158(%rsp), %r11
leaq 0x150(%rsp), %r10
leaq 0x148(%rsp), %rax
movq %r14, (%rsp)
movq %rbx, 0x8(%rsp)
movq %r11, 0x10(%rsp)
movq %r10, 0x18(%rsp)
movq %rax, 0x20(%rsp)
movb $0x0, %al
callq 0x29150
cmpl $0x9, %eax
je 0x479c0
movl 0x4194(%rsp), %esi
leaq 0x41a0(%rsp), %rdx
leaq 0x21f561(%rip), %rdi    # 0x266f0a
movb $0x0, %al
callq 0x1cbff0
movl $0x0, 0x51c4(%rsp)
jmp 0x47b84
cmpl $0x0, 0x18c(%rsp)
jge 0x479cf
jmp 0x478c5
movq 0x51a0(%rsp), %rdi
movl 0x18c(%rsp), %esi
callq 0x3a3c0
movq %rax, 0x140(%rsp)
movq 0x51a8(%rsp), %rdi
movl 0x18c(%rsp), %esi
callq 0x3a3e0
movq %rax, 0x138(%rsp)
movsd 0x178(%rsp), %xmm0
movsd 0x170(%rsp), %xmm1
movsd 0x168(%rsp), %xmm2
leaq 0x120(%rsp), %rdi
movq %rdi, 0x40(%rsp)
callq 0x17fd90
movsd 0x148(%rsp), %xmm0
movsd 0x160(%rsp), %xmm1
movsd 0x158(%rsp), %xmm2
movsd 0x150(%rsp), %xmm3
leaq 0x100(%rsp), %rdi
movq %rdi, 0x48(%rsp)
callq 0x15e2d0
leaq 0x226517(%rip), %rax    # 0x26df88
movq %rax, 0x30(%rsp)
movsd (%rax), %xmm0
movaps 0x21f32f(%rip), %xmm1   # 0x266db0
pxor %xmm1, %xmm0
leaq 0x6bb964(%rip), %rsi    # 0x7033f0
leaq 0xe0(%rsp), %rdi
movq %rdi, 0x28(%rsp)
callq 0x15ea20
movq 0x28(%rsp), %rsi
movq 0x48(%rsp), %rdi
callq 0x160270
movq 0x30(%rsp), %rax
movsd (%rax), %xmm0
leaq 0x6bb91b(%rip), %rsi    # 0x7033d8
leaq 0xc0(%rsp), %rdi
movq %rdi, 0x38(%rsp)
callq 0x15ea20
movq 0x38(%rsp), %rsi
movq 0x48(%rsp), %rdi
callq 0x160270
movq 0x40(%rsp), %rsi
movq 0x48(%rsp), %rdx
leaq 0x80(%rsp), %rdi
movq %rdi, 0x50(%rsp)
callq 0x5e450
movq 0x50(%rsp), %rsi
movq 0x138(%rsp), %rdi
callq 0x57740
jmp 0x47b0e
movq 0x138(%rsp), %rdi
movsd 0x180(%rsp), %xmm0
callq 0x44670
jmp 0x47b26
movq 0x140(%rsp), %rdi
movq 0x138(%rsp), %rsi
callq 0x55a00
jmp 0x47b3d
leaq 0x80(%rsp), %rdi
callq 0x5e680
jmp 0x478c5
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x78(%rsp)
movl %eax, 0x74(%rsp)
leaq 0x80(%rsp), %rdi
callq 0x5e680
jmp 0x47b96
movq 0x4198(%rsp), %rdi
callq 0x297e0
movl $0x1, 0x51c4(%rsp)
movl 0x51c4(%rsp), %eax
addq $0x51c8, %rsp           # imm = 0x51C8
popq %rbx
popq %r14
retq
movq 0x78(%rsp), %rdi
callq 0x29c10