// File: /workspace/github_cpp_makefile_star2_O3/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 $0x78, %rsp
movq %rsi, %rbx
movq %rdi, %r14
movsd (%rdi), %xmm0
ucomisd (%rsi), %xmm0
jne 0x187a0
jp 0x187a0
movsd 0x8(%r14), %xmm1
xorpd %xmm2, %xmm2
ucomisd 0x8(%rbx), %xmm1
xorpd %xmm1, %xmm1
jne 0x187a0
jnp 0x18d90
callq 0x734c
movsd %xmm0, 0x8(%rsp)
movsd 0x8(%r14), %xmm0
callq 0x734c
movsd %xmm0, 0x30(%rsp)
movsd (%rbx), %xmm0
callq 0x734c
movsd %xmm0, 0x20(%rsp)
movsd 0x8(%rbx), %xmm0
callq 0x734c
movsd %xmm0, 0x68(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x51f0
mulsd 0x22972(%rip), %xmm0    # 0x3b160
callq 0x52e0
movsd %xmm0, 0x8(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x51f0
mulsd 0x22954(%rip), %xmm0    # 0x3b160
callq 0x52e0
movsd %xmm0, 0x20(%rsp)
movsd 0x68(%rsp), %xmm0
subsd 0x30(%rsp), %xmm0
movl $0x1387, %ebx           # imm = 0x1387
movsd %xmm0, 0x68(%rsp)
movsd %xmm0, 0x30(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x18(%rsp)
movsd 0x30(%rsp), %xmm0
callq 0x50e0
mulsd 0x18(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x40(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x28(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x50(%rsp)
movsd 0x30(%rsp), %xmm0
callq 0x5640
movsd 0x50(%rsp), %xmm1
mulsd 0x10(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd 0x28(%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, 0x28(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movaps %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 0x30(%rsp), %xmm0
callq 0x5640
mulsd 0x10(%rsp), %xmm0
movapd 0x40(%rsp), %xmm1
mulsd 0x28(%rsp), %xmm1
addsd %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
movapd %xmm1, 0x40(%rsp)
callq 0x5080
movsd %xmm0, 0x28(%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 0x30(%rsp), %xmm0
callq 0x50e0
mulsd 0x10(%rsp), %xmm0
divsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movapd %xmm0, %xmm1
mulsd %xmm0, %xmm1
movsd 0x21d67(%rip), %xmm0    # 0x3a720
subsd %xmm1, %xmm0
ucomisd 0x2280b(%rip), %xmm0    # 0x3b1d0
jb 0x189cd
sqrtsd %xmm0, %xmm0
jmp 0x189d2
callq 0x52b0
movapd %xmm0, 0x50(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
addsd %xmm0, %xmm0
movsd %xmm0, 0x70(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
mulsd 0x70(%rsp), %xmm0
movapd 0x50(%rsp), %xmm10
mulsd %xmm10, %xmm10
divsd %xmm10, %xmm0
movapd 0x40(%rsp), %xmm8
movapd %xmm8, %xmm1
subsd %xmm0, %xmm1
movapd %xmm10, %xmm0
mulsd 0x2273c(%rip), %xmm0    # 0x3b168
movapd %xmm10, %xmm2
mulsd 0x2186f(%rip), %xmm2    # 0x3a2a8
movsd 0x21cef(%rip), %xmm3    # 0x3a730
addsd %xmm3, %xmm2
movsd 0x22723(%rip), %xmm4    # 0x3b170
mulsd %xmm4, %xmm2
addsd %xmm3, %xmm2
mulsd %xmm0, %xmm2
movsd 0x21cbf(%rip), %xmm0    # 0x3a720
subsd %xmm2, %xmm0
mulsd %xmm4, %xmm0
movsd 0x10(%rsp), %xmm4
mulsd %xmm0, %xmm4
movsd 0x18(%rsp), %xmm0
mulsd %xmm2, %xmm0
mulsd %xmm8, %xmm2
movapd %xmm1, %xmm3
addsd %xmm1, %xmm3
addsd 0x2211e(%rip), %xmm3    # 0x3abb0
mulsd %xmm2, %xmm3
addsd %xmm1, %xmm3
mulsd %xmm0, %xmm3
movsd 0x28(%rsp), %xmm7
addsd %xmm7, %xmm3
mulsd %xmm4, %xmm3
addsd 0x68(%rsp), %xmm3
movapd %xmm3, %xmm0
subsd 0x30(%rsp), %xmm0
andpd 0x2271c(%rip), %xmm0    # 0x3b1e0
subl $0x1, %ebx
setb %al
movsd 0x226a6(%rip), %xmm2    # 0x3b178
ucomisd %xmm0, %xmm2
ja 0x18ae4
movapd %xmm3, %xmm0
testb %al, %al
je 0x1882e
mulsd 0x22693(%rip), %xmm10   # 0x3b180
divsd 0x22692(%rip), %xmm10   # 0x3b188
movsd 0x22692(%rip), %xmm0    # 0x3b190
mulsd %xmm10, %xmm0
addsd 0x2268d(%rip), %xmm0    # 0x3b198
movapd %xmm3, 0x30(%rsp)
movsd 0x22687(%rip), %xmm3    # 0x3b1a0
mulsd %xmm10, %xmm3
movsd 0x22682(%rip), %xmm2    # 0x3b1a8
mulsd %xmm10, %xmm2
addsd 0x2267d(%rip), %xmm2    # 0x3b1b0
mulsd %xmm10, %xmm2
addsd 0x22678(%rip), %xmm2    # 0x3b1b8
mulsd %xmm10, %xmm2
addsd 0x22673(%rip), %xmm2    # 0x3b1c0
mulsd %xmm3, %xmm2
movsd 0x18(%rsp), %xmm9
movapd %xmm9, %xmm3
mulsd %xmm2, %xmm3
movapd %xmm1, %xmm4
mulsd %xmm1, %xmm4
mulsd %xmm9, %xmm9
movsd 0x21bba(%rip), %xmm6    # 0x3a730
mulsd %xmm6, %xmm9
movsd 0x21725(%rip), %xmm5    # 0x3a2a8
addsd %xmm5, %xmm9
mulsd %xmm4, %xmm6
addsd %xmm5, %xmm6
movapd %xmm10, %xmm5
unpcklpd %xmm2, %xmm5            # xmm5 = xmm5[0],xmm2[0]
mulsd 0x22627(%rip), %xmm2    # 0x3b1c8
mulsd %xmm1, %xmm2
mulsd %xmm9, %xmm2
mulpd 0x2263e(%rip), %xmm5    # 0x3b1f0
mulsd %xmm6, %xmm2
movapd %xmm10, %xmm6
shufpd $0x2, 0x2263c(%rip), %xmm0 # xmm0 = xmm0[0],mem[1]
unpcklpd %xmm4, %xmm6            # xmm6 = xmm6[0],xmm4[0]
mulpd %xmm6, %xmm0
addpd 0x2263c(%rip), %xmm0    # 0x3b210
unpcklpd %xmm8, %xmm10           # xmm10 = xmm10[0],xmm8[0]
mulpd %xmm0, %xmm10
movapd 0x2263a(%rip), %xmm0    # 0x3b220
unpcklpd %xmm2, %xmm0            # xmm0 = xmm0[0],xmm2[0]
addpd %xmm10, %xmm0
mulpd %xmm5, %xmm0
movapd 0x22635(%rip), %xmm2    # 0x3b230
unpcklpd %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0]
addpd %xmm0, %xmm2
movapd 0x22635(%rip), %xmm0    # 0x3b240
unpcklpd %xmm3, %xmm0            # xmm0 = xmm0[0],xmm3[0]
mulpd %xmm2, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
subsd %xmm1, %xmm7
mulsd %xmm0, %xmm7
movsd %xmm7, 0x28(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x18(%rsp)
movapd 0x30(%rsp), %xmm0
callq 0x50e0
mulsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x40(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x50(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x70(%rsp)
movapd 0x30(%rsp), %xmm0
callq 0x5640
movsd 0x70(%rsp), %xmm1
mulsd 0x50(%rsp), %xmm1
mulsd %xmm0, %xmm1
movapd %xmm1, %xmm0
movsd 0x10(%rsp), %xmm1
mulsd 0x40(%rsp), %xmm1
subsd %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
callq 0x5080
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x40(%rsp)
movapd 0x30(%rsp), %xmm0
callq 0x50e0
mulsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x40(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x5640
movsd %xmm0, 0x50(%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)
movapd 0x30(%rsp), %xmm0
callq 0x5640
movsd 0x8(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd 0x50(%rsp), %xmm0
mulsd 0x10(%rsp), %xmm0
subsd %xmm0, %xmm1
movsd 0x40(%rsp), %xmm0
callq 0x5080
movsd 0x18(%rsp), %xmm0
callq 0x735d
movsd 0x28(%rsp), %xmm2
movapd %xmm0, %xmm1
movapd %xmm2, %xmm0
addq $0x78, %rsp
popq %rbx
popq %r14
retq