// File: /workspace/github_cpp_makefile_star2_O1/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x118, %rsp            # imm = 0x118
movsd %xmm0, 0x48(%rsp)
movq %rcx, 0x70(%rsp)
movq %rdx, %r13
movq %rsi, %rbx
movq %rdi, 0x38(%rsp)
leaq 0xa0(%rsp), %rdi
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
xorpd %xmm2, %xmm2
callq 0xaada8
cmpl $0x0, 0x9c(%rbx)
xorpd %xmm0, %xmm0
jle 0x257a5
movq %rbx, %rsi
xorl %r14d, %r14d
leaq 0xd0(%rsp), %r15
movq %r13, 0x40(%rsp)
movq %rbx, 0x10(%rsp)
movq 0x90(%rsi), %rax
movq (%rax,%r14,8), %rbx
movl 0x2c(%rbx), %eax
testl %eax, %eax
jle 0x25415
movq 0x20(%rbx), %rcx
movq (%rcx), %rdx
jmp 0x25417
xorl %edx, %edx
cmpl $0x2, %eax
jl 0x25426
movq 0x20(%rbx), %rcx
movq 0x8(%rcx), %rcx
jmp 0x25428
xorl %ecx, %ecx
cmpl $0x3, %eax
jl 0x25437
movq 0x20(%rbx), %rax
movq 0x10(%rax), %rax
jmp 0x25439
xorl %eax, %eax
testq %rdx, %rdx
je 0x25792
testq %rcx, %rcx
je 0x25792
testq %rax, %rax
je 0x25792
movq %rax, 0x80(%rsp)
movq %rcx, 0x28(%rsp)
movq %rdx, 0x30(%rsp)
movsd %xmm0, 0x8(%rsp)
leaq 0x30(%rbx), %rsi
leaq 0x100(%rsp), %r12
movq %r12, %rdi
callq 0x84d8e
leaq 0x78(%rbx), %rsi
movq %r15, %rbp
leaq 0xe8(%rsp), %r15
movq %r15, %rdi
callq 0x864ac
movq %rbp, %rdi
movq %r13, %rsi
movq %r12, %rdx
callq 0x84fe0
movq %rbp, %rdi
movq %r15, %rsi
callq 0x85aae
movapd %xmm0, %xmm1
movsd 0xb8edc(%rip), %xmm0    # 0xde398
ucomisd %xmm1, %xmm0
jae 0x25616
movsd %xmm1, 0x78(%rsp)
movq %rbp, %rdi
callq 0x85b38
movq 0x70(%rsp), %r12
movsd (%r12), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
movq %rbp, %r15
jne 0x25514
jp 0x25514
movsd 0x8(%r12), %xmm0
ucomisd %xmm1, %xmm0
jne 0x25514
jp 0x25514
movsd 0x10(%r12), %xmm0
ucomisd %xmm1, %xmm0
movsd 0xb8c40(%rip), %xmm3    # 0xde150
jne 0x25514
jnp 0x2555d
leaq 0x88(%rsp), %rbp
movq %rbp, %rdi
movq %r15, %rsi
callq 0x864ac
movq %r12, %rdi
movq %rbp, %rsi
callq 0x85aae
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jae 0x25629
movapd %xmm0, %xmm3
cmpnlesd %xmm1, %xmm3
andpd %xmm3, %xmm0
movsd 0xb8bfb(%rip), %xmm2    # 0xde150
andnpd %xmm2, %xmm3
orpd %xmm0, %xmm3
movapd %xmm3, 0x50(%rsp)
movsd 0x48(%rsp), %xmm0
ucomisd %xmm1, %xmm0
movsd 0xb8bdb(%rip), %xmm0    # 0xde150
jbe 0x255a5
movsd 0x90(%rbx), %xmm0
subsd 0x48(%rsp), %xmm0
andpd 0xb8e43(%rip), %xmm0    # 0xde3d0
movsd 0xb8e03(%rip), %xmm1    # 0xde398
maxsd %xmm0, %xmm1
movsd 0xb8baf(%rip), %xmm0    # 0xde150
divsd %xmm1, %xmm0
movsd %xmm0, 0x68(%rsp)
leaq 0x18(%rsp), %r12
movq %r12, %rdi
movq %rbx, %rsi
movq 0x40(%rsp), %r13
movq %r13, %rdx
callq 0x3d1ba
movq %rbx, %rdi
movq %r12, %rsi
callq 0x3d79e
testl %eax, %eax
je 0x2562e
movl 0x2c(%rbx), %eax
cmpl $0x4, %eax
movq 0x10(%rsp), %rsi
xorpd %xmm2, %xmm2
movq 0x30(%rsp), %rdi
movapd 0x50(%rsp), %xmm3
movsd 0x78(%rsp), %xmm4
jl 0x2569e
movq 0x20(%rbx), %rcx
cmpq $0x0, 0x18(%rcx)
je 0x2569e
cmpl $0x4, %eax
jl 0x25638
movq 0x20(%rbx), %rax
movq 0x18(%rax), %rax
jmp 0x2563a
movq 0x10(%rsp), %rsi
movsd 0x8(%rsp), %xmm0
movq %rbp, %r15
jmp 0x25792
movq 0x40(%rsp), %r13
movq 0x10(%rsp), %rsi
jmp 0x2578c
xorl %eax, %eax
movsd 0xb8b26(%rip), %xmm0    # 0xde168
testq %rax, %rax
je 0x2566b
cvttsd2si 0x18(%rsp), %ecx
cvttsd2si 0x20(%rsp), %edx
imull 0xc8(%rax), %edx
movq 0xb8(%rax), %rax
addl %ecx, %edx
movslq %edx, %rcx
movsd (%rax,%rcx,8), %xmm0
leaq 0x10f596(%rip), %rax    # 0x134c08
movsd (%rax), %xmm1
ucomisd %xmm0, %xmm1
jae 0x2578c
subsd %xmm4, %xmm0
andpd 0xb8d44(%rip), %xmm0    # 0xde3d0
divsd %xmm4, %xmm0
ucomisd 0xb8ce0(%rip), %xmm0    # 0xde378
ja 0x2578c
movsd 0xb8aaa(%rip), %xmm0    # 0xde150
divsd %xmm4, %xmm0
mulsd 0x68(%rsp), %xmm3
mulsd %xmm0, %xmm3
ucomisd %xmm3, %xmm2
jae 0x2578c
movsd 0x18(%rsp), %xmm0
movsd 0x20(%rsp), %xmm1
movapd %xmm3, 0x50(%rsp)
callq 0x931a6
movsd %xmm0, 0x30(%rsp)
movsd 0x18(%rsp), %xmm0
movsd 0x20(%rsp), %xmm1
movq 0x28(%rsp), %rdi
callq 0x931a6
movsd %xmm0, 0x28(%rsp)
movsd 0x18(%rsp), %xmm0
movsd 0x20(%rsp), %xmm1
movq 0x80(%rsp), %rdi
callq 0x931a6
movaps %xmm0, %xmm2
leaq 0x88(%rsp), %rdi
movsd 0x30(%rsp), %xmm0
movsd 0x28(%rsp), %xmm1
callq 0xaada8
movsd 0x88(%rsp), %xmm0
movapd 0x50(%rsp), %xmm3
mulsd %xmm3, %xmm0
movsd 0x90(%rsp), %xmm1
mulsd %xmm3, %xmm1
movsd 0x98(%rsp), %xmm2
mulsd %xmm3, %xmm2
leaq 0xb8(%rsp), %rbx
movq %rbx, %rdi
callq 0xaada8
leaq 0xa0(%rsp), %rdi
movq %rbx, %rsi
callq 0xaae5a
movq 0x10(%rsp), %rsi
movsd 0x8(%rsp), %xmm0
addsd 0x50(%rsp), %xmm0
jmp 0x25792
movsd 0x8(%rsp), %xmm0
incq %r14
movslq 0x9c(%rsi), %rax
cmpq %rax, %r14
jl 0x253fa
ucomisd 0xb8bdb(%rip), %xmm0    # 0xde388
jbe 0x257e9
movapd %xmm0, %xmm3
movsd 0xa0(%rsp), %xmm0
movsd 0xa8(%rsp), %xmm1
divsd %xmm3, %xmm0
divsd %xmm3, %xmm1
movsd 0xb0(%rsp), %xmm2
divsd %xmm3, %xmm2
movq 0x38(%rsp), %rbx
movq %rbx, %rdi
callq 0xaada8
jmp 0x257fd
leaq 0x10f350(%rip), %rsi    # 0x134b40
movq 0x38(%rsp), %rbx
movq %rbx, %rdi
callq 0xaad8a
movq %rbx, %rax
addq $0x118, %rsp            # imm = 0x118
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq