// File: /workspace/github_cpp_makefile_star2_O1/sus304[P]ForRocket/src/environment/vincenty.cpp
// Function: forrocket::vdownrange(Eigen::Matrix<double, 3, 1, 0, 3, 1>&, Eigen::Matrix<double, 3, 1, 0, 3, 1>&)
std::pair<double, double> forrocket::vdownrange(Eigen::Vector3d& observer_LLH, Eigen::Vector3d& target_LLH) {
    // Input: [lat, lon, alt], [lat, lon, alt] ([deg, m])
    // Output: downrange [m], Azimuth start->end [deg]

    int itr_limit = 5000;

    if (observer_LLH(0) == target_LLH(0) && observer_LLH(1) == target_LLH(1)) {
        return std::make_pair(0.0, 0.0);
    }
    
    double lat1 = deg2rad(observer_LLH(0));
    double lon1 = deg2rad(observer_LLH(1));
    double lat2 = deg2rad(target_LLH(0));
    double lon2 = deg2rad(target_LLH(1));

    WGS84 wgs84;

    double U1 = std::atan((1.0 - wgs84.f) * std::tan(lat1));
    double U2 = std::atan((1.0 - wgs84.f) * std::tan(lat2));
    double diff_lon = lon2 - lon1;

    double sin_sigma = 0.0;
    double cos_sigma = 0.0;
    double sigma = 0.0;
    double sin_alpha = 0.0;
    double cos_alpha = 0.0;
    double cos_2sigma_m = 0.0;
    double coeff = 0.0;

    double lamda = diff_lon;
    for (int i=0; i < itr_limit; ++i) {
        sin_sigma = std::pow(std::cos(U2) * std::sin(lamda), 2) + std::pow(std::cos(U1) * std::sin(U2) - std::sin(U1) * std::cos(U2) * std::cos(lamda), 2);
        sin_sigma = std::sqrt(sin_sigma);
        cos_sigma = std::sin(U1) * std::sin(U2) + std::cos(U1) * std::cos(U2) * std::cos(lamda);
        sigma = std::atan2(sin_sigma, cos_sigma);

        sin_alpha = std::cos(U1) * std::cos(U2) * std::sin(lamda) / sin_sigma;
        cos_alpha = std::sqrt(1.0 - std::pow(sin_alpha, 2));

        cos_2sigma_m = cos_sigma - 2.0 * std::sin(U1) * std::sin(U2) / std::pow(cos_alpha, 2);

        coeff = wgs84.f / 16.0 * std::pow(cos_alpha, 2) * (4.0 + wgs84.f * (4.0 - 3.0 * std::pow(cos_alpha, 2)));
        double lamda_itr = lamda;
        lamda = diff_lon + (1.0 - coeff) * wgs84.f * sin_alpha * (sigma + coeff * sin_sigma * (cos_2sigma_m + coeff * cos_sigma * (-1.0 + 2.0 * cos_2sigma_m)));

        if (std::abs(lamda - lamda_itr) < 1e-12) {
            break;
        } // TODO: 収束しなかった時の処理追加
    }

    double u_squr = std::pow(cos_alpha, 2) * (std::pow(wgs84.a, 2) - std::pow(wgs84.b, 2)) / std::pow(wgs84.b, 2);
    double A = 1.0 + u_squr / 16384.0 * (4096.0 + u_squr * (-768.0 + u_squr * (320.0 - 175.0 * u_squr)));
    double B = u_squr / 1024.0 * (256.0 + u_squr * (-128.0 + u_squr * (74.0 - 47.0 * u_squr)));
    double delta_sigma = B * sin_sigma * (cos_2sigma_m + 0.25 * B *(cos_sigma * (-1.0 + 2.0 * std::pow(cos_2sigma_m, 2))
                        - (1.0 / 6.0) * B * cos_2sigma_m * (-3.0 + 4.0 * std::pow(sin_sigma, 2)) * (-3.0 + 4.0 * std::pow(cos_2sigma_m, 2))));
    
    double downrange = wgs84.b * A * (sigma - delta_sigma);
    double alpha1 = std::atan2(std::cos(U2) * std::sin(lamda), (std::cos(U1) * std::sin(U2) - std::sin(U1) * std::cos(U2) * std::cos(lamda)));  // observer to target azimuth
    double alpha2 = std::atan2(std::cos(U1) * std::sin(lamda), (-std::sin(U1) * std::cos(U2) + std::cos(U1) * std::sin(U2) * std::cos(lamda)));  // target to observer azimuth
    return std::make_pair(downrange, rad2deg(alpha1));
}

// Assembly:
pushq %r14
pushq %rbx
subq $0x58, %rsp
movq %rsi, %rbx
movq %rdi, %r14
movsd (%rdi), %xmm0
ucomisd (%rsi), %xmm0
jne 0x18d6c
jp 0x18d6c
movsd 0x8(%r14), %xmm1
xorpd %xmm2, %xmm2
ucomisd 0x8(%rbx), %xmm1
xorpd %xmm1, %xmm1
jne 0x18d6c
jnp 0x19361
callq 0x726c
movsd %xmm0, 0x8(%rsp)
movsd 0x8(%r14), %xmm0
callq 0x726c
movsd %xmm0, 0x28(%rsp)
movsd (%rbx), %xmm0
callq 0x726c
movsd %xmm0, 0x20(%rsp)
movsd 0x8(%rbx), %xmm0
callq 0x726c
movsd %xmm0, 0x48(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x51f0
mulsd 0x2233e(%rip), %xmm0    # 0x3b0f8
callq 0x52e0
movsd %xmm0, 0x8(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x51f0
mulsd 0x22320(%rip), %xmm0    # 0x3b0f8
callq 0x52e0
movsd %xmm0, 0x20(%rsp)
movsd 0x48(%rsp), %xmm0
subsd 0x28(%rsp), %xmm0
movl $0x1387, %ebx           # imm = 0x1387
movsd %xmm0, 0x48(%rsp)
movsd %xmm0, 0x28(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x18(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
mulsd 0x18(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x30(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x40(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x38(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5640
movsd 0x38(%rsp), %xmm1
mulsd 0x10(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd 0x40(%rsp), %xmm0
mulsd 0x30(%rsp), %xmm0
subsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
addsd 0x18(%rsp), %xmm0
ucomisd 0x2183b(%rip), %xmm0    # 0x3a6e8
jb 0x18eb5
sqrtsd %xmm0, %xmm0
jmp 0x18eba
callq 0x52b0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x40(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x30(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
mulsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5640
mulsd 0x10(%rsp), %xmm0
movsd 0x30(%rsp), %xmm1
mulsd 0x40(%rsp), %xmm1
addsd %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
movsd %xmm1, 0x30(%rsp)
callq 0x5080
movsd %xmm0, 0x40(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
mulsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
mulsd 0x10(%rsp), %xmm0
divsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movapd %xmm0, %xmm1
mulsd %xmm0, %xmm1
movsd 0x21749(%rip), %xmm0    # 0x3a6e0
subsd %xmm1, %xmm0
ucomisd 0x21745(%rip), %xmm0    # 0x3a6e8
jb 0x18fab
sqrtsd %xmm0, %xmm0
jmp 0x18fb0
callq 0x52b0
movsd %xmm0, 0x38(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
addsd %xmm0, %xmm0
movsd %xmm0, 0x50(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
mulsd 0x50(%rsp), %xmm0
movsd 0x38(%rsp), %xmm5
mulsd %xmm5, %xmm5
divsd %xmm5, %xmm0
movsd 0x30(%rsp), %xmm11
movapd %xmm11, %xmm1
subsd %xmm0, %xmm1
movapd %xmm5, %xmm0
mulsd 0x220fa(%rip), %xmm0    # 0x3b100
movapd %xmm5, %xmm2
mulsd 0x21296(%rip), %xmm2    # 0x3a2a8
movsd 0x216de(%rip), %xmm3    # 0x3a6f8
addsd %xmm3, %xmm2
movsd 0x220e2(%rip), %xmm4    # 0x3b108
mulsd %xmm4, %xmm2
addsd %xmm3, %xmm2
mulsd %xmm0, %xmm2
movsd 0x216a6(%rip), %xmm0    # 0x3a6e0
subsd %xmm2, %xmm0
mulsd %xmm4, %xmm0
movsd 0x10(%rsp), %xmm4
mulsd %xmm0, %xmm4
movsd 0x18(%rsp), %xmm10
movapd %xmm10, %xmm0
mulsd %xmm2, %xmm0
mulsd %xmm11, %xmm2
movapd %xmm1, %xmm3
addsd %xmm1, %xmm3
addsd 0x21aff(%rip), %xmm3    # 0x3ab70
mulsd %xmm2, %xmm3
addsd %xmm1, %xmm3
mulsd %xmm0, %xmm3
movsd 0x40(%rsp), %xmm7
addsd %xmm7, %xmm3
mulsd %xmm4, %xmm3
addsd 0x48(%rsp), %xmm3
movapd %xmm3, %xmm0
subsd 0x28(%rsp), %xmm0
andpd 0x220ed(%rip), %xmm0    # 0x3b190
subl $0x1, %ebx
setb %al
movsd 0x2205f(%rip), %xmm2    # 0x3b110
ucomisd %xmm0, %xmm2
movapd %xmm5, %xmm4
ja 0x190c7
movapd %xmm3, %xmm0
testb %al, %al
je 0x18dfa
mulsd 0x22049(%rip), %xmm4    # 0x3b118
divsd 0x22049(%rip), %xmm4    # 0x3b120
movsd 0x22049(%rip), %xmm2    # 0x3b128
movsd 0x22049(%rip), %xmm0    # 0x3b130
mulsd %xmm4, %xmm0
addsd 0x22045(%rip), %xmm0    # 0x3b138
mulsd %xmm4, %xmm2
mulsd %xmm4, %xmm0
addsd 0x2203d(%rip), %xmm0    # 0x3b140
mulsd %xmm4, %xmm0
addsd 0x22039(%rip), %xmm0    # 0x3b148
mulsd %xmm2, %xmm0
addsd 0x215c5(%rip), %xmm0    # 0x3a6e0
movsd %xmm3, 0x28(%rsp)
movsd 0x22027(%rip), %xmm3    # 0x3b150
mulsd %xmm4, %xmm3
movsd 0x22023(%rip), %xmm2    # 0x3b158
mulsd %xmm4, %xmm2
addsd 0x2201f(%rip), %xmm2    # 0x3b160
mulsd %xmm4, %xmm2
addsd 0x2201b(%rip), %xmm2    # 0x3b168
mulsd %xmm4, %xmm2
addsd 0x22017(%rip), %xmm2    # 0x3b170
mulsd %xmm3, %xmm2
movapd %xmm10, %xmm3
mulsd %xmm2, %xmm3
movsd 0x219d2(%rip), %xmm4    # 0x3ab40
movapd %xmm1, %xmm5
mulsd %xmm1, %xmm5
movapd %xmm5, %xmm6
addsd %xmm5, %xmm6
addsd 0x219ea(%rip), %xmm6    # 0x3ab70
mulsd %xmm2, %xmm4
mulsd %xmm10, %xmm10
movsd 0x21560(%rip), %xmm9    # 0x3a6f8
mulsd %xmm9, %xmm10
movsd 0x21102(%rip), %xmm8    # 0x3a2a8
addsd %xmm8, %xmm10
mulsd %xmm9, %xmm5
addsd %xmm8, %xmm5
mulsd 0x21fbb(%rip), %xmm2    # 0x3b178
mulsd %xmm1, %xmm2
mulsd %xmm10, %xmm2
mulsd %xmm5, %xmm2
mulsd %xmm6, %xmm11
addsd %xmm2, %xmm11
mulsd %xmm4, %xmm11
addsd %xmm1, %xmm11
mulsd %xmm3, %xmm11
mulsd 0x21f95(%rip), %xmm0    # 0x3b180
subsd %xmm11, %xmm7
mulsd %xmm0, %xmm7
movsd %xmm7, 0x40(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x18(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
mulsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x30(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x38(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x50(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5640
movsd 0x50(%rsp), %xmm1
mulsd 0x38(%rsp), %xmm1
mulsd %xmm0, %xmm1
movapd %xmm1, %xmm0
movsd 0x10(%rsp), %xmm1
mulsd 0x30(%rsp), %xmm1
subsd %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
callq 0x5080
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x30(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
mulsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x38(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x8(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movsd 0x8(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5640
movsd 0x8(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd 0x38(%rsp), %xmm0
mulsd 0x10(%rsp), %xmm0
subsd %xmm0, %xmm1
movsd 0x30(%rsp), %xmm0
callq 0x5080
movsd 0x18(%rsp), %xmm0
callq 0x727d
movsd 0x40(%rsp), %xmm2
movapd %xmm0, %xmm1
movapd %xmm2, %xmm0
addq $0x58, %rsp
popq %rbx
popq %r14
retq
nopl (%rax)