// File: chrislarson1[P]MVEE/src/mvee.cpp
// Function: mvee::Mvee::khachiyan(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, double, double)
void Mvee::khachiyan(MatrixXd& data, double eps, double lmcoeff)
{
    // Khachiyan's algorithm
    auto t0 = chrono::system_clock::now();    
    iters = 0; 
    long double err = INFINITY; 
    double alpha;
    int n = data.rows();
    int d = data.cols();
    MatrixXd X = data.transpose();
    MatrixXd Q(X.rows() + 1, X.cols());
    Q.row(0) = X.row(0);
    Q.row(1) = X.row(1);
    Q.row(2).setOnes();
    VectorXd u = (1 / (double) n) * VectorXd::Ones(n);
    VectorXd uhat = u;
    MatrixXd G(d + 1, d + 1);
    MatrixXd noiseye = MatrixXd::Identity(G.rows(), G.cols()) * lmcoeff;
    VectorXd g(n);
    double m; int i;
    while (err > eps)
    {
        G = Q * u.asDiagonal() * Q.transpose() + noiseye;
        g = (Q.transpose() * G.inverse() * Q).diagonal();
        m = g.maxCoeff();
        i = findIdx(g, m);
        alpha = (m - d - 1) / ((d + 1) * (m - 1));
        uhat = (1 - alpha) * u;
        uhat(i) += alpha;
        err = (uhat - u).norm();
        u = uhat;
        iters++;
    }
    time = chrono::duration<double>(
        chrono::system_clock::now() - t0
    ).count();
    
    // Decompose US^2U^T
    MatrixXd E = (1 / (double) d) * (
        X * u.asDiagonal() * X.transpose()
        - (X * u) * (X * u).transpose()
    ).inverse();
    VectorXd x = X * u;
    VectorXd s(d);
    MatrixXd V(d, d);
    MatrixXd U(d, d);
    decompose(E, s, U, V);

    // Cache result
    _centroid = toStdVec(x, [](double z) { return z; });
    _radii    = toStdVec(s, [](double z) { return 1 / sqrt(z); });
    _pose     = toStdMat(V, [](double z) { return z; });
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x770, %rsp            # imm = 0x770
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movsd %xmm0, -0x18(%rbp)
movsd %xmm1, -0x20(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x678(%rbp)
callq 0x1cf960
movq %rax, %rcx
movq -0x678(%rbp), %rax
movq %rcx, -0x28(%rbp)
movq $0x0, 0x50(%rax)
flds 0xae647(%rip)           # 0x29f000
fstpt -0x40(%rbp)
movq -0x10(%rbp), %rdi
callq 0x1d5c30
movl %eax, -0x4c(%rbp)
movq -0x10(%rbp), %rdi
callq 0x1e8d50
movl %eax, -0x50(%rbp)
movq -0x10(%rbp), %rdi
callq 0x1eba90
movq %rax, -0x70(%rbp)
leaq -0x68(%rbp), %rdi
movq %rdi, -0x670(%rbp)
leaq -0x70(%rbp), %rsi
callq 0x1d4e50
movq -0x670(%rbp), %rdi
callq 0x1d5c30
movq -0x670(%rbp), %rdi
incq %rax
movq %rax, -0x90(%rbp)
callq 0x1e8d50
movq %rax, -0x98(%rbp)
leaq -0x88(%rbp), %rdi
leaq -0x90(%rbp), %rsi
leaq -0x98(%rbp), %rdx
callq 0x1d87f0
jmp 0x1f0a3a
xorl %eax, %eax
movl %eax, %edx
leaq -0xe0(%rbp), %rdi
leaq -0x68(%rbp), %rsi
callq 0x1e9830
jmp 0x1f0a50
xorl %eax, %eax
movl %eax, %edx
leaq -0x118(%rbp), %rdi
leaq -0x88(%rbp), %rsi
callq 0x1e9830
jmp 0x1f0a69
leaq -0x118(%rbp), %rdi
leaq -0xe0(%rbp), %rsi
callq 0x1e3a50
jmp 0x1f0a7e
leaq -0x150(%rbp), %rdi
leaq -0x68(%rbp), %rsi
movl $0x1, %edx
callq 0x1e9830
jmp 0x1f0a95
leaq -0x188(%rbp), %rdi
leaq -0x88(%rbp), %rsi
movl $0x1, %edx
callq 0x1e9830
jmp 0x1f0aaf
leaq -0x188(%rbp), %rdi
leaq -0x150(%rbp), %rsi
callq 0x1e3a50
jmp 0x1f0ac4
leaq -0x1c0(%rbp), %rdi
leaq -0x88(%rbp), %rsi
movl $0x2, %edx
callq 0x1e9830
jmp 0x1f0ade
leaq -0x1c0(%rbp), %rdi
callq 0x1d6890
jmp 0x1f0aec
cvtsi2sdl -0x4c(%rbp), %xmm1
movsd 0xae50f(%rip), %xmm0    # 0x29f008
divsd %xmm1, %xmm0
movsd %xmm0, -0x218(%rbp)
movslq -0x4c(%rbp), %rsi
leaq -0x230(%rbp), %rdi
callq 0x1d24c0
jmp 0x1f0b17
leaq -0x210(%rbp), %rdi
leaq -0x218(%rbp), %rsi
leaq -0x230(%rbp), %rdx
callq 0x1e4380
jmp 0x1f0b33
leaq -0x1d0(%rbp), %rdi
leaq -0x210(%rbp), %rsi
callq 0x1d3270
jmp 0x1f0b48
leaq -0x240(%rbp), %rdi
leaq -0x1d0(%rbp), %rsi
callq 0x1ef090
jmp 0x1f0b5d
movl -0x50(%rbp), %eax
incl %eax
movl %eax, -0x25c(%rbp)
movl -0x50(%rbp), %eax
incl %eax
movl %eax, -0x260(%rbp)
leaq -0x258(%rbp), %rdi
leaq -0x25c(%rbp), %rsi
leaq -0x260(%rbp), %rdx
callq 0x1e9d90
jmp 0x1f0b8f
leaq -0x258(%rbp), %rdi
movq %rdi, -0x688(%rbp)
callq 0x1d5c30
movq -0x688(%rbp), %rdi
movq %rax, -0x680(%rbp)
callq 0x1e8d50
movq -0x680(%rbp), %rsi
movq %rax, %rdx
leaq -0x2d0(%rbp), %rdi
callq 0x1d0700
jmp 0x1f0bcd
leaq -0x2b8(%rbp), %rdi
leaq -0x2d0(%rbp), %rsi
leaq -0x20(%rbp), %rdx
callq 0x1e8ce0
jmp 0x1f0be6
leaq -0x278(%rbp), %rdi
leaq -0x2b8(%rbp), %rsi
callq 0x1eec80
jmp 0x1f0bfb
leaq -0x2e0(%rbp), %rdi
leaq -0x4c(%rbp), %rsi
callq 0x1e15c0
jmp 0x1f0c0d
jmp 0x1f0c0f
fldt -0x40(%rbp)
fldl -0x18(%rbp)
fxch %st(1)
fucompi %st(1), %st
fstp %st(0)
jbe 0x1f0fe6
jmp 0x1f0c23
leaq -0x1d0(%rbp), %rdi
callq 0x1ef630
movq %rax, -0x690(%rbp)
jmp 0x1f0c38
movq -0x690(%rbp), %rax
movq %rax, -0x348(%rbp)
leaq -0x88(%rbp), %rdi
leaq -0x348(%rbp), %rsi
callq 0x1d22c0
movq %rdx, -0x6a0(%rbp)
movq %rax, -0x698(%rbp)
jmp 0x1f0c69
movq -0x6a0(%rbp), %rax
movq -0x698(%rbp), %rcx
movq %rcx, -0x340(%rbp)
movq %rax, -0x338(%rbp)
leaq -0x88(%rbp), %rdi
callq 0x1eba90
movq %rax, -0x6a8(%rbp)
jmp 0x1f0c9a
movq -0x6a8(%rbp), %rax
movq %rax, -0x350(%rbp)
leaq -0x330(%rbp), %rdi
leaq -0x340(%rbp), %rsi
leaq -0x350(%rbp), %rdx
callq 0x1d6ad0
jmp 0x1f0cc4
leaq -0x318(%rbp), %rdi
leaq -0x330(%rbp), %rsi
leaq -0x278(%rbp), %rdx
callq 0x1d0f60
jmp 0x1f0ce0
leaq -0x258(%rbp), %rdi
leaq -0x318(%rbp), %rsi
callq 0x1e0ce0
jmp 0x1f0cf5
leaq -0x88(%rbp), %rdi
callq 0x1eba90
movq %rax, -0x6b0(%rbp)
jmp 0x1f0d0a
movq -0x6b0(%rbp), %rax
movq %rax, -0x3a0(%rbp)
leaq -0x258(%rbp), %rdi
callq 0x1de030
movq %rax, -0x6b8(%rbp)
jmp 0x1f0d2d
movq -0x6b8(%rbp), %rax
movq %rax, -0x3a8(%rbp)
leaq -0x3a0(%rbp), %rdi
leaq -0x3a8(%rbp), %rsi
callq 0x1eb880
movq %rdx, -0x6c8(%rbp)
movq %rax, -0x6c0(%rbp)
jmp 0x1f0d5e
movq -0x6c8(%rbp), %rax
movq -0x6c0(%rbp), %rcx
movq %rcx, -0x398(%rbp)
movq %rax, -0x390(%rbp)
leaq -0x388(%rbp), %rdi
leaq -0x398(%rbp), %rsi
leaq -0x88(%rbp), %rdx
callq 0x1e04a0
jmp 0x1f0d96
leaq -0x370(%rbp), %rdi
leaq -0x388(%rbp), %rsi
callq 0x1efb80
jmp 0x1f0dab
leaq -0x2e0(%rbp), %rdi
leaq -0x370(%rbp), %rsi
callq 0x1df5f0
jmp 0x1f0dc0
leaq -0x2e0(%rbp), %rdi
callq 0x1ee0a0
movsd %xmm0, -0x6d0(%rbp)
jmp 0x1f0dd6
movsd -0x6d0(%rbp), %xmm0
movsd %xmm0, -0x2e8(%rbp)
movsd -0x2e8(%rbp), %xmm0
leaq -0x2e0(%rbp), %rdi
callq 0x1d7ce0
movl %eax, -0x6d4(%rbp)
jmp 0x1f0e02
movl -0x6d4(%rbp), %eax
movl %eax, -0x2ec(%rbp)
movsd -0x2e8(%rbp), %xmm3
movl -0x50(%rbp), %eax
cvtsi2sd %eax, %xmm0
movaps %xmm3, %xmm1
subsd %xmm0, %xmm1
movsd 0xae1dc(%rip), %xmm0    # 0x29f008
subsd %xmm0, %xmm1
incl %eax
cvtsi2sd %eax, %xmm2
subsd %xmm0, %xmm3
mulsd %xmm3, %xmm2
divsd %xmm2, %xmm1
movsd %xmm1, -0x48(%rbp)
movsd -0x48(%rbp), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0x3e0(%rbp)
leaq -0x3d8(%rbp), %rdi
leaq -0x3e0(%rbp), %rsi
leaq -0x1d0(%rbp), %rdx
callq 0x1d7590
jmp 0x1f0e74
leaq -0x240(%rbp), %rdi
leaq -0x3d8(%rbp), %rsi
callq 0x1d9d90
jmp 0x1f0e89
movsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x6e8(%rbp)
movslq -0x2ec(%rbp), %rsi
leaq -0x240(%rbp), %rdi
callq 0x1ef370
movq %rax, -0x6e0(%rbp)
jmp 0x1f0eb2
movq -0x6e0(%rbp), %rax
movsd -0x6e8(%rbp), %xmm1
movsd (%rax), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rax)
leaq -0x3f8(%rbp), %rdi
leaq -0x240(%rbp), %rsi
leaq -0x1d0(%rbp), %rdx
callq 0x1ed190
jmp 0x1f0ee9
leaq -0x3f8(%rbp), %rdi
callq 0x1e49a0
movsd %xmm0, -0x6f0(%rbp)
jmp 0x1f0eff
movsd -0x6f0(%rbp), %xmm0
movsd %xmm0, -0x668(%rbp)
fldl -0x668(%rbp)
fstpt -0x40(%rbp)
leaq -0x1d0(%rbp), %rdi
leaq -0x240(%rbp), %rsi
callq 0x1e81c0
jmp 0x1f0f2d
movq -0x678(%rbp), %rax
movq 0x50(%rax), %rcx
addq $0x1, %rcx
movq %rcx, 0x50(%rax)
jmp 0x1f0c0f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f154c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f1540
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f1534
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f1528
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f151c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f1510
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f1504
callq 0x1cf960
movq %rax, -0x410(%rbp)
leaq -0x410(%rbp), %rdi
leaq -0x28(%rbp), %rsi
callq 0x1d7e10
movq %rax, -0x6f8(%rbp)
jmp 0x1f100b
movq -0x6f8(%rbp), %rax
movq %rax, -0x408(%rbp)
leaq -0x400(%rbp), %rdi
leaq -0x408(%rbp), %rsi
callq 0x1e63b0
jmp 0x1f102e
leaq -0x400(%rbp), %rdi
callq 0x1d6e90
movsd %xmm0, -0x700(%rbp)
jmp 0x1f1044
movq -0x678(%rbp), %rax
movsd -0x700(%rbp), %xmm0
movsd %xmm0, 0x48(%rax)
cvtsi2sdl -0x50(%rbp), %xmm1
movsd 0xadfa3(%rip), %xmm0    # 0x29f008
divsd %xmm1, %xmm0
movsd %xmm0, -0x498(%rbp)
leaq -0x1d0(%rbp), %rdi
callq 0x1ef630
movq %rax, -0x708(%rbp)
jmp 0x1f1086
movq -0x708(%rbp), %rax
movq %rax, -0x548(%rbp)
leaq -0x68(%rbp), %rdi
leaq -0x548(%rbp), %rsi
callq 0x1d22c0
movq %rdx, -0x718(%rbp)
movq %rax, -0x710(%rbp)
jmp 0x1f10b4
movq -0x718(%rbp), %rax
movq -0x710(%rbp), %rcx
movq %rcx, -0x540(%rbp)
movq %rax, -0x538(%rbp)
leaq -0x68(%rbp), %rdi
callq 0x1eba90
movq %rax, -0x720(%rbp)
jmp 0x1f10e2
movq -0x720(%rbp), %rax
movq %rax, -0x550(%rbp)
leaq -0x530(%rbp), %rdi
leaq -0x540(%rbp), %rsi
leaq -0x550(%rbp), %rdx
callq 0x1d6ad0
jmp 0x1f110c
leaq -0x68(%rbp), %rdi
leaq -0x1d0(%rbp), %rsi
callq 0x1e1240
movq %rdx, -0x730(%rbp)
movq %rax, -0x728(%rbp)
jmp 0x1f112c
movq -0x730(%rbp), %rax
movq -0x728(%rbp), %rcx
movq %rcx, -0x580(%rbp)
movq %rax, -0x578(%rbp)
leaq -0x68(%rbp), %rdi
leaq -0x1d0(%rbp), %rsi
callq 0x1e1240
movq %rdx, -0x740(%rbp)
movq %rax, -0x738(%rbp)
jmp 0x1f1168
movq -0x740(%rbp), %rax
movq -0x738(%rbp), %rcx
movq %rcx, -0x5a0(%rbp)
movq %rax, -0x598(%rbp)
leaq -0x5a0(%rbp), %rdi
callq 0x1eb190
movq %rdx, -0x750(%rbp)
movq %rax, -0x748(%rbp)
jmp 0x1f11a0
movq -0x750(%rbp), %rax
movq -0x748(%rbp), %rcx
movq %rcx, -0x590(%rbp)
movq %rax, -0x588(%rbp)
leaq -0x570(%rbp), %rdi
leaq -0x580(%rbp), %rsi
leaq -0x590(%rbp), %rdx
callq 0x1e6560
jmp 0x1f11d8
leaq -0x518(%rbp), %rdi
leaq -0x530(%rbp), %rsi
leaq -0x570(%rbp), %rdx
callq 0x1d6d10
jmp 0x1f11f4
leaq -0x4d8(%rbp), %rdi
leaq -0x518(%rbp), %rsi
callq 0x1dba00
jmp 0x1f1209
leaq -0x490(%rbp), %rdi
leaq -0x498(%rbp), %rsi
leaq -0x4d8(%rbp), %rdx
callq 0x1dbab0
jmp 0x1f1225
leaq -0x428(%rbp), %rdi
leaq -0x490(%rbp), %rsi
callq 0x1d0af0
jmp 0x1f123a
leaq -0x68(%rbp), %rdi
leaq -0x1d0(%rbp), %rsi
callq 0x1e1240
movq %rdx, -0x760(%rbp)
movq %rax, -0x758(%rbp)
jmp 0x1f125a
movq -0x760(%rbp), %rax
movq -0x758(%rbp), %rcx
movq %rcx, -0x5c0(%rbp)
movq %rax, -0x5b8(%rbp)
leaq -0x5b0(%rbp), %rdi
leaq -0x5c0(%rbp), %rsi
callq 0x1ddcd0
jmp 0x1f128b
leaq -0x5d0(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x1e15c0
jmp 0x1f129d
leaq -0x5e8(%rbp), %rdi
leaq -0x50(%rbp), %rdx
movq %rdx, %rsi
callq 0x1e9d90
jmp 0x1f12b2
leaq -0x600(%rbp), %rdi
leaq -0x50(%rbp), %rdx
movq %rdx, %rsi
callq 0x1e9d90
jmp 0x1f12c7
movq -0x678(%rbp), %rdi
leaq -0x428(%rbp), %rsi
leaq -0x5d0(%rbp), %rdx
leaq -0x600(%rbp), %rcx
leaq -0x5e8(%rbp), %r8
callq 0x1dc5c0
jmp 0x1f12f1
leaq -0x619(%rbp), %rdi
callq 0x1f1570
movq %rax, %rdx
leaq -0x618(%rbp), %rdi
leaq -0x5b0(%rbp), %rsi
callq 0x1eccf0
jmp 0x1f1315
movq -0x678(%rbp), %rdi
leaq -0x618(%rbp), %rsi
movq %rsi, -0x768(%rbp)
callq 0x1d1200
movq -0x768(%rbp), %rdi
callq 0x1e7270
leaq -0x639(%rbp), %rdi
callq 0x1f1590
movq %rax, %rdx
leaq -0x638(%rbp), %rdi
leaq -0x5d0(%rbp), %rsi
callq 0x1eccf0
jmp 0x1f135f
movq -0x678(%rbp), %rdi
addq $0x18, %rdi
leaq -0x638(%rbp), %rsi
movq %rsi, -0x770(%rbp)
callq 0x1d1200
movq -0x770(%rbp), %rdi
callq 0x1e7270
leaq -0x659(%rbp), %rdi
callq 0x1f15b0
movq %rax, %rdx
leaq -0x658(%rbp), %rdi
leaq -0x5e8(%rbp), %rsi
callq 0x1ed170
jmp 0x1f13ad
movq -0x678(%rbp), %rdi
addq $0x30, %rdi
leaq -0x658(%rbp), %rsi
callq 0x1d9420
leaq -0x658(%rbp), %rdi
callq 0x1d2ec0
leaq -0x600(%rbp), %rdi
callq 0x1d5cb0
leaq -0x5e8(%rbp), %rdi
callq 0x1d5cb0
leaq -0x5d0(%rbp), %rdi
callq 0x1dfd00
leaq -0x5b0(%rbp), %rdi
callq 0x1dfd00
leaq -0x428(%rbp), %rdi
callq 0x1d5cb0
leaq -0x2e0(%rbp), %rdi
callq 0x1dfd00
leaq -0x278(%rbp), %rdi
callq 0x1d5cb0
leaq -0x258(%rbp), %rdi
callq 0x1d5cb0
leaq -0x240(%rbp), %rdi
callq 0x1dfd00
leaq -0x1d0(%rbp), %rdi
callq 0x1dfd00
leaq -0x88(%rbp), %rdi
callq 0x1d5cb0
leaq -0x68(%rbp), %rdi
callq 0x1d5cb0
addq $0x770, %rsp            # imm = 0x770
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f14f8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f14ec
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f14e0
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
jmp 0x1f14d4
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
leaq -0x600(%rbp), %rdi
callq 0x1d5cb0
leaq -0x5e8(%rbp), %rdi
callq 0x1d5cb0
leaq -0x5d0(%rbp), %rdi
callq 0x1dfd00
leaq -0x5b0(%rbp), %rdi
callq 0x1dfd00
leaq -0x428(%rbp), %rdi
callq 0x1d5cb0
leaq -0x2e0(%rbp), %rdi
callq 0x1dfd00
leaq -0x278(%rbp), %rdi
callq 0x1d5cb0
leaq -0x258(%rbp), %rdi
callq 0x1d5cb0
leaq -0x240(%rbp), %rdi
callq 0x1dfd00
leaq -0x1d0(%rbp), %rdi
callq 0x1dfd00
leaq -0x88(%rbp), %rdi
callq 0x1d5cb0
leaq -0x68(%rbp), %rdi
callq 0x1d5cb0
movq -0xa0(%rbp), %rdi
callq 0x1ebeb0
nopw %cs:(%rax,%rax)