// File: sus304[P]ForRocket/src/environment/coordinate.cpp
// Function: forrocket::Coordinate::ECEF2LLH(Eigen::Matrix<double, 3, 1, 0, 3, 1>)
Eigen::Vector3d forrocket::Coordinate::ECEF2LLH(const Eigen::Vector3d pos_ECEF) {
    // return: degree
    double x = pos_ECEF[0];
    double y = pos_ECEF[1];
    double z = pos_ECEF[2];
    double lat, lon, height;
    double p = std::sqrt(x * x + y * y);
    double theta = std::atan2(z * wgs.a, (p * wgs.b));
    double e_dash_square = (wgs.a * wgs.a - wgs.b * wgs.b) / std::pow(wgs.b, 2);
    lat = std::atan2(z + e_dash_square * wgs.b * std::pow(std::sin(theta), 3), p - wgs.e_square * wgs.a * std::pow(std::cos(theta), 3));
    lon = std::atan2(y, x);
    double N = wgs.a / std::sqrt(1 - wgs.e_square * std::pow(std::sin(lat), 2));
    height = (p / std::cos(lat) - N);
    return Eigen::Vector3d(rad2deg(lat), rad2deg(lon), height);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xe0, %rsp
movq %rdx, -0xe0(%rbp)
movq %rdi, %rax
movq -0xe0(%rbp), %rdi
movq %rax, -0x88(%rbp)
movq %rax, %rcx
movq %rcx, -0x80(%rbp)
movq %rax, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdi, -0x18(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0xa8(%rbp)
xorl %eax, %eax
movl %eax, %esi
callq 0x2f640
movq -0xe0(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x20(%rbp)
movl $0x1, %esi
callq 0x2f640
movq -0xe0(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x28(%rbp)
movl $0x2, %esi
callq 0x2f640
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movsd -0x20(%rbp), %xmm0
movsd -0x20(%rbp), %xmm2
movsd -0x28(%rbp), %xmm1
mulsd -0x28(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
callq 0x6370
movq -0xa8(%rbp), %rax
movsd %xmm0, -0x50(%rbp)
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd -0x50(%rbp), %xmm1
mulsd 0x28(%rax), %xmm1
callq 0x60a0
movq -0xa8(%rbp), %rax
movsd %xmm0, -0x58(%rbp)
movsd (%rax), %xmm0
movsd (%rax), %xmm2
movsd 0x28(%rax), %xmm1
mulsd 0x28(%rax), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xd8(%rbp)
movsd 0x28(%rax), %xmm0
movl $0x2, %edi
callq 0x283f0
movq -0xa8(%rbp), %rax
movaps %xmm0, %xmm1
movsd -0xd8(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x60(%rbp)
movsd -0x30(%rbp), %xmm0
movsd %xmm0, -0xd0(%rbp)
movsd -0x60(%rbp), %xmm0
mulsd 0x28(%rax), %xmm0
movsd %xmm0, -0xc8(%rbp)
movsd -0x58(%rbp), %xmm0
callq 0x6120
movl $0x3, %edi
callq 0x283f0
movsd -0xd0(%rbp), %xmm1
movq -0xa8(%rbp), %rax
movaps %xmm0, %xmm2
movsd -0xc8(%rbp), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xb0(%rbp)
movsd -0x50(%rbp), %xmm0
movsd %xmm0, -0xb8(%rbp)
movsd 0x30(%rax), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0xc0(%rbp)
movsd -0x58(%rbp), %xmm0
callq 0x67f0
movl $0x3, %edi
callq 0x283f0
movsd -0xc0(%rbp), %xmm1
movsd -0xb8(%rbp), %xmm2
movaps %xmm0, %xmm3
movsd -0xb0(%rbp), %xmm0
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm3, %xmm1
addsd %xmm2, %xmm1
callq 0x60a0
movsd %xmm0, -0x38(%rbp)
movsd -0x28(%rbp), %xmm0
movsd -0x20(%rbp), %xmm1
callq 0x60a0
movq -0xa8(%rbp), %rax
movsd %xmm0, -0x40(%rbp)
movsd (%rax), %xmm0
movsd %xmm0, -0x98(%rbp)
movsd 0x30(%rax), %xmm0
movsd %xmm0, -0xa0(%rbp)
movsd -0x38(%rbp), %xmm0
callq 0x6120
movl $0x2, %edi
callq 0x283f0
movaps %xmm0, %xmm1
movsd -0xa0(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm1, %xmm0
movsd 0x67aa4(%rip), %xmm1    # 0x95638
addsd %xmm1, %xmm0
callq 0x6370
movaps %xmm0, %xmm1
movsd -0x98(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x68(%rbp)
movsd -0x50(%rbp), %xmm0
movsd %xmm0, -0x90(%rbp)
movsd -0x38(%rbp), %xmm0
callq 0x67f0
movaps %xmm0, %xmm1
movsd -0x90(%rbp), %xmm0
divsd %xmm1, %xmm0
subsd -0x68(%rbp), %xmm0
movsd %xmm0, -0x48(%rbp)
movsd -0x38(%rbp), %xmm0
callq 0x70c0
movsd %xmm0, -0x70(%rbp)
movsd -0x40(%rbp), %xmm0
callq 0x70c0
movq -0x88(%rbp), %rdi
movsd %xmm0, -0x78(%rbp)
leaq -0x70(%rbp), %rsi
leaq -0x78(%rbp), %rdx
leaq -0x48(%rbp), %rcx
callq 0x2f6d0
movq -0x80(%rbp), %rax
addq $0xe0, %rsp
popq %rbp
retq
nopw %cs:(%rax,%rax)