// File: /workspace/github_cpp_makefile_star2_O3/mitchellsayer[P]FastPointCloudSegmentation/src/./point_cloud_segmenter.cc
// Function: PointCloudSegmenter::CalculatePlaneNormal(std::vector<Vec3, std::allocator<Vec3>>&)
Vec3 PointCloudSegmenter::CalculatePlaneNormal(std::vector<Vec3>& cur_p_gnd) {
  int n = cur_p_gnd.size();

  if (n > 3) {
    //Calculate centroid
    Vec3 centroid;

    std::vector<Vec3>::iterator it;

    for (it = cur_p_gnd.begin(); it != cur_p_gnd.end(); it++) {
      centroid.x += it->x;
      centroid.y += it->y;
      centroid.z += it->z;
    }

    centroid.x /= n;
    centroid.y /= n;
    centroid.z /= n;

    cloud_centroid = centroid;

    //Calc covarience matrix
    double xx = 0, xy = 0, xz = 0,
           yy = 0, yz = 0, zz = 0;

    Vec3 r;

    for (it = cur_p_gnd.begin(); it != cur_p_gnd.end(); it++) {
      r.x = it->x - centroid.x;
      r.y = it->y - centroid.y;
      r.z = it->z - centroid.z;

      xx += r.x * r.x;
      xy += r.x * r.y;
      xz += r.x * r.z;
      yy += r.y * r.y;
      yz += r.y * r.z;
      zz += r.z * r.z;
    }

    double det_x = yy*zz - yz*yz;
    double det_y = xx*zz - xz*xz;
    double det_z = xx*yy - xy*xy;

    double det_max = std::max(det_x, std::max(det_y, det_z));

    Vec3 norm;

    // Pick path with best conditioning
    if (det_max == det_x) {
      norm.x = det_x;
      norm.y = xz*yz - xy*zz;
      norm.z = xy*yz - xz*yy;
    } else if (det_max == det_y) {
      norm.x = xz*yz - xy*zz;
      norm.y = det_y;
      norm.z = xy*xz - yz*xx;
    } else {
      norm.x = xy*yz - xz*yy;
      norm.y = xy*xz - yz*xx;
      norm.z = det_z;
    }

    double dir_mag = sqrt(pow(norm.x, 2) + pow(norm.y, 2) + pow(norm.z, 2));

    norm.x /= dir_mag;
    norm.y /= dir_mag;
    norm.z /= dir_mag;

    return norm;
  }

  return Vec3();
}

// Assembly:
movq %rdi, %rax
movq (%rdx), %rsi
movq 0x8(%rdx), %rdi
movq %rdi, %rcx
subq %rsi, %rcx
shrq $0x2, %rcx
imull $0xb6db6db7, %ecx, %ecx # imm = 0xB6DB6DB7
cmpl $0x4, %ecx
jl 0x2f80
pushq %rbp
movq %rsp, %rbp
cmpq %rdi, %rsi
je 0x2f96
xorps %xmm7, %xmm7
xorps %xmm8, %xmm8
xorps %xmm6, %xmm6
addss (%rsi), %xmm7
addss 0x4(%rsi), %xmm8
addss 0x8(%rsi), %xmm6
addq $0x1c, %rsi
cmpq %rdi, %rsi
jne 0x2f66
jmp 0x2fa0
xorps %xmm0, %xmm0
movups %xmm0, (%rax)
movq $0x0, 0x10(%rax)
movl $0xbf800000, 0x18(%rax) # imm = 0xBF800000
retq
xorps %xmm6, %xmm6
xorps %xmm8, %xmm8
xorps %xmm7, %xmm7
cvtsi2ss %ecx, %xmm0
divss %xmm0, %xmm7
divss %xmm0, %xmm8
divss %xmm0, %xmm6
movss %xmm7, 0x81d7(%rip)     # 0xb190
movss %xmm8, 0x81d2(%rip)     # 0xb194
movss %xmm6, 0x81ce(%rip)     # 0xb198
movabsq $-0x4080000000000000, %rcx # imm = 0xBF80000000000000
movq %rcx, 0x81c9(%rip)      # 0xb1a4
movq $0x0, 0x81b6(%rip) # 0xb19c
movq (%rdx), %rsi
movq 0x8(%rdx), %rdx
cmpq %rdx, %rsi
je 0x30aa
xorpd %xmm5, %xmm5
xorpd %xmm1, %xmm1
xorps %xmm0, %xmm0
xorpd %xmm3, %xmm3
xorpd %xmm4, %xmm4
xorpd %xmm2, %xmm2
movss (%rsi), %xmm9
movss 0x4(%rsi), %xmm10
subss %xmm7, %xmm9
subss %xmm8, %xmm10
movss 0x8(%rsi), %xmm11
subss %xmm6, %xmm11
movaps %xmm9, %xmm12
mulss %xmm9, %xmm12
cvtss2sd %xmm12, %xmm12
addsd %xmm12, %xmm5
movaps %xmm9, %xmm12
mulss %xmm10, %xmm12
cvtss2sd %xmm12, %xmm12
addsd %xmm12, %xmm1
mulss %xmm11, %xmm9
cvtss2sd %xmm9, %xmm9
addsd %xmm9, %xmm0
movaps %xmm10, %xmm9
mulss %xmm10, %xmm9
cvtss2sd %xmm9, %xmm9
addsd %xmm9, %xmm3
mulss %xmm11, %xmm10
xorps %xmm9, %xmm9
cvtss2sd %xmm10, %xmm9
addsd %xmm9, %xmm2
mulss %xmm11, %xmm11
xorps %xmm9, %xmm9
cvtss2sd %xmm11, %xmm9
addsd %xmm9, %xmm4
addq $0x1c, %rsi
cmpq %rdx, %rsi
jne 0x300d
jmp 0x30c1
xorpd %xmm2, %xmm2
xorpd %xmm4, %xmm4
xorpd %xmm3, %xmm3
xorps %xmm0, %xmm0
xorpd %xmm1, %xmm1
xorpd %xmm5, %xmm5
movapd 0x4f66(%rip), %xmm10    # 0x8030
movapd %xmm2, %xmm6
mulsd %xmm2, %xmm6
movapd %xmm3, %xmm7
mulsd %xmm4, %xmm7
subsd %xmm6, %xmm7
movapd %xmm0, %xmm8
xorpd %xmm10, %xmm8
movapd %xmm0, %xmm9
mulsd %xmm0, %xmm9
movapd %xmm5, %xmm6
mulsd %xmm4, %xmm6
subsd %xmm9, %xmm6
movapd %xmm1, %xmm11
xorpd %xmm10, %xmm11
movapd %xmm1, %xmm12
mulsd %xmm1, %xmm12
movapd %xmm5, %xmm9
mulsd %xmm3, %xmm9
subsd %xmm12, %xmm9
movapd %xmm9, %xmm12
maxsd %xmm6, %xmm12
maxsd %xmm7, %xmm12
movq %rcx, 0x14(%rax)
movq $0x0, 0xc(%rax)
ucomisd %xmm7, %xmm12
popq %rbp
jne 0x316c
jp 0x316c
mulsd %xmm11, %xmm4
mulsd %xmm2, %xmm0
addsd %xmm4, %xmm0
mulsd %xmm8, %xmm3
mulsd %xmm2, %xmm1
addsd %xmm3, %xmm1
movapd %xmm1, %xmm9
movapd %xmm0, %xmm6
jmp 0x31b8
xorpd %xmm2, %xmm10
ucomisd %xmm6, %xmm12
mulsd %xmm10, %xmm5
movapd %xmm1, %xmm7
mulsd %xmm0, %xmm7
addsd %xmm5, %xmm7
jne 0x31a3
jp 0x31a3
mulsd %xmm11, %xmm4
mulsd %xmm2, %xmm0
addsd %xmm4, %xmm0
movapd %xmm7, %xmm9
movapd %xmm0, %xmm7
jmp 0x31b8
mulsd %xmm8, %xmm3
mulsd %xmm2, %xmm1
addsd %xmm3, %xmm1
movapd %xmm7, %xmm6
movapd %xmm1, %xmm7
xorps %xmm0, %xmm0
cvtsd2ss %xmm7, %xmm0
xorps %xmm1, %xmm1
cvtsd2ss %xmm6, %xmm1
xorps %xmm2, %xmm2
cvtsd2ss %xmm9, %xmm2
cvtss2sd %xmm0, %xmm0
movaps %xmm0, %xmm3
mulsd %xmm0, %xmm3
cvtss2sd %xmm1, %xmm1
movaps %xmm1, %xmm4
mulsd %xmm1, %xmm4
addsd %xmm3, %xmm4
cvtss2sd %xmm2, %xmm2
movaps %xmm2, %xmm3
mulsd %xmm2, %xmm3
addsd %xmm4, %xmm3
sqrtsd %xmm3, %xmm3
divsd %xmm3, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, (%rax)
divsd %xmm3, %xmm1
xorps %xmm0, %xmm0
cvtsd2ss %xmm1, %xmm0
movss %xmm0, 0x4(%rax)
divsd %xmm3, %xmm2
xorps %xmm0, %xmm0
cvtsd2ss %xmm2, %xmm0
movss %xmm0, 0x8(%rax)
retq