// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/GSV/GSVRun.cpp
// Function: gaps::GSVRun::ReadLaserObjFile(int, gaps::RNArray<gaps::GSVScanline*>&, gaps::RNArray<gaps::GSVScan*>&, int)
int GSVRun::
ReadLaserObjFile(int laser_index, RNArray<GSVScanline *>& scanlines, RNArray<GSVScan *>& scanline_scans, RNBoolean read_points)
{
  // Create filename
  char filename[4096];
  sprintf(filename, "%s/%s/laser_point_cloud_%d.obj", 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;
  }

  // Create laser
  GSVLaser *laser = NULL;
  while (NLasers() <= laser_index) 
    InsertLaser(new GSVLaser(GSV_SICK_LASER));
  laser = Laser(laser_index);

  // Read points for each scanline
  int line_count = 0;
  const int buffer_size = 16*1024;
  char buffer[buffer_size];
  char keyword[1024];
  GSVScanline *scanline  = NULL;
  int num_scanline_points = 0;
  const int max_scanline_points = 1024;
  GSVPoint scanline_points[max_scanline_points];
  while (fgets(buffer, buffer_size, fp)) {
    line_count++;
    if (buffer[0] == '\0') continue;
    if (buffer[0] == '#') continue;
    if (buffer[0] == 'v') {
      // Parse point
      double x, y, z; 
      if (sscanf(buffer, "%s%lf%lf%lf", keyword, &x, &y, &z) != 4) {
        RNFail("Error reading line %d from %s\n", line_count, filename);
        return 0;
      }

      // Insert point
      if (scanline) {
        if (num_scanline_points < max_scanline_points) {
          scanline_points[num_scanline_points].SetPosition(R3Point(x, y, z));
          num_scanline_points++;
        }
      }
    }
    else if (buffer[0] == 'g') {
      // Set points in previous scanline
      if (scanline) {
        if (read_points) {
          // Project car points onto ground
          if (num_scanline_points > 25) {
            for (int i = num_scanline_points - 25; i < num_scanline_points; i++) {
              R3Point position =  scanline_points[i].Position();
              position[2] = scanline_points[num_scanline_points-25].Position()[2];
              scanline_points[i].SetPosition(position);
            }
          }
          scanline->SetPoints(scanline_points, num_scanline_points);
        }
        num_scanline_points = 0;
        scanline = NULL;
      }

      // Get scanline string
      char scanline_string[1024];
      if (sscanf(buffer, "%s%s", keyword, scanline_string) != 2) {
        RNFail("Error reading line %d from %s\n", line_count, filename);
        return 0;
      }

      // Parse scanline string
      int segment_index = 0;
      // int scanline_index = 0;
      char *bufferp = strtok(scanline_string, "_ \n\t");
      if (bufferp) { 
        assert(!strcmp(bufferp, "seg"));
        bufferp = strtok(NULL, "_ \n\t"); 
        if (bufferp) {
          segment_index = atoi(bufferp);
          bufferp = strtok(NULL, "_ \n\t"); 
          if (bufferp) {
            assert(!strcmp(bufferp, "scanline"));
            bufferp = strtok(NULL, "_ \n\t"); 
            if (bufferp) {
              // scanline_index = atoi(bufferp);
            }
          }
        }
      }

      // Get/create segment
      GSVSegment *segment = NULL;
      while (NSegments() <= segment_index) 
        InsertSegment(new GSVSegment());
      segment = Segment(segment_index);
        
      // Get/create survey
      GSVSurvey *survey = NULL;
      while (segment->NSurveys() <= laser_index) 
        segment->InsertSurvey(new GSVSurvey());
      survey = segment->Survey(laser_index);

      // Insert survey into laser
      if (survey->Laser() == NULL) laser->InsertSurvey(survey);
      assert(survey->Laser() == laser);

      // Get/create scan
      GSVScan *scan = new GSVScan();
      survey->InsertScan(scan);

      // Create scanline
      scanline = new GSVScanline();

      // Insert scanline into array to be returned
      scanlines.Insert(scanline);

      // Insert scan into array to be returned (so that can insert scanlines conditionally based on pose)
      scanline_scans.Insert(scan);
    }
  }

  // Set points in last scanline
  if (scanline) {
    if (read_points) {
      // Project car points onto ground
      if (num_scanline_points > 25) {
        for (int i = num_scanline_points - 25; i < num_scanline_points; i++) {
          R3Point position =  scanline_points[i].Position();
          position[2] = scanline_points[num_scanline_points-25].Position()[2];
          scanline_points[i].SetPosition(position);
        }
      }
      scanline->SetPoints(scanline_points, num_scanline_points);
    }
    num_scanline_points = 0;
    scanline = NULL;
  }

  // Close file
  fclose(fp);

  // Return success
  return 1;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x19888, %rsp          # imm = 0x19888
movl %r8d, %r14d
movq %rcx, 0x60(%rsp)
movq %rdx, 0x58(%rsp)
movl %esi, %r12d
movq %rdi, %r13
movq 0x8(%rdi), %rax
movq 0x48(%rdi), %rcx
movq 0x20(%rax), %rdx
leal 0x1(%r12), %r8d
leaq 0x115e3d(%rip), %rsi    # 0x14ee29
xorl %ebp, %ebp
leaq 0x880(%rsp), %rbx
movq %rbx, %rdi
xorl %eax, %eax
callq 0x27140
leaq 0x1373dc(%rip), %rsi    # 0x1703e3
movq %rbx, %rdi
callq 0x27b30
testq %rax, %rax
je 0x394fb
movl %r14d, 0x14(%rsp)
movq %rax, 0x28(%rsp)
cmpl %r12d, 0x44(%r13)
jg 0x3906b
leaq 0x38(%r13), %r14
movl $0x78, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
movl $0x2, %esi
callq 0x3ee38
movq %r13, 0x8(%r15)
movl 0x44(%r13), %edx
movl %edx, 0x10(%r15)
movq %r14, %rdi
movq %r15, %rsi
callq 0xfa862
movq %r13, %rdi
callq 0x38086
cmpl %r12d, 0x44(%r13)
jle 0x3902c
movq 0x38(%r13), %rax
movslq %r12d, %rcx
movq %rcx, 0x50(%rsp)
movq (%rax,%rcx,8), %rbp
xorl %ebx, %ebx
leaq (%rsp,%rbx), %rdi
addq $0x5880, %rdi           # imm = 0x5880
callq 0x459f0
addq $0x50, %rbx
cmpq $0x14000, %rbx          # imm = 0x14000
jne 0x3907d
leaq 0x1880(%rsp), %rdi
movl $0x4000, %esi           # imm = 0x4000
movq 0x28(%rsp), %rbx
movq %rbx, %rdx
callq 0x27fd0
testq %rax, %rax
movl 0x14(%rsp), %r14d
je 0x39518
movq %rbp, 0x48(%rsp)
leaq 0x18(%r13), %rax
movq %rax, 0x68(%rsp)
leaq 0x30(%rsp), %rdx
movq $0x0, 0x8(%rsp)
movq $0x0, 0x18(%rsp)
xorl %ebp, %ebp
incl %ebp
movq 0x8(%rsp), %r15
movzbl 0x1880(%rsp), %eax
cmpl $0x22, %eax
jle 0x39104
cmpl $0x23, %eax
je 0x3910c
jmp 0x39132
testl %eax, %eax
jne 0x391fa
leaq 0x1880(%rsp), %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %rbx, %rdx
callq 0x27fd0
leaq 0x30(%rsp), %rdx
incl %ebp
testq %rax, %rax
jne 0x390f0
jmp 0x39516
cmpl $0x67, %eax
leaq 0x80(%rsp), %rcx
je 0x391ff
cmpl $0x76, %eax
jne 0x391fa
movq %rdx, %rcx
leaq 0x1880(%rsp), %rdi
leaq 0x115d0e(%rip), %rsi    # 0x14ee6c
leaq 0x480(%rsp), %rdx
leaq 0x78(%rsp), %r8
leaq 0x70(%rsp), %r9
xorl %eax, %eax
callq 0x27900
cmpl $0x4, %eax
jne 0x393e7
cmpq $0x0, 0x18(%rsp)
je 0x3940a
cmpl $0x3ff, %r15d           # imm = 0x3FF
jg 0x3940a
movl %eax, 0x24(%rsp)
movslq %r15d, %rax
leaq (%rax,%rax,4), %rbx
shlq $0x4, %rbx
movsd 0x30(%rsp), %xmm0
movsd 0x78(%rsp), %xmm1
movsd 0x70(%rsp), %xmm2
leaq 0x80(%rsp), %rdi
callq 0xd5e2e
movl 0x24(%rsp), %eax
movaps 0x80(%rsp), %xmm0
movaps %xmm0, 0x5880(%rsp,%rbx)
movsd 0x90(%rsp), %xmm0
movsd %xmm0, 0x5890(%rsp,%rbx)
movq 0x28(%rsp), %rbx
incl %r15d
jmp 0x3940a
jmp 0x394ce
testl %r14d, %r14d
sete %al
cmpq $0x0, 0x18(%rsp)
sete %dl
movl $0x0, %esi
cmovel %r15d, %esi
orb %al, %dl
jne 0x392f4
cmpl $0x1a, %r15d
jl 0x392c9
movq 0x8(%rsp), %rax
addl $-0x19, %eax
leaq (%rax,%rax,4), %rcx
shlq $0x4, %rcx
leaq (%rsp,%rcx), %r15
addq $0x5880, %r15           # imm = 0x5880
cltq
leaq (%rax,%rax,4), %rax
shlq $0x4, %rax
leaq (%rsp,%rax), %r14
addq $0x5880, %r14           # imm = 0x5880
movl $0x10, %ebx
movsd -0x10(%r14,%rbx), %xmm0
movsd -0x8(%r14,%rbx), %xmm1
movsd (%r14,%rbx), %xmm2
leaq 0x80(%rsp), %rdi
callq 0xd5e2e
movsd (%r15), %xmm0
movsd 0x8(%r15), %xmm1
movsd 0x10(%r15), %xmm2
leaq 0x30(%rsp), %rdi
callq 0xd5e2e
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x90(%rsp)
movaps 0x80(%rsp), %xmm1
movaps %xmm1, -0x10(%r14,%rbx)
movsd %xmm0, (%r14,%rbx)
addq $0x50, %rbx
cmpl $0x7e0, %ebx            # imm = 0x7E0
jne 0x3925e
movq 0x18(%rsp), %rdi
leaq 0x5880(%rsp), %rsi
movq 0x8(%rsp), %rdx
callq 0x4152e
xorl %esi, %esi
movq 0x28(%rsp), %rbx
movl 0x14(%rsp), %r14d
leaq 0x80(%rsp), %rcx
movq %rsi, 0x8(%rsp)
leaq 0x1880(%rsp), %rdi
leaq 0x119835(%rip), %rsi    # 0x152b3d
leaq 0x480(%rsp), %rdx
xorl %eax, %eax
movq %rcx, %r15
callq 0x27900
movl %eax, 0x24(%rsp)
cmpl $0x2, %eax
jne 0x39418
movq %r15, %rdi
leaq 0x115b47(%rip), %r15    # 0x14ee78
movq %r15, %rsi
callq 0x27f90
xorl %r14d, %r14d
testq %rax, %rax
je 0x393ae
xorl %edi, %edi
movq %r15, %rsi
callq 0x27f90
testq %rax, %rax
je 0x393ae
movq %rax, %rdi
callq 0x27f80
movl %eax, %r14d
xorl %edi, %edi
movq %r15, %rsi
callq 0x27f90
testq %rax, %rax
je 0x393ae
xorl %edi, %edi
movq %r15, %rsi
callq 0x27f90
jmp 0x393ae
movl $0x98, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
xorl %esi, %esi
callq 0x39944
movq %r13, 0x10(%r15)
movl 0x24(%r13), %edx
movl %edx, 0x18(%r15)
movq 0x68(%rsp), %rdi
movq %r15, %rsi
callq 0xfa862
movq %r13, %rdi
callq 0x38086
cmpl %r14d, 0x24(%r13)
jle 0x39376
movq 0x18(%r13), %rax
movslq %r14d, %rcx
movq (%rax,%rcx,8), %r14
cmpl %r12d, 0x2c(%r14)
jg 0x3943e
movl $0x80, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
callq 0x3f218
movq %r14, %rdi
movq %r15, %rsi
callq 0x3a29c
jmp 0x393bf
leaq 0x115a1c(%rip), %rdi    # 0x14ee0a
movl %ebp, %esi
leaq 0x880(%rsp), %rdx
movl %eax, %r14d
xorl %eax, %eax
callq 0xfd64a
movl %r14d, %eax
movl 0x14(%rsp), %r14d
cmpl $0x4, %eax
je 0x394ce
jmp 0x3962b
leaq 0x1159eb(%rip), %rdi    # 0x14ee0a
movl %ebp, %esi
leaq 0x880(%rsp), %rdx
xorl %eax, %eax
callq 0xfd64a
movq $0x0, 0x18(%rsp)
jmp 0x394be
movq 0x20(%r14), %rax
movq 0x50(%rsp), %rcx
movq (%rax,%rcx,8), %r15
cmpq $0x0, 0x18(%r15)
jne 0x3945f
movq 0x48(%rsp), %rdi
movq %r15, %rsi
callq 0x3efe4
movl $0x78, %edi
callq 0x27930
movq %rax, %r14
movq %rax, %rdi
callq 0x3f9e0
movq %r15, %rdi
movq %r14, %rsi
callq 0x3f62a
movl $0xc8, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
callq 0x40e50
movq 0x58(%rsp), %rdi
movl 0xc(%rdi), %edx
movq %r15, 0x18(%rsp)
movq %r15, %rsi
callq 0xfa862
movq 0x60(%rsp), %rdi
movl 0xc(%rdi), %edx
movq %r14, %rsi
callq 0xfa862
movl 0x14(%rsp), %r14d
cmpl $0x2, 0x24(%rsp)
movq 0x8(%rsp), %r15
jne 0x3962b
leaq 0x1880(%rsp), %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %rbx, %rdx
callq 0x27fd0
movl %r15d, %ecx
movq %rcx, 0x8(%rsp)
testq %rax, %rax
leaq 0x30(%rsp), %rdx
jne 0x390e9
jmp 0x39524
leaq 0x115946(%rip), %rdi    # 0x14ee48
leaq 0x880(%rsp), %rsi
xorl %eax, %eax
callq 0xfd64a
jmp 0x39617
jmp 0x39524
movq $0x0, 0x18(%rsp)
xorl %r15d, %r15d
testl %r14d, %r14d
je 0x3960a
cmpq $0x0, 0x18(%rsp)
je 0x3960a
movq %r15, 0x8(%rsp)
cmpl $0x1a, %r15d
jl 0x395f3
movq 0x8(%rsp), %rax
addl $-0x19, %eax
leaq (%rax,%rax,4), %rcx
shlq $0x4, %rcx
leaq (%rsp,%rcx), %r12
addq $0x5880, %r12           # imm = 0x5880
cltq
leaq (%rax,%rax,4), %rax
shlq $0x4, %rax
leaq (%rsp,%rax), %r13
addq $0x5880, %r13           # imm = 0x5880
movl $0x10, %ebp
leaq 0x80(%rsp), %r14
leaq 0x30(%rsp), %r15
movsd -0x10(%r13,%rbp), %xmm0
movsd -0x8(%r13,%rbp), %xmm1
movsd (%r13,%rbp), %xmm2
movq %r14, %rdi
callq 0xd5e2e
movsd (%r12), %xmm0
movsd 0x8(%r12), %xmm1
movsd 0x10(%r12), %xmm2
movq %r15, %rdi
callq 0xd5e2e
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x90(%rsp)
movaps 0x80(%rsp), %xmm1
movaps %xmm1, -0x10(%r13,%rbp)
movsd %xmm0, (%r13,%rbp)
addq $0x50, %rbp
cmpl $0x7e0, %ebp            # imm = 0x7E0
jne 0x3958a
leaq 0x5880(%rsp), %rsi
movq 0x18(%rsp), %rdi
movq 0x8(%rsp), %rdx
callq 0x4152e
movq %rbx, %rdi
callq 0x27920
movl $0x1, %ebp
movl %ebp, %eax
addq $0x19888, %rsp          # imm = 0x19888
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
xorl %ebp, %ebp
jmp 0x39617
movq %rax, %rbx
movl $0xc8, %esi
jmp 0x39662
movq %rax, %rbx
movl $0x78, %esi
movq %r14, %rdi
jmp 0x39665
movq %rax, %rbx
movl $0x80, %esi
jmp 0x39662
movq %rax, %rbx
movl $0x98, %esi
jmp 0x39662
movq %rax, %rbx
movl $0x78, %esi
movq %r15, %rdi
callq 0x27950
movq %rbx, %rdi
callq 0x28160