// File: /workspace/github_cpp_makefile_star2_O2/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 $0x28, %rsp
movq %rsi, %r14
movq %rdi, %rbx
movq 0x10(%rdx), %rax
leaq 0x10(%rsp), %rsi
movq %rax, 0x10(%rsi)
movups (%rdx), %xmm0
movaps %xmm0, (%rsi)
movq %r14, %rdi
callq 0x1123e
movsd 0xd0(%r14), %xmm0
addsd 0x1f9ef(%rip), %xmm0    # 0x31940
subsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
jb 0x11f73
sqrtsd %xmm0, %xmm0
jmp 0x11f7c
callq 0x52a0
xorpd %xmm2, %xmm2
mulsd 0x1f9c4(%rip), %xmm0    # 0x31948
movsd %xmm0, (%rbx)
movsd 0x1f9b0(%rip), %xmm1    # 0x31940
movapd %xmm1, %xmm0
subsd 0xd0(%r14), %xmm0
addsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
ucomisd %xmm2, %xmm0
jb 0x11fbb
sqrtsd %xmm0, %xmm0
jmp 0x11fc8
callq 0x52a0
movsd 0x1f978(%rip), %xmm1    # 0x31940
mulsd 0x1f978(%rip), %xmm0    # 0x31948
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 0x12004
sqrtsd %xmm0, %xmm0
jmp 0x1200d
callq 0x52a0
xorpd %xmm1, %xmm1
mulsd 0x1f933(%rip), %xmm0    # 0x31948
movsd %xmm0, 0x10(%rbx)
movsd 0xd0(%r14), %xmm0
addsd 0x1f915(%rip), %xmm0    # 0x31940
addsd 0xf0(%r14), %xmm0
addsd 0x110(%r14), %xmm0
ucomisd %xmm1, %xmm0
jb 0x12049
sqrtsd %xmm0, %xmm0
jmp 0x1204e
callq 0x52a0
mulsd 0x1f8f2(%rip), %xmm0    # 0x31948
movsd %xmm0, 0x18(%rbx)
leaq 0x8(%rsp), %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x1263c
movq (%r15), %rax
cmpq $0x3, %rax
ja 0x122e5
leaq 0xe8(%r14), %r12
leaq 0x18(%rbx), %r15
leaq 0x1f8ce(%rip), %rcx     # 0x31958
movslq (%rcx,%rax,4), %rax
addq %rcx, %rax
jmpq *%rax
movsd 0xd0(%r14), %xmm0
addsd 0x1f89c(%rip), %xmm0    # 0x31940
subsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x12218
sqrtsd %xmm0, %xmm0
jmp 0x1221d
movsd 0x1f86b(%rip), %xmm0    # 0x31940
subsd 0xd0(%r14), %xmm0
subsd 0xf0(%r14), %xmm0
addsd 0x110(%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x12174
sqrtsd %xmm0, %xmm0
jmp 0x12179
movsd 0xd0(%r14), %xmm0
addsd 0x1f82f(%rip), %xmm0    # 0x31940
addsd 0xf0(%r14), %xmm0
addsd 0x110(%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x121bf
sqrtsd %xmm0, %xmm0
jmp 0x121c4
movsd 0x1f7fe(%rip), %xmm0    # 0x31940
subsd 0xd0(%r14), %xmm0
addsd 0xf0(%r14), %xmm0
subsd 0x110(%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x12271
sqrtsd %xmm0, %xmm0
jmp 0x12276
callq 0x52a0
mulsd 0x1f7c7(%rip), %xmm0    # 0x31948
movsd %xmm0, 0x10(%rbx)
mulsd 0x1f7c2(%rip), %xmm0    # 0x31950
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 0x1220c
callq 0x52a0
leaq 0x10(%rbx), %r15
mulsd 0x1f778(%rip), %xmm0    # 0x31948
movsd %xmm0, 0x18(%rbx)
movupd 0xf8(%r14), %xmm1
movupd 0x108(%r14), %xmm2
mulsd 0x1f761(%rip), %xmm0    # 0x31950
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 0x122d1
callq 0x52a0
mulsd 0x1f723(%rip), %xmm0    # 0x31948
movsd %xmm0, (%rbx)
movupd 0xe8(%r14), %xmm1
mulsd 0x1f716(%rip), %xmm0    # 0x31950
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 0x122d1
callq 0x52a0
mulsd 0x1f6ca(%rip), %xmm0    # 0x31948
movsd %xmm0, 0x8(%rbx)
movsd 0xe8(%r14), %xmm1
addsd 0xd8(%r14), %xmm1
mulsd 0x1f6b3(%rip), %xmm0    # 0x31950
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)
movq %rbx, %rdi
callq 0x12666
movq %rbx, %rax
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r14
popq %r15
retq