// File: 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:
subq $0x199c8, %rsp          # imm = 0x199C8
movq %rdi, 0x199b8(%rsp)
movl %esi, 0x199b4(%rsp)
movq %rdx, 0x199a8(%rsp)
movq %rcx, 0x199a0(%rsp)
movl %r8d, 0x1999c(%rsp)
movq 0x199b8(%rsp), %rax
movq %rax, 0x68(%rsp)
leaq 0x18990(%rsp), %rcx
movq %rcx, 0x70(%rsp)
movq 0x8(%rax), %rdi
callq 0x47cf0
movq 0x68(%rsp), %rdi
movq %rax, 0x78(%rsp)
callq 0x44310
movq 0x70(%rsp), %rdi
movq 0x78(%rsp), %rdx
movq %rax, %rcx
movl 0x199b4(%rsp), %r8d
addl $0x1, %r8d
leaq 0x220029(%rip), %rsi    # 0x266f29
movb $0x0, %al
callq 0x2a280
leaq 0x18990(%rsp), %rdi
leaq 0x23c4dd(%rip), %rsi    # 0x2833f3
callq 0x2a520
movq %rax, 0x18988(%rsp)
cmpq $0x0, 0x18988(%rsp)
jne 0x46f54
leaq 0x18990(%rsp), %rsi
leaq 0x22000b(%rip), %rdi    # 0x266f48
movb $0x0, %al
callq 0x1cbff0
movl $0x0, 0x199c4(%rsp)
jmp 0x477ba
movq $0x0, 0x18980(%rsp)
movq 0x68(%rsp), %rdi
callq 0x44340
cmpl 0x199b4(%rsp), %eax
jg 0x46fcd
movl $0x78, %edi
callq 0x292e0
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x60(%rsp)
movl $0x2, %esi
callq 0x52880
jmp 0x46f94
movq 0x60(%rsp), %rsi
movq 0x68(%rsp), %rdi
callq 0x45aa0
jmp 0x46f60
movq 0x60(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x18978(%rsp)
movl %eax, 0x18974(%rsp)
movl $0x78, %esi
callq 0x291f0
jmp 0x477c9
movq 0x68(%rsp), %rdi
movl 0x199b4(%rsp), %esi
callq 0x44780
movq %rax, 0x18980(%rsp)
movl $0x0, 0x18970(%rsp)
movl $0x4000, 0x1896c(%rsp) # imm = 0x4000
movq $0x0, 0x14558(%rsp)
movl $0x0, 0x14554(%rsp)
movl $0x400, 0x14550(%rsp) # imm = 0x400
leaq 0x550(%rsp), %rax
movq %rax, %rcx
addq $0x14000, %rcx          # imm = 0x14000
movq %rcx, 0x50(%rsp)
movq %rax, 0x58(%rsp)
movq 0x58(%rsp), %rdi
movq %rdi, 0x48(%rsp)
callq 0x5ee80
movq 0x48(%rsp), %rax
movq 0x50(%rsp), %rcx
addq $0x50, %rax
cmpq %rcx, %rax
movq %rax, 0x58(%rsp)
jne 0x4703a
jmp 0x47063
leaq 0x14960(%rsp), %rdi
movq 0x18988(%rsp), %rdx
movl $0x4000, %esi           # imm = 0x4000
callq 0x29bd0
cmpq $0x0, %rax
je 0x4765f
movl 0x18970(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x18970(%rsp)
movsbl 0x14960(%rsp), %eax
cmpl $0x0, %eax
jne 0x470a7
jmp 0x47063
movsbl 0x14960(%rsp), %eax
cmpl $0x23, %eax
jne 0x470b6
jmp 0x47063
movsbl 0x14960(%rsp), %eax
cmpl $0x76, %eax
jne 0x471b5
leaq 0x14960(%rsp), %rdi
leaq 0x14560(%rsp), %rdx
leaq 0x21fe8e(%rip), %rsi    # 0x266f6c
leaq 0x548(%rsp), %rcx
leaq 0x540(%rsp), %r8
leaq 0x538(%rsp), %r9
movb $0x0, %al
callq 0x29150
cmpl $0x4, %eax
je 0x4712f
movl 0x18970(%rsp), %esi
leaq 0x18990(%rsp), %rdx
leaq 0x21fdf2(%rip), %rdi    # 0x266f0a
movb $0x0, %al
callq 0x1cbff0
movl $0x0, 0x199c4(%rsp)
jmp 0x477ba
cmpq $0x0, 0x14558(%rsp)
je 0x471b0
cmpl $0x400, 0x14554(%rsp) # imm = 0x400
jge 0x471ae
movslq 0x14554(%rsp), %rcx
leaq 0x550(%rsp), %rax
imulq $0x50, %rcx, %rcx
addq %rcx, %rax
movq %rax, 0x40(%rsp)
movsd 0x548(%rsp), %xmm0
movsd 0x540(%rsp), %xmm1
movsd 0x538(%rsp), %xmm2
leaq 0x520(%rsp), %rdi
callq 0x17fd90
movq 0x40(%rsp), %rdi
leaq 0x520(%rsp), %rsi
callq 0x44e10
movl 0x14554(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x14554(%rsp)
jmp 0x471b0
jmp 0x4765a
movsbl 0x14960(%rsp), %eax
cmpl $0x67, %eax
jne 0x47658
cmpq $0x0, 0x14558(%rsp)
je 0x47309
cmpl $0x0, 0x1999c(%rsp)
je 0x472f2
cmpl $0x19, 0x14554(%rsp)
jle 0x472d6
movl 0x14554(%rsp), %eax
subl $0x19, %eax
movl %eax, 0x51c(%rsp)
movl 0x51c(%rsp), %eax
cmpl 0x14554(%rsp), %eax
jge 0x472d4
movslq 0x51c(%rsp), %rax
leaq 0x550(%rsp), %rsi
imulq $0x50, %rax, %rax
addq %rax, %rsi
leaq 0x500(%rsp), %rdi
callq 0x3a200
movl 0x14554(%rsp), %eax
subl $0x19, %eax
cltq
leaq 0x550(%rsp), %rsi
imulq $0x50, %rax, %rax
addq %rax, %rsi
leaq 0x4e8(%rsp), %rdi
callq 0x3a200
leaq 0x4e8(%rsp), %rdi
movl $0x2, %esi
callq 0x44530
movsd (%rax), %xmm0
movsd %xmm0, 0x38(%rsp)
leaq 0x500(%rsp), %rdi
movl $0x2, %esi
callq 0x44530
movsd 0x38(%rsp), %xmm0
movsd %xmm0, (%rax)
movslq 0x51c(%rsp), %rax
leaq 0x550(%rsp), %rdi
imulq $0x50, %rax, %rax
addq %rax, %rdi
leaq 0x500(%rsp), %rsi
callq 0x44e10
movl 0x51c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x51c(%rsp)
jmp 0x47202
jmp 0x472d6
movq 0x14558(%rsp), %rdi
leaq 0x550(%rsp), %rsi
movl 0x14554(%rsp), %edx
callq 0x574f0
movl $0x0, 0x14554(%rsp)
movq $0x0, 0x14558(%rsp)
leaq 0x14960(%rsp), %rdi
leaq 0x14560(%rsp), %rdx
leaq 0xe0(%rsp), %rcx
leaq 0x223ab4(%rip), %rsi    # 0x26addc
movb $0x0, %al
callq 0x29150
cmpl $0x2, %eax
je 0x47361
movl 0x18970(%rsp), %esi
leaq 0x18990(%rsp), %rdx
leaq 0x21fbc0(%rip), %rdi    # 0x266f0a
movb $0x0, %al
callq 0x1cbff0
movl $0x0, 0x199c4(%rsp)
jmp 0x477ba
movl $0x0, 0xdc(%rsp)
leaq 0xe0(%rsp), %rdi
leaq 0x21fbfd(%rip), %rsi    # 0x266f78
callq 0x29520
movq %rax, 0xd0(%rsp)
cmpq $0x0, 0xd0(%rsp)
je 0x4741c
xorl %eax, %eax
movl %eax, %edi
leaq 0x21fbd6(%rip), %rsi    # 0x266f78
callq 0x29520
movq %rax, 0xd0(%rsp)
cmpq $0x0, 0xd0(%rsp)
je 0x4741a
movq 0xd0(%rsp), %rdi
callq 0x29610
movl %eax, 0xdc(%rsp)
xorl %eax, %eax
movl %eax, %edi
leaq 0x21fb9f(%rip), %rsi    # 0x266f78
callq 0x29520
movq %rax, 0xd0(%rsp)
cmpq $0x0, 0xd0(%rsp)
je 0x47418
xorl %eax, %eax
movl %eax, %edi
leaq 0x21fb7c(%rip), %rsi    # 0x266f78
callq 0x29520
movq %rax, 0xd0(%rsp)
cmpq $0x0, 0xd0(%rsp)
je 0x47416
jmp 0x47416
jmp 0x47418
jmp 0x4741a
jmp 0x4741c
movq $0x0, 0xc8(%rsp)
movq 0x68(%rsp), %rdi
callq 0x39fe0
cmpl 0xdc(%rsp), %eax
jg 0x47494
movl $0x98, %edi
callq 0x292e0
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x30(%rsp)
xorl %eax, %eax
movl %eax, %esi
callq 0x47df0
jmp 0x4745b
movq 0x30(%rsp), %rsi
movq 0x68(%rsp), %rdi
callq 0x457b0
jmp 0x47428
movq 0x30(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x18978(%rsp)
movl %eax, 0x18974(%rsp)
movl $0x98, %esi
callq 0x291f0
jmp 0x477c9
movq 0x68(%rsp), %rdi
movl 0xdc(%rsp), %esi
callq 0x3a000
movq %rax, 0xc8(%rsp)
movq $0x0, 0xc0(%rsp)
movq 0xc8(%rsp), %rdi
callq 0x3a030
cmpl 0x199b4(%rsp), %eax
jg 0x47531
movq 0xc8(%rsp), %rax
movq %rax, 0x20(%rsp)
movl $0x80, %edi
callq 0x292e0
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x28(%rsp)
callq 0x52fc0
jmp 0x474f8
movq 0x28(%rsp), %rsi
movq 0x20(%rsp), %rdi
callq 0x48e60
jmp 0x474b9
movq 0x28(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x18978(%rsp)
movl %eax, 0x18974(%rsp)
movl $0x80, %esi
callq 0x291f0
jmp 0x477c9
movq 0xc8(%rsp), %rdi
movl 0x199b4(%rsp), %esi
callq 0x3a050
movq %rax, 0xc0(%rsp)
movq 0xc0(%rsp), %rdi
callq 0x44bf0
cmpq $0x0, %rax
jne 0x47575
movq 0x18980(%rsp), %rdi
movq 0xc0(%rsp), %rsi
callq 0x52bf0
movl $0x78, %edi
callq 0x292e0
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x18(%rsp)
callq 0x54060
jmp 0x47591
movq 0x18(%rsp), %rax
movq %rax, 0xb8(%rsp)
movq 0xc0(%rsp), %rdi
movq 0xb8(%rsp), %rsi
callq 0x53950
movl $0xc8, %edi
callq 0x292e0
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x10(%rsp)
callq 0x56820
jmp 0x475cf
movq 0x10(%rsp), %rax
movq %rax, 0x14558(%rsp)
movq 0x199a8(%rsp), %rdi
movq 0x14558(%rsp), %rsi
callq 0x47d90
movq 0x199a0(%rsp), %rdi
movq 0xb8(%rsp), %rsi
callq 0x47dc0
jmp 0x47658
movq 0x18(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x18978(%rsp)
movl %eax, 0x18974(%rsp)
movl $0x78, %esi
callq 0x291f0
jmp 0x477c9
movq 0x10(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x18978(%rsp)
movl %eax, 0x18974(%rsp)
movl $0xc8, %esi
callq 0x291f0
jmp 0x477c9
jmp 0x4765a
jmp 0x47063
cmpq $0x0, 0x14558(%rsp)
je 0x477a2
cmpl $0x0, 0x1999c(%rsp)
je 0x4778b
cmpl $0x19, 0x14554(%rsp)
jle 0x4776f
movl 0x14554(%rsp), %eax
subl $0x19, %eax
movl %eax, 0xb4(%rsp)
movl 0xb4(%rsp), %eax
cmpl 0x14554(%rsp), %eax
jge 0x4776d
movslq 0xb4(%rsp), %rax
leaq 0x550(%rsp), %rsi
imulq $0x50, %rax, %rax
addq %rax, %rsi
leaq 0x98(%rsp), %rdi
callq 0x3a200
movl 0x14554(%rsp), %eax
subl $0x19, %eax
cltq
leaq 0x550(%rsp), %rsi
imulq $0x50, %rax, %rax
addq %rax, %rsi
leaq 0x80(%rsp), %rdi
callq 0x3a200
leaq 0x80(%rsp), %rdi
movl $0x2, %esi
callq 0x44530
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x98(%rsp), %rdi
movl $0x2, %esi
callq 0x44530
movsd 0x8(%rsp), %xmm0
movsd %xmm0, (%rax)
movslq 0xb4(%rsp), %rax
leaq 0x550(%rsp), %rdi
imulq $0x50, %rax, %rax
addq %rax, %rdi
leaq 0x98(%rsp), %rsi
callq 0x44e10
movl 0xb4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xb4(%rsp)
jmp 0x4769b
jmp 0x4776f
movq 0x14558(%rsp), %rdi
leaq 0x550(%rsp), %rsi
movl 0x14554(%rsp), %edx
callq 0x574f0
movl $0x0, 0x14554(%rsp)
movq $0x0, 0x14558(%rsp)
movq 0x18988(%rsp), %rdi
callq 0x297e0
movl $0x1, 0x199c4(%rsp)
movl 0x199c4(%rsp), %eax
addq $0x199c8, %rsp          # imm = 0x199C8
retq
movq 0x18978(%rsp), %rdi
callq 0x29c10
nopw %cs:(%rax,%rax)