// File: tomfunkhouser[P]gaps/apps/conf2sfl/conf2sfl.cpp
// Function: LoadSurfels(gaps::R3SurfelScene*, char const*, gaps::R3Segmentation*, gaps::R2Grid const&, gaps::R3Point const&, gaps::R3Vector const&, gaps::R3Vector const&, gaps::R3Matrix const&)
static int
LoadSurfels(R3SurfelScene *scene, 
  const char *scan_name, R3Segmentation *segmentation, const R2Grid& depth_image, 
  const R3Point& viewpoint, const R3Vector& towards, const R3Vector& up,
  const R3Matrix& intrinsics)
{
  // Get convenient variables
  R3SurfelTree *tree = scene->Tree();
  R3SurfelObject *parent_object = scene->RootObject();
  R3SurfelNode *parent_node = tree->RootNode();
  R2Point center(intrinsics[0][2], intrinsics[1][2]);
  RNLength xfocal = intrinsics[0][0];
  RNLength yfocal = intrinsics[1][1];
  int width = depth_image.XResolution();
  int height = depth_image.YResolution();
  
  // Create array of points outside any cluster
  RNArray<R3SegmentationPoint *> unclustered_points;
  for (int i = 0; i < segmentation->points.NEntries(); i++) {
    R3SegmentationPoint *point = segmentation->points.Kth(i);
    if (!point->cluster) unclustered_points.Insert(point);
  }

  // Create node
  char node_name[1024];
  sprintf(node_name, "SCAN:%s", scan_name);
  R3SurfelNode *scan_node = new R3SurfelNode(node_name);
  if (!scan_node) {
    RNFail("Unable to allocate node for %s\n", scan_name);
    return 0;
  }
            
  // Insert node into tree
  tree->InsertNode(scan_node, parent_node);

  // Load surfels for points inside every cluster
  for (int i = 0; i < segmentation->clusters.NEntries(); i++) {
    R3SegmentationCluster *cluster = segmentation->clusters.Kth(i);
    if (cluster->points.NEntries() == 0) continue;
    
    // Create node name
    char node_name[1024];
    sprintf(node_name, "SCAN:%s:%d", scan_name, i);

    // Load surfels
    LoadSurfels(scene, cluster->points, parent_object, scan_node, node_name, depth_image, viewpoint);
  }

  // Load surfels for points outside any cluster
  if (unclustered_points.NEntries() > 0) {
    // Create node name
    char node_name[1024];
    sprintf(node_name, "SCAN:%s:unclustered", scan_name);

    // Load surfels
    LoadSurfels(scene, unclustered_points, NULL, scan_node, node_name, depth_image, viewpoint);
  }
            
  // Update node properties
  scan_node->UpdateProperties();

  // Create scan
  R3SurfelScan *scan = new R3SurfelScan(scan_name);
  if (!scan) {
    RNFail("Unable to allocate scan\n");
    return 0;
  }

  // Assign scan properties
  scan->SetViewpoint(viewpoint);
  scan->SetOrientation(towards, up);
  scan->SetImageDimensions(width, height);
  scan->SetImageCenter(center);
  scan->SetXFocal(xfocal);
  scan->SetYFocal(yfocal);
  scan->SetNode(scan_node);
          
  // Insert scan
  scene->InsertScan(scan);

  // Create image
  R3SurfelImage *image = new R3SurfelImage(scan_name);
  if (!image) {
    RNFail("Unable to allocate image\n");
    return 0;
  }

  // Assign image properties
  image->SetViewpoint(viewpoint);
  image->SetOrientation(towards, up);
  image->SetImageDimensions(width, height);
  image->SetImageCenter(center);
  image->SetXFocal(xfocal);
  image->SetYFocal(yfocal);
  image->SetScan(scan);
          
  // Insert image
  scene->InsertImage(image);

  // Return success
  return 1;
}

// Assembly:
subq $0xd38, %rsp            # imm = 0xD38
movq 0xd48(%rsp), %rax
movq 0xd40(%rsp), %rax
movq %rdi, 0xd28(%rsp)
movq %rsi, 0xd20(%rsp)
movq %rdx, 0xd18(%rsp)
movq %rcx, 0xd10(%rsp)
movq %r8, 0xd08(%rsp)
movq %r9, 0xd00(%rsp)
movq 0xd28(%rsp), %rdi
callq 0x1edc0
movq %rax, 0xcf8(%rsp)
movq 0xd28(%rsp), %rdi
callq 0x20200
movq %rax, 0xcf0(%rsp)
movq 0xcf8(%rsp), %rdi
callq 0x20220
movq %rax, 0xce8(%rsp)
movq 0xd48(%rsp), %rdi
xorl %esi, %esi
callq 0x20240
movsd 0x10(%rax), %xmm0
movsd %xmm0, 0x58(%rsp)
movq 0xd48(%rsp), %rdi
movl $0x1, %esi
callq 0x20240
movsd 0x58(%rsp), %xmm0
movsd 0x10(%rax), %xmm1
leaq 0xcd8(%rsp), %rdi
callq 0x167da0
movq 0xd48(%rsp), %rdi
xorl %esi, %esi
callq 0x20240
movsd (%rax), %xmm0
movsd %xmm0, 0xcd0(%rsp)
movq 0xd48(%rsp), %rdi
movl $0x1, %esi
callq 0x20240
movsd 0x8(%rax), %xmm0
movsd %xmm0, 0xcc8(%rsp)
movq 0xd10(%rsp), %rdi
callq 0x1f1a0
movl %eax, 0xcc4(%rsp)
movq 0xd10(%rsp), %rdi
callq 0x1f1c0
movl %eax, 0xcc0(%rsp)
leaq 0xcb0(%rsp), %rdi
callq 0x20260
movl $0x0, 0xcac(%rsp)
movl 0xcac(%rsp), %eax
movl %eax, 0x54(%rsp)
movq 0xd18(%rsp), %rdi
callq 0x1ecf0
movl %eax, %ecx
movl 0x54(%rsp), %eax
cmpl %ecx, %eax
jge 0x1bfa0
movq 0xd18(%rsp), %rdi
movl 0xcac(%rsp), %esi
callq 0x1f6a0
movq %rax, 0xca0(%rsp)
movq 0xca0(%rsp), %rax
cmpq $0x0, 0xc0(%rax)
jne 0x1bf88
movq 0xca0(%rsp), %rsi
leaq 0xcb0(%rsp), %rdi
callq 0x1f3d0
jmp 0x1bf6d
jmp 0x1bf88
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xc98(%rsp)
movl %eax, 0xc94(%rsp)
jmp 0x1c4ef
jmp 0x1bf8a
movl 0xcac(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xcac(%rsp)
jmp 0x1bf06
movq 0xd20(%rsp), %rdx
leaq 0x1e14f7(%rip), %rsi    # 0x1fd4a6
xorl %eax, %eax
leaq 0x890(%rsp), %rdi
callq 0x16130
movl $0xd0, %edi
callq 0x16740
movq %rax, 0x48(%rsp)
jmp 0x1bfcf
movq 0x48(%rsp), %rdi
leaq 0x890(%rsp), %rsi
callq 0x5acf0
jmp 0x1bfe3
movq 0x48(%rsp), %rax
movq %rax, 0x888(%rsp)
cmpq $0x0, 0x888(%rsp)
jne 0x1c056
movq 0xd20(%rsp), %rsi
leaq 0x1e14a4(%rip), %rdi    # 0x1fd4ae
xorl %eax, %eax
callq 0x1779b0
jmp 0x1c013
movl $0x0, 0xd34(%rsp)
movl $0x1, 0x884(%rsp)
jmp 0x1c4d3
movq 0x48(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xc98(%rsp)
movl %eax, 0xc94(%rsp)
movl $0xd0, %esi
callq 0x16750
jmp 0x1c4ef
movq 0xcf8(%rsp), %rdi
movq 0x888(%rsp), %rsi
movq 0xce8(%rsp), %rdx
callq 0x5d9f0
jmp 0x1c075
movl $0x0, 0x880(%rsp)
movl 0x880(%rsp), %eax
movl %eax, 0x44(%rsp)
movq 0xd18(%rsp), %rdi
addq $0x18, %rdi
callq 0x1ecf0
movl %eax, %ecx
movl 0x44(%rsp), %eax
cmpl %ecx, %eax
jge 0x1c176
movq 0xd18(%rsp), %rdi
addq $0x18, %rdi
movl 0x880(%rsp), %esi
callq 0x20270
movq %rax, 0x38(%rsp)
jmp 0x1c0c9
movq 0x38(%rsp), %rax
movq %rax, 0x878(%rsp)
movq 0x878(%rsp), %rdi
addq $0x8, %rdi
callq 0x1ecf0
cmpl $0x0, %eax
jne 0x1c0ee
jmp 0x1c160
movq 0xd20(%rsp), %rdx
movl 0x880(%rsp), %ecx
leaq 0x1e13ca(%rip), %rsi    # 0x1fd4ce
xorl %eax, %eax
leaq 0x470(%rsp), %rdi
movq %rdi, 0x30(%rsp)
callq 0x16130
movq 0x30(%rsp), %r8
movq 0xd28(%rsp), %rdi
movq 0x878(%rsp), %rsi
addq $0x8, %rsi
movq 0xcf0(%rsp), %rdx
movq 0x888(%rsp), %rcx
movq 0xd10(%rsp), %r9
movq 0xd08(%rsp), %r10
movq %rsp, %rax
movq %r10, (%rax)
callq 0x1e0c0
jmp 0x1c15e
jmp 0x1c160
movl 0x880(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x880(%rsp)
jmp 0x1c080
leaq 0xcb0(%rsp), %rdi
callq 0x1ecf0
cmpl $0x0, %eax
jle 0x1c1ee
movq 0xd20(%rsp), %rdx
leaq 0x1e1342(%rip), %rsi    # 0x1fd4d9
xorl %eax, %eax
movl %eax, 0x2c(%rsp)
leaq 0x70(%rsp), %rdi
movq %rdi, 0x20(%rsp)
callq 0x16130
movq 0x20(%rsp), %r8
movl 0x2c(%rsp), %eax
movq 0xd28(%rsp), %rdi
movq 0x888(%rsp), %rcx
movq 0xd10(%rsp), %r9
movq 0xd08(%rsp), %rsi
movq %rsp, %rdx
movq %rsi, (%rdx)
movl %eax, %edx
leaq 0xcb0(%rsp), %rsi
callq 0x1e0c0
jmp 0x1c1ec
jmp 0x1c1ee
movq 0x888(%rsp), %rdi
callq 0x5cc70
jmp 0x1c1fd
movl $0xd0, %edi
callq 0x16740
movq %rax, 0x18(%rsp)
jmp 0x1c20e
movq 0x18(%rsp), %rdi
movq 0xd20(%rsp), %rsi
callq 0x61d80
jmp 0x1c222
movq 0x18(%rsp), %rax
movq %rax, 0x68(%rsp)
cmpq $0x0, 0x68(%rsp)
jne 0x1c287
leaq 0x1e12b2(%rip), %rdi    # 0x1fd4ed
xorl %eax, %eax
callq 0x1779b0
jmp 0x1c244
movl $0x0, 0xd34(%rsp)
movl $0x1, 0x884(%rsp)
jmp 0x1c4d3
movq 0x18(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xc98(%rsp)
movl %eax, 0xc94(%rsp)
movl $0xd0, %esi
callq 0x16750
jmp 0x1c4ef
movq 0x68(%rsp), %rdi
movq 0xd08(%rsp), %rsi
movq (%rdi), %rax
movq 0x18(%rax), %rax
callq *%rax
jmp 0x1c29f
movq 0x68(%rsp), %rdi
movq 0xd00(%rsp), %rsi
movq 0xd40(%rsp), %rdx
movq (%rdi), %rax
movq 0x20(%rax), %rax
callq *%rax
jmp 0x1c2bf
movq 0x68(%rsp), %rdi
movl 0xcc4(%rsp), %esi
movl 0xcc0(%rsp), %edx
movq (%rdi), %rax
movq 0x38(%rax), %rax
callq *%rax
jmp 0x1c2dd
movq 0x68(%rsp), %rdi
movq (%rdi), %rax
movq 0x40(%rax), %rax
leaq 0xcd8(%rsp), %rsi
callq *%rax
jmp 0x1c2f5
movq 0x68(%rsp), %rdi
movsd 0xcd0(%rsp), %xmm0
movq (%rdi), %rax
movq 0x50(%rax), %rax
callq *%rax
jmp 0x1c30e
movq 0x68(%rsp), %rdi
movsd 0xcc8(%rsp), %xmm0
movq (%rdi), %rax
movq 0x58(%rax), %rax
callq *%rax
jmp 0x1c327
movq 0x68(%rsp), %rdi
movq 0x888(%rsp), %rsi
movq (%rdi), %rax
movq 0x78(%rax), %rax
callq *%rax
jmp 0x1c33f
movq 0xd28(%rsp), %rdi
movq 0x68(%rsp), %rsi
movq (%rdi), %rax
movq 0x90(%rax), %rax
callq *%rax
jmp 0x1c35a
movl $0x1d8, %edi            # imm = 0x1D8
callq 0x16740
movq %rax, 0x10(%rsp)
jmp 0x1c36b
movq 0x10(%rsp), %rdi
movq 0xd20(%rsp), %rsi
callq 0x62b40
jmp 0x1c37f
movq 0x10(%rsp), %rax
movq %rax, 0x60(%rsp)
cmpq $0x0, 0x60(%rsp)
jne 0x1c3e4
leaq 0x1e116e(%rip), %rdi    # 0x1fd506
xorl %eax, %eax
callq 0x1779b0
jmp 0x1c3a1
movl $0x0, 0xd34(%rsp)
movl $0x1, 0x884(%rsp)
jmp 0x1c4d3
movq 0x10(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xc98(%rsp)
movl %eax, 0xc94(%rsp)
movl $0x1d8, %esi            # imm = 0x1D8
callq 0x16750
jmp 0x1c4ef
movq 0x60(%rsp), %rdi
movq 0xd08(%rsp), %rsi
movq (%rdi), %rax
movq 0x60(%rax), %rax
callq *%rax
jmp 0x1c3fc
movq 0x60(%rsp), %rdi
movq 0xd00(%rsp), %rsi
movq 0xd40(%rsp), %rdx
movq (%rdi), %rax
movq 0x68(%rax), %rax
callq *%rax
jmp 0x1c41c
movq 0x60(%rsp), %rdi
movl 0xcc4(%rsp), %esi
movl 0xcc0(%rsp), %edx
movq (%rdi), %rax
movq 0x70(%rax), %rax
callq *%rax
jmp 0x1c43a
movq 0x60(%rsp), %rdi
movq (%rdi), %rax
movq 0x78(%rax), %rax
leaq 0xcd8(%rsp), %rsi
callq *%rax
jmp 0x1c452
movq 0x60(%rsp), %rdi
movsd 0xcd0(%rsp), %xmm0
movq (%rdi), %rax
movq 0x88(%rax), %rax
callq *%rax
jmp 0x1c46e
movq 0x60(%rsp), %rdi
movsd 0xcc8(%rsp), %xmm0
movq (%rdi), %rax
movq 0x90(%rax), %rax
callq *%rax
jmp 0x1c48a
movq 0x60(%rsp), %rdi
movq 0x68(%rsp), %rsi
movq (%rdi), %rax
movq 0xd0(%rax), %rax
callq *%rax
jmp 0x1c4a2
movq 0xd28(%rsp), %rdi
movq 0x60(%rsp), %rsi
movq (%rdi), %rax
movq 0xa0(%rax), %rax
callq *%rax
jmp 0x1c4bd
movl $0x1, 0xd34(%rsp)
movl $0x1, 0x884(%rsp)
leaq 0xcb0(%rsp), %rdi
callq 0x1f730
movl 0xd34(%rsp), %eax
addq $0xd38, %rsp            # imm = 0xD38
retq
leaq 0xcb0(%rsp), %rdi
callq 0x1f730
movq 0xc98(%rsp), %rdi
callq 0x16ca0
nopl (%rax)