// File: /workspace/github_cpp_makefile_star2_O3/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 %r15
pushq %r14
pushq %r12
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 0x1565e
movsd 0xd0(%r14), %xmm0
addsd 0x240cb(%rip), %xmm0    # 0x3a720
subsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
jb 0x16677
sqrtsd %xmm0, %xmm0
jmp 0x16680
callq 0x52b0
xorpd %xmm2, %xmm2
movsd %xmm0, 0x8(%rsp)
movsd 0x24092(%rip), %xmm1    # 0x3a720
movapd %xmm1, %xmm0
subsd 0xd0(%r14), %xmm0
addsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
ucomisd %xmm2, %xmm0
jb 0x166bc
xorps %xmm2, %xmm2
sqrtsd %xmm0, %xmm2
jmp 0x166cd
callq 0x52b0
movsd 0x24057(%rip), %xmm1    # 0x3a720
movapd %xmm0, %xmm2
movapd %xmm1, %xmm0
subsd 0xd0(%r14), %xmm0
subsd 0xf0(%r14), %xmm0
addsd 0x110(%r14), %xmm0
xorpd %xmm3, %xmm3
ucomisd %xmm3, %xmm0
jb 0x166fc
sqrtsd %xmm0, %xmm4
jmp 0x16715
movsd %xmm2, 0x10(%rsp)
callq 0x52b0
xorpd %xmm3, %xmm3
movsd 0x10(%rsp), %xmm2
movapd %xmm0, %xmm4
movsd 0x8(%rsp), %xmm1
mulsd 0x24005(%rip), %xmm1    # 0x3a728
mulsd 0x23ffd(%rip), %xmm2    # 0x3a728
mulsd 0x23ff5(%rip), %xmm4    # 0x3a728
movsd 0xd0(%r14), %xmm0
addsd 0x23fdc(%rip), %xmm0    # 0x3a720
addsd 0xf0(%r14), %xmm0
addsd 0x110(%r14), %xmm0
ucomisd %xmm3, %xmm0
jb 0x16762
sqrtsd %xmm0, %xmm0
jmp 0x1678b
movsd %xmm1, 0x8(%rsp)
movsd %xmm2, 0x10(%rsp)
movsd %xmm4, 0x18(%rsp)
callq 0x52b0
movsd 0x18(%rsp), %xmm4
movsd 0x10(%rsp), %xmm2
movsd 0x8(%rsp), %xmm1
movsd 0x23f8d(%rip), %xmm3    # 0x3a720
mulsd 0x23f8d(%rip), %xmm0    # 0x3a728
xorl %eax, %eax
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
leaq 0xe8(%r14), %r12
leaq 0x18(%rbx), %r15
movsd 0xd0(%r14), %xmm0
movsd 0xf0(%r14), %xmm2
movsd 0x110(%r14), %xmm1
leaq 0x23f45(%rip), %rcx     # 0x3a738
movslq (%rcx,%rax,4), %rax
addq %rcx, %rax
jmpq *%rax
addsd 0x23f1c(%rip), %xmm0    # 0x3a720
subsd %xmm2, %xmm0
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x16930
sqrtsd %xmm0, %xmm0
jmp 0x16935
subsd %xmm0, %xmm3
subsd %xmm2, %xmm3
addsd %xmm1, %xmm3
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm3
jb 0x16888
xorps %xmm0, %xmm0
sqrtsd %xmm3, %xmm0
jmp 0x16891
addsd 0x23ed6(%rip), %xmm0    # 0x3a720
addsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x168d7
sqrtsd %xmm0, %xmm0
jmp 0x168dc
subsd %xmm0, %xmm3
addsd %xmm2, %xmm3
subsd %xmm1, %xmm3
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm3
jb 0x16989
xorps %xmm0, %xmm0
sqrtsd %xmm3, %xmm0
jmp 0x16992
movapd %xmm3, %xmm0
callq 0x52b0
mulsd 0x23e8f(%rip), %xmm0    # 0x3a728
movsd %xmm0, 0x10(%rbx)
mulsd 0x23e8a(%rip), %xmm0    # 0x3a730
movupd 0xe0(%r14), %xmm1
movupd 0x100(%r14), %xmm2
movhpd 0xf8(%r14), %xmm1       # xmm1 = xmm1[0],mem[0]
addpd %xmm2, %xmm1
movapd %xmm0, %xmm2
unpcklpd %xmm0, %xmm2            # xmm2 = xmm2[0],xmm0[0]
divpd %xmm2, %xmm1
movapd %xmm1, (%rbx)
jmp 0x16924
callq 0x52b0
leaq 0x10(%rbx), %r15
mulsd 0x23e40(%rip), %xmm0    # 0x3a728
movsd %xmm0, 0x18(%rbx)
movupd 0xf8(%r14), %xmm1
movupd 0x108(%r14), %xmm2
mulsd 0x23e29(%rip), %xmm0    # 0x3a730
movhpd 0xe0(%r14), %xmm2       # xmm2 = xmm2[0],mem[0]
subpd %xmm1, %xmm2
movapd %xmm0, %xmm1
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
divpd %xmm1, %xmm2
movapd %xmm2, (%rbx)
addq $0xd8, %r14
jmp 0x169ed
callq 0x52b0
mulsd 0x23deb(%rip), %xmm0    # 0x3a728
movsd %xmm0, (%rbx)
movupd 0xe8(%r14), %xmm1
mulsd 0x23dde(%rip), %xmm0    # 0x3a730
movhpd 0x100(%r14), %xmm1      # xmm1 = xmm1[0],mem[0]
movupd 0xd8(%r14), %xmm2
addpd %xmm2, %xmm1
movapd %xmm0, %xmm2
unpcklpd %xmm0, %xmm2            # xmm2 = xmm2[0],xmm0[0]
divpd %xmm2, %xmm1
movupd %xmm1, 0x8(%rbx)
leaq 0x108(%r14), %r12
addq $0xf8, %r14
jmp 0x169ed
movapd %xmm3, %xmm0
callq 0x52b0
mulsd 0x23d8e(%rip), %xmm0    # 0x3a728
movsd %xmm0, 0x8(%rbx)
movsd 0xe8(%r14), %xmm1
addsd 0xd8(%r14), %xmm1
mulsd 0x23d77(%rip), %xmm0    # 0x3a730
divsd %xmm0, %xmm1
movsd %xmm1, (%rbx)
movsd 0x108(%r14), %xmm1
addsd 0xf8(%r14), %xmm1
movq %r14, %r12
addq $0x100, %r14            # imm = 0x100
divsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rbx)
addq $0xe0, %r12
movsd (%r12), %xmm1
subsd (%r14), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, (%r15)
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 0x16a51
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 %r12
popq %r14
popq %r15
retq