// File: /workspace/github_cpp_makefile_star2_O1/sus304[P]ForRocket/src/environment/coordinate.cpp
// Function: forrocket::Coordinate::Quaternion(Eigen::Matrix<double, 3, 1, 0, 3, 1>)
Eigen::Vector4d forrocket::Coordinate::Quaternion(const Eigen::Vector3d euler_angle) {
    // Input: radian
    setNED2Body(euler_angle);
    Eigen::Vector4d q;
    q(0) = 0.5 * std::sqrt(1.0 + dcm.NED2body(0, 0) - dcm.NED2body(1, 1) - dcm.NED2body(2, 2));
    q(1) = 0.5 * std::sqrt(1.0 - dcm.NED2body(0, 0) + dcm.NED2body(1, 1) - dcm.NED2body(2, 2));
    q(2) = 0.5 * std::sqrt(1.0 - dcm.NED2body(0, 0) - dcm.NED2body(1, 1) + dcm.NED2body(2, 2));
    q(3) = 0.5 * std::sqrt(1.0 + dcm.NED2body(0, 0) + dcm.NED2body(1, 1) + dcm.NED2body(2, 2));

    Eigen::VectorXd::Index index_quat_max;
    double quat_max = q.maxCoeff(&index_quat_max);
    switch(index_quat_max) {
        case 0:
            q(0) = 0.5 * sqrt(1.0 + dcm.NED2body(0, 0) - dcm.NED2body(1,1) - dcm.NED2body(2,2));
            q(1) = (dcm.NED2body(0, 1) + dcm.NED2body(1, 0)) / (4.0 * q(0));
            q(2) = (dcm.NED2body(2, 0) + dcm.NED2body(0, 2)) / (4.0 * q(0));
            q(3) = (dcm.NED2body(1, 2) - dcm.NED2body(2, 1)) / (4.0 * q(0));
            break;
        case 1:
            q(1) = 0.5 * sqrt(1.0 - dcm.NED2body(0, 0) + dcm.NED2body(1,1) - dcm.NED2body(2,2));
            q(0) = (dcm.NED2body(0, 1) + dcm.NED2body(1, 0)) / (4.0 * q(1));
            q(2) = (dcm.NED2body(1, 2) + dcm.NED2body(2, 1)) / (4.0 * q(1));
            q(3) = (dcm.NED2body(2, 0) - dcm.NED2body(0, 2)) / (4.0 * q(1));
            break;
        case 2:
            q(2) = 0.5 * sqrt(1.0 - dcm.NED2body(0, 0) - dcm.NED2body(1,1) + dcm.NED2body(2,2));
            q(0) = (dcm.NED2body(2, 0) + dcm.NED2body(0, 2)) / (4.0 * q(2));
            q(1) = (dcm.NED2body(1, 2) + dcm.NED2body(2, 1)) / (4.0 * q(2));
            q(3) = (dcm.NED2body(0, 1) - dcm.NED2body(1, 0)) / (4.0 * q(2));
            break;
        case 3:
            q(3) = 0.5 * sqrt(1.0 + dcm.NED2body(0, 0) + dcm.NED2body(1,1) + dcm.NED2body(2,2));
            q(0) = (dcm.NED2body(1, 2) - dcm.NED2body(2, 1)) / (4.0 * q(3));
            q(1) = (dcm.NED2body(2, 0) - dcm.NED2body(0, 2)) / (4.0 * q(3));
            q(2) = (dcm.NED2body(0, 1) - dcm.NED2body(1, 0)) / (4.0 * q(3));  
            break;
        default:
            break;
    }
    q.normalize();
    return q;
}

// Assembly:
pushq %r14
pushq %rbx
subq $0x38, %rsp
movq %rsi, %r14
movq %rdi, %rbx
movq 0x10(%rdx), %rax
leaq 0x20(%rsp), %rsi
movq %rax, 0x10(%rsi)
movups (%rdx), %xmm0
movaps %xmm0, (%rsi)
movq %r14, %rdi
callq 0x15b30
movsd 0xd0(%r14), %xmm0
addsd 0x23ba5(%rip), %xmm0    # 0x3a6e0
subsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
jb 0x16b5d
sqrtsd %xmm0, %xmm0
jmp 0x16b66
callq 0x52b0
xorpd %xmm2, %xmm2
mulsd 0x23b82(%rip), %xmm0    # 0x3a6f0
movsd %xmm0, 0x18(%rsp)
movsd %xmm0, (%rbx)
movsd 0x23b60(%rip), %xmm1    # 0x3a6e0
movapd %xmm1, %xmm0
subsd 0xd0(%r14), %xmm0
addsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
ucomisd %xmm2, %xmm0
jb 0x16bab
sqrtsd %xmm0, %xmm0
jmp 0x16bb8
callq 0x52b0
movsd 0x23b28(%rip), %xmm1    # 0x3a6e0
mulsd 0x23b30(%rip), %xmm0    # 0x3a6f0
movsd %xmm0, 0x10(%rsp)
movsd %xmm0, 0x8(%rbx)
movapd %xmm1, %xmm0
subsd 0xd0(%r14), %xmm0
subsd 0xf0(%r14), %xmm0
addsd 0x110(%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x16bfa
sqrtsd %xmm0, %xmm4
jmp 0x16c07
callq 0x52b0
xorpd %xmm1, %xmm1
movapd %xmm0, %xmm4
mulsd 0x23ae1(%rip), %xmm4    # 0x3a6f0
movsd %xmm4, 0x10(%rbx)
movsd 0xd0(%r14), %xmm0
addsd 0x23abb(%rip), %xmm0    # 0x3a6e0
addsd 0xf0(%r14), %xmm0
addsd 0x110(%r14), %xmm0
ucomisd %xmm1, %xmm0
jb 0x16c43
sqrtsd %xmm0, %xmm0
jmp 0x16c54
movsd %xmm4, 0x8(%rsp)
callq 0x52b0
movsd 0x8(%rsp), %xmm4
movsd 0x23a84(%rip), %xmm3    # 0x3a6e0
mulsd 0x23a8c(%rip), %xmm0    # 0x3a6f0
xorl %eax, %eax
movsd 0x18(%rsp), %xmm1
movsd 0x10(%rsp), %xmm2
ucomisd %xmm1, %xmm2
seta %al
maxsd %xmm1, %xmm2
ucomisd %xmm2, %xmm4
movl $0x2, %ecx
cmovbeq %rax, %rcx
maxsd %xmm2, %xmm4
ucomisd %xmm4, %xmm0
movl $0x3, %eax
cmovbeq %rcx, %rax
movsd %xmm0, 0x18(%rbx)
movsd 0xd0(%r14), %xmm0
movsd 0xf0(%r14), %xmm2
movsd 0x110(%r14), %xmm1
leaq 0x23a3e(%rip), %rcx     # 0x3a700
movslq (%rcx,%rax,4), %rax
addq %rcx, %rax
jmpq *%rax
addsd 0x23a0d(%rip), %xmm0    # 0x3a6e0
subsd %xmm2, %xmm0
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x16e37
sqrtsd %xmm0, %xmm0
jmp 0x16e3c
subsd %xmm0, %xmm3
subsd %xmm2, %xmm3
addsd %xmm1, %xmm3
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm3
jb 0x16d5e
xorps %xmm0, %xmm0
sqrtsd %xmm3, %xmm0
jmp 0x16d67
addsd 0x239c7(%rip), %xmm0    # 0x3a6e0
addsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x16dc8
sqrtsd %xmm0, %xmm0
jmp 0x16dcd
subsd %xmm0, %xmm3
addsd %xmm2, %xmm3
subsd %xmm1, %xmm3
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm3
jb 0x16e9a
xorps %xmm0, %xmm0
sqrtsd %xmm3, %xmm0
jmp 0x16ea3
movapd %xmm3, %xmm0
callq 0x52b0
mulsd 0x23981(%rip), %xmm0    # 0x3a6f0
movsd %xmm0, 0x10(%rbx)
movsd 0xe0(%r14), %xmm1
addsd 0x100(%r14), %xmm1
mulsd 0x2396a(%rip), %xmm0    # 0x3a6f8
divsd %xmm0, %xmm1
movsd %xmm1, (%rbx)
movsd 0x108(%r14), %xmm1
addsd 0xf8(%r14), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rbx)
movsd 0xe8(%r14), %xmm1
subsd 0xd8(%r14), %xmm1
jmp 0x16eff
callq 0x52b0
mulsd 0x2391b(%rip), %xmm0    # 0x3a6f0
movsd %xmm0, 0x18(%rbx)
movsd 0x108(%r14), %xmm1
subsd 0xf8(%r14), %xmm1
mulsd 0x23904(%rip), %xmm0    # 0x3a6f8
divsd %xmm0, %xmm1
movsd %xmm1, (%rbx)
movsd 0xe0(%r14), %xmm1
subsd 0x100(%r14), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rbx)
movsd 0xe8(%r14), %xmm1
subsd 0xd8(%r14), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rbx)
jmp 0x16f08
callq 0x52b0
mulsd 0x238ac(%rip), %xmm0    # 0x3a6f0
movsd %xmm0, (%rbx)
movsd 0xe8(%r14), %xmm1
addsd 0xd8(%r14), %xmm1
mulsd 0x23896(%rip), %xmm0    # 0x3a6f8
divsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rbx)
movsd 0xe0(%r14), %xmm1
addsd 0x100(%r14), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rbx)
movsd 0x108(%r14), %xmm1
subsd 0xf8(%r14), %xmm1
jmp 0x16eff
movapd %xmm3, %xmm0
callq 0x52b0
mulsd 0x23845(%rip), %xmm0    # 0x3a6f0
movsd %xmm0, 0x8(%rbx)
movsd 0xe8(%r14), %xmm1
addsd 0xd8(%r14), %xmm1
mulsd 0x2382e(%rip), %xmm0    # 0x3a6f8
divsd %xmm0, %xmm1
movsd %xmm1, (%rbx)
movsd 0x108(%r14), %xmm1
addsd 0xf8(%r14), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rbx)
movsd 0xe0(%r14), %xmm1
subsd 0x100(%r14), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x18(%rbx)
movapd (%rbx), %xmm1
movapd 0x10(%rbx), %xmm0
movapd %xmm1, %xmm2
mulpd %xmm1, %xmm2
movapd %xmm0, %xmm3
mulpd %xmm0, %xmm3
addpd %xmm2, %xmm3
movapd %xmm3, %xmm2
unpckhpd %xmm3, %xmm2            # xmm2 = xmm2[1],xmm3[1]
addpd %xmm3, %xmm2
xorpd %xmm3, %xmm3
ucomisd %xmm3, %xmm2
jbe 0x16f58
movq %xmm2, %xmm2            # xmm2 = xmm2[0],zero
sqrtpd %xmm2, %xmm2
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
divpd %xmm2, %xmm1
movapd %xmm1, (%rbx)
divpd %xmm2, %xmm0
movapd %xmm0, 0x10(%rbx)
movq %rbx, %rax
addq $0x38, %rsp
popq %rbx
popq %r14
retq
nop