// File: /workspace/github_cpp_makefile_star2_O2/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 $0x68, %rsp
movq %rsi, %rbx
movq %rdi, %r14
movsd (%rdi), %xmm0
ucomisd (%rsi), %xmm0
jne 0x13fa8
jp 0x13fa8
movsd 0x8(%r14), %xmm1
xorpd %xmm2, %xmm2
ucomisd 0x8(%rbx), %xmm1
xorpd %xmm1, %xmm1
jne 0x13fa8
jnp 0x145c1
callq 0x5a6c
movsd %xmm0, 0x8(%rsp)
movsd 0x8(%r14), %xmm0
callq 0x5a6c
movsd %xmm0, 0x20(%rsp)
movsd (%rbx), %xmm0
callq 0x5a6c
movsd %xmm0, 0x28(%rsp)
movsd 0x8(%rbx), %xmm0
callq 0x5a6c
movsd %xmm0, 0x58(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5200
mulsd 0x1e88a(%rip), %xmm0    # 0x32880
callq 0x52d0
movsd %xmm0, 0x8(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5200
mulsd 0x1e86c(%rip), %xmm0    # 0x32880
callq 0x52d0
movsd %xmm0, 0x28(%rsp)
movsd 0x58(%rsp), %xmm0
subsd 0x20(%rsp), %xmm0
movl $0x1389, %ebx           # imm = 0x1389
xorpd %xmm3, %xmm3
xorpd %xmm1, %xmm1
movsd %xmm0, 0x58(%rsp)
movsd %xmm0, 0x20(%rsp)
xorpd %xmm8, %xmm8
xorpd %xmm9, %xmm9
xorpd %xmm0, %xmm0
movsd %xmm0, 0x18(%rsp)
decl %ebx
je 0x1430f
movsd 0x28(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x18(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
mulsd 0x18(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x40(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x38(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x30(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5610
movsd 0x30(%rsp), %xmm1
mulsd 0x10(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd 0x38(%rsp), %xmm0
mulsd 0x40(%rsp), %xmm0
subsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
addsd 0x18(%rsp), %xmm0
sqrtsd %xmm0, %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x38(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
movaps %xmm0, 0x40(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x10(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5610
mulsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5610
mulsd 0x10(%rsp), %xmm0
movapd 0x40(%rsp), %xmm1
mulsd 0x38(%rsp), %xmm1
addsd %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
movapd %xmm1, 0x40(%rsp)
callq 0x5070
movsd %xmm0, 0x38(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x10(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5610
mulsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
mulsd 0x10(%rsp), %xmm0
divsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
movapd %xmm0, %xmm1
mulsd %xmm0, %xmm1
movsd 0x1d75b(%rip), %xmm0    # 0x31940
subsd %xmm1, %xmm0
ucomisd 0x1e6ff(%rip), %xmm0    # 0x328f0
jb 0x141f9
sqrtsd %xmm0, %xmm0
jmp 0x141fe
callq 0x52a0
movsd %xmm0, 0x10(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
addsd %xmm0, %xmm0
movsd %xmm0, 0x60(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
mulsd 0x60(%rsp), %xmm0
movsd 0x10(%rsp), %xmm6
movapd %xmm6, %xmm2
mulsd %xmm6, %xmm2
divsd %xmm2, %xmm0
movapd 0x40(%rsp), %xmm9
movapd %xmm9, %xmm1
subsd %xmm0, %xmm1
movapd %xmm2, %xmm0
mulsd 0x1e630(%rip), %xmm0    # 0x32888
movapd %xmm2, %xmm3
mulsd 0x1d1cc(%rip), %xmm3    # 0x31430
movsd 0x1d6e4(%rip), %xmm4    # 0x31950
addsd %xmm4, %xmm3
movsd 0x1e618(%rip), %xmm5    # 0x32890
mulsd %xmm5, %xmm3
addsd %xmm4, %xmm3
mulsd %xmm0, %xmm3
movsd 0x1d6b4(%rip), %xmm0    # 0x31940
subsd %xmm3, %xmm0
mulsd %xmm5, %xmm0
movsd 0x30(%rsp), %xmm5
mulsd %xmm0, %xmm5
movsd 0x18(%rsp), %xmm0
mulsd %xmm3, %xmm0
mulsd %xmm9, %xmm3
movapd %xmm1, %xmm4
addsd %xmm1, %xmm4
addsd 0x1e013(%rip), %xmm4    # 0x322d0
mulsd %xmm3, %xmm4
addsd %xmm1, %xmm4
mulsd %xmm0, %xmm4
movsd 0x38(%rsp), %xmm8
addsd %xmm8, %xmm4
mulsd %xmm5, %xmm4
addsd 0x58(%rsp), %xmm4
movapd %xmm4, %xmm0
subsd 0x20(%rsp), %xmm0
andpd 0x1e60f(%rip), %xmm0    # 0x32900
movsd 0x1e59f(%rip), %xmm3    # 0x32898
ucomisd %xmm0, %xmm3
movsd %xmm4, 0x20(%rsp)
movapd %xmm6, %xmm3
jbe 0x14058
jmp 0x1431d
mulsd %xmm3, %xmm3
movapd %xmm3, %xmm2
movsd 0x20(%rsp), %xmm4
movsd %xmm4, 0x20(%rsp)
mulsd 0x1e575(%rip), %xmm2    # 0x328a0
divsd 0x1e575(%rip), %xmm2    # 0x328a8
movsd 0x1e575(%rip), %xmm0    # 0x328b0
mulsd %xmm2, %xmm0
addsd 0x1e571(%rip), %xmm0    # 0x328b8
movsd 0x1e571(%rip), %xmm4    # 0x328c0
mulsd %xmm2, %xmm4
movsd 0x1e56d(%rip), %xmm3    # 0x328c8
mulsd %xmm2, %xmm3
addsd 0x1e569(%rip), %xmm3    # 0x328d0
mulsd %xmm2, %xmm3
addsd 0x1e565(%rip), %xmm3    # 0x328d8
mulsd %xmm2, %xmm3
addsd 0x1e561(%rip), %xmm3    # 0x328e0
mulsd %xmm4, %xmm3
movsd 0x18(%rsp), %xmm10
movapd %xmm10, %xmm4
mulsd %xmm3, %xmm4
movapd %xmm1, %xmm5
mulsd %xmm1, %xmm5
mulsd %xmm10, %xmm10
movsd 0x1d5a8(%rip), %xmm7    # 0x31950
mulsd %xmm7, %xmm10
movsd 0x1d07b(%rip), %xmm6    # 0x31430
addsd %xmm6, %xmm10
mulsd %xmm5, %xmm7
addsd %xmm6, %xmm7
movapd %xmm2, %xmm6
unpcklpd %xmm3, %xmm6            # xmm6 = xmm6[0],xmm3[0]
mulsd 0x1e516(%rip), %xmm3    # 0x328e8
mulsd %xmm1, %xmm3
mulsd %xmm10, %xmm3
mulpd 0x1e52d(%rip), %xmm6    # 0x32910
mulsd %xmm7, %xmm3
movapd %xmm2, %xmm7
shufpd $0x2, 0x1e52c(%rip), %xmm0 # xmm0 = xmm0[0],mem[1]
unpcklpd %xmm5, %xmm7            # xmm7 = xmm7[0],xmm5[0]
mulpd %xmm7, %xmm0
addpd 0x1e52c(%rip), %xmm0    # 0x32930
unpcklpd %xmm9, %xmm2            # xmm2 = xmm2[0],xmm9[0]
mulpd %xmm0, %xmm2
movapd 0x1e52b(%rip), %xmm0    # 0x32940
unpcklpd %xmm3, %xmm0            # xmm0 = xmm0[0],xmm3[0]
addpd %xmm2, %xmm0
mulpd %xmm6, %xmm0
movapd 0x1e527(%rip), %xmm2    # 0x32950
unpcklpd %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0]
addpd %xmm0, %xmm2
movapd 0x1e527(%rip), %xmm0    # 0x32960
unpcklpd %xmm4, %xmm0            # xmm0 = xmm0[0],xmm4[0]
mulpd %xmm2, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
subsd %xmm1, %xmm8
mulsd %xmm0, %xmm8
movsd %xmm8, 0x38(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x18(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
mulsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x40(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x30(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x60(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5610
movsd 0x60(%rsp), %xmm1
mulsd 0x30(%rsp), %xmm1
mulsd %xmm0, %xmm1
movapd %xmm1, %xmm0
movsd 0x10(%rsp), %xmm1
mulsd 0x40(%rsp), %xmm1
subsd %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
callq 0x5070
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x40(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
mulsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x40(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x30(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x8(%rsp)
movsd 0x28(%rsp), %xmm0
callq 0x50e0
movsd 0x8(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5610
movsd 0x8(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd 0x30(%rsp), %xmm0
mulsd 0x10(%rsp), %xmm0
subsd %xmm0, %xmm1
movsd 0x40(%rsp), %xmm0
callq 0x5070
movsd 0x18(%rsp), %xmm0
callq 0x5a7d
movsd 0x38(%rsp), %xmm2
movapd %xmm0, %xmm1
movapd %xmm2, %xmm0
addq $0x68, %rsp
popq %rbx
popq %r14
retq
nopl (%rax)