// File: tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelUtils.cpp
// Function: gaps::EstimateColor(gaps::R3SurfelScene const*, gaps::R3Point const&, gaps::R3Vector const&, double)
RNRgb
EstimateColor(const R3SurfelScene *scene,
  const R3Point& position, const R3Vector& normal,
  double timestamp)
{
  // Initialize color
  RNRgb total_color(0,0,0);
  double total_weight = 0;
 
  // Check all images
  for (int i = 0; i < scene->NImages(); i++) {
    R3SurfelImage *image = scene->Image(i);

    // Get/check color channels
    const R2Grid *red_channel = image->Channel(R3_SURFEL_RED_CHANNEL);
    const R2Grid *green_channel = image->Channel(R3_SURFEL_GREEN_CHANNEL);
    const R2Grid *blue_channel = image->Channel(R3_SURFEL_BLUE_CHANNEL);
    if (!red_channel || !green_channel || !blue_channel) continue;

    // Get useful variables
    R3Point viewpoint = image->Viewpoint();
    R3Vector towards = image->Towards();
    R3Vector vector = position - viewpoint;

    // Compute/check depth
    double depth = vector.Dot(towards);
    if (depth <= 1E-3) continue;
    vector.Normalize();

    // Compute/check N dot V
    double angle_weight = 1.0;
    if (!normal.IsZero()) {
      double ndotv = normal.Dot(-vector);
      if (ndotv <= 0) continue;
      angle_weight = ndotv;
    }

    // Compute/check timestamp difference
    double timestamp_weight = 1.0;
    if (timestamp > 0) {
      double image_timestamp = image->Timestamp();
      double timestamp_difference = fabs(image_timestamp - timestamp);
      if (timestamp_difference < 1E-3) timestamp_difference = 1E-3;
      timestamp_weight = 1.0 / timestamp_difference;
    }
   
    // Project 3D world position to 2D pixels
    R2Point image_position = image->ImagePosition(position);
    if (!image->ContainsImagePosition(image_position)) continue;

    // Check depth consistency
    const R2Grid *depth_channel = image->Channel(R3_SURFEL_DEPTH_CHANNEL);
    if (depth_channel) {
      RNScalar max_depth_difference_fraction = 0.1;
      RNScalar image_depth = image->PixelDepth(image_position.X(), image_position.Y());
      if (RNIsNegativeOrZero(image_depth)) continue;
      if (fabs(image_depth - depth) / depth > max_depth_difference_fraction) continue;
    }
    
    // Compute weight
    double depth_weight = 1.0 / depth;
    double weight = timestamp_weight * angle_weight * depth_weight;
    if (weight <= 0) continue;
   
    // Get color at the 2D pixel
    double r = red_channel->GridValue(image_position);
    double g = green_channel->GridValue(image_position);
    double b = blue_channel->GridValue(image_position);
    RNRgb color(r, g, b);
   
    // Add to weighted sums
    total_color += weight * color;
    total_weight += weight;
  }

  // Return weighted average color
  return (total_weight > 0) ? total_color / total_weight : RNgray_rgb;
}

// Assembly:
subq $0x1b8, %rsp            # imm = 0x1B8
movq %rdi, 0x28(%rsp)
movq %rdi, %rax
movq %rax, 0x30(%rsp)
movq %rdi, 0x1b0(%rsp)
movq %rsi, 0x1a8(%rsp)
movq %rdx, 0x1a0(%rsp)
movq %rcx, 0x198(%rsp)
movsd %xmm0, 0x190(%rsp)
leaq 0x178(%rsp), %rdi
xorps %xmm2, %xmm2
movaps %xmm2, %xmm0
movaps %xmm2, %xmm1
callq 0x141cf0
xorps %xmm0, %xmm0
movsd %xmm0, 0x170(%rsp)
movl $0x0, 0x16c(%rsp)
movl 0x16c(%rsp), %eax
movl %eax, 0x24(%rsp)
movq 0x1a8(%rsp), %rdi
callq 0x16cc0
movl %eax, %ecx
movl 0x24(%rsp), %eax
cmpl %ecx, %eax
jge 0x39305
movq 0x1a8(%rsp), %rdi
movl 0x16c(%rsp), %esi
callq 0x30800
movq %rax, 0x160(%rsp)
movq 0x160(%rsp), %rdi
xorl %esi, %esi
callq 0x43b30
movq %rax, 0x158(%rsp)
movq 0x160(%rsp), %rdi
movl $0x1, %esi
callq 0x43b30
movq %rax, 0x150(%rsp)
movq 0x160(%rsp), %rdi
movl $0x2, %esi
callq 0x43b30
movq %rax, 0x148(%rsp)
cmpq $0x0, 0x158(%rsp)
je 0x38f18
cmpq $0x0, 0x150(%rsp)
je 0x38f18
cmpq $0x0, 0x148(%rsp)
jne 0x38f1d
jmp 0x392ef
movq 0x160(%rsp), %rdi
callq 0x30940
movq %rax, %rsi
leaq 0x130(%rsp), %rdi
callq 0xf8390
movq 0x160(%rsp), %rsi
leaq 0x118(%rsp), %rdi
callq 0x31ca0
movq 0x1a0(%rsp), %rsi
leaq 0x100(%rsp), %rdi
leaq 0x130(%rsp), %rdx
callq 0xf86b0
leaq 0x100(%rsp), %rdi
leaq 0x118(%rsp), %rsi
callq 0xf9ac0
movsd %xmm0, 0xf8(%rsp)
movsd 0x16b41e(%rip), %xmm0   # 0x1a43b0
ucomisd 0xf8(%rsp), %xmm0
jb 0x38fa2
jmp 0x392ef
leaq 0x100(%rsp), %rdi
callq 0xf9c10
movsd 0x16b1b1(%rip), %xmm0   # 0x1a4168
movsd %xmm0, 0xf0(%rsp)
movq 0x198(%rsp), %rdi
callq 0x43bb0
cmpl $0x0, %eax
jne 0x39034
movq 0x198(%rsp), %rax
movq %rax, 0x18(%rsp)
leaq 0xd0(%rsp), %rdi
leaq 0x100(%rsp), %rsi
callq 0xfa7a0
movq 0x18(%rsp), %rdi
leaq 0xd0(%rsp), %rsi
callq 0xf9ac0
movsd %xmm0, 0xe8(%rsp)
xorps %xmm0, %xmm0
ucomisd 0xe8(%rsp), %xmm0
jb 0x39022
jmp 0x392ef
movsd 0xe8(%rsp), %xmm0
movsd %xmm0, 0xf0(%rsp)
movsd 0x16b12c(%rip), %xmm0   # 0x1a4168
movsd %xmm0, 0xc8(%rsp)
movsd 0x190(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x390d1
movq 0x160(%rsp), %rdi
callq 0x31d20
movsd %xmm0, 0xc0(%rsp)
movsd 0xc0(%rsp), %xmm0
subsd 0x190(%rsp), %xmm0
movaps 0x16b33a(%rip), %xmm1   # 0x1a43c0
pand %xmm1, %xmm0
movsd %xmm0, 0xb8(%rsp)
movsd 0x16b315(%rip), %xmm0   # 0x1a43b0
ucomisd 0xb8(%rsp), %xmm0
jbe 0x390b7
movsd 0x16b302(%rip), %xmm0   # 0x1a43b0
movsd %xmm0, 0xb8(%rsp)
movsd 0x16b0a9(%rip), %xmm0   # 0x1a4168
divsd 0xb8(%rsp), %xmm0
movsd %xmm0, 0xc8(%rsp)
movq 0x160(%rsp), %rsi
movq 0x1a0(%rsp), %rdx
leaq 0xa8(%rsp), %rdi
callq 0x43c20
movq 0x160(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x6a480
cmpl $0x0, %eax
jne 0x3910d
jmp 0x392ef
movq 0x160(%rsp), %rdi
movl $0x3, %esi
callq 0x43b30
movq %rax, 0xa0(%rsp)
cmpq $0x0, 0xa0(%rsp)
je 0x391ec
movsd 0x16b25a(%rip), %xmm0   # 0x1a4398
movsd %xmm0, 0x98(%rsp)
movq 0x160(%rsp), %rax
movq %rax, 0x8(%rsp)
leaq 0xa8(%rsp), %rdi
callq 0x308e0
cvttsd2si %xmm0, %eax
movl %eax, 0x14(%rsp)
leaq 0xa8(%rsp), %rdi
callq 0x30910
movq 0x8(%rsp), %rdi
movl 0x14(%rsp), %esi
cvttsd2si %xmm0, %edx
callq 0x43c60
movsd %xmm0, 0x90(%rsp)
movsd 0x90(%rsp), %xmm0
leaq 0x1c33a7(%rip), %rax    # 0x1fc548
movsd (%rax), %xmm1
callq 0x308b0
cmpl $0x0, %eax
je 0x391b4
jmp 0x392ef
movsd 0x90(%rsp), %xmm0
subsd 0xf8(%rsp), %xmm0
movaps 0x16b1f3(%rip), %xmm1   # 0x1a43c0
pand %xmm1, %xmm0
divsd 0xf8(%rsp), %xmm0
ucomisd 0x98(%rsp), %xmm0
jbe 0x391ea
jmp 0x392ef
jmp 0x391ec
movsd 0x16af74(%rip), %xmm0   # 0x1a4168
divsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0xc8(%rsp), %xmm0
mulsd 0xf0(%rsp), %xmm0
mulsd 0x88(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
xorps %xmm0, %xmm0
ucomisd 0x80(%rsp), %xmm0
jb 0x3923d
jmp 0x392ef
movq 0x158(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x43cc0
movsd %xmm0, 0x78(%rsp)
movq 0x150(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x43cc0
movsd %xmm0, 0x70(%rsp)
movq 0x148(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x43cc0
movsd %xmm0, 0x68(%rsp)
movsd 0x78(%rsp), %xmm0
movsd 0x70(%rsp), %xmm1
movsd 0x68(%rsp), %xmm2
leaq 0x50(%rsp), %rdi
callq 0x141cf0
movsd 0x80(%rsp), %xmm0
leaq 0x38(%rsp), %rdi
leaq 0x50(%rsp), %rsi
callq 0x43d20
leaq 0x178(%rsp), %rdi
leaq 0x38(%rsp), %rsi
callq 0x141eb0
movsd 0x80(%rsp), %xmm0
addsd 0x170(%rsp), %xmm0
movsd %xmm0, 0x170(%rsp)
movl 0x16c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x16c(%rsp)
jmp 0x38e6a
movsd 0x170(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x39334
movq 0x28(%rsp), %rdi
movsd 0x170(%rsp), %xmm0
leaq 0x178(%rsp), %rsi
callq 0x43d60
jmp 0x39345
movq 0x28(%rsp), %rdi
leaq 0x5c60b0(%rip), %rsi    # 0x5ff3f0
callq 0x141cb0
movq 0x30(%rsp), %rax
addq $0x1b8, %rsp            # imm = 0x1B8
retq
nopw %cs:(%rax,%rax)