// File: istellartech[P]OpenTsiolkovsky/src/Orbit.cpp
// Function: elementECI2Orbit(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)
Orbit elementECI2Orbit(Vector3d posECI_, Vector3d velECI_){
    //    output
    //    a     Semi-major Axis(km)         長半径(km)
    //    e     Eccentricity(-)             離心率(-)
    //    i     Inclination(rad)            軌道傾斜角(rad)
    //    Omega RAAN(rad)                   昇交点赤経(rad)
    //    omega Argument of Perigee(rad)    近地点引数(rad)
    //    nu    true Anomaly                真近地点離角(rad)
    //    M     Mean Anomaly                平均近地点(rad)
    // cf. http://ccar.colorado.edu/asen5070/handouts/cart2kep2002.pdf
    Orbit element = Orbit();
    double a, e, i, Omega, omega, nu;
    double mu = 3.986004418e5;        // earth gravitational constant 地球の重力定数(km3/s2)
    Vector3d momentumECI_;
    double momentum_abs_;
    double posECI_abs_;
    double velECI_abs_;
    posECI_ = posECI_ / 1000; //m -> km
    velECI_ = velECI_ / 1000;
    momentumECI_ = posECI_.cross(velECI_);
    momentum_abs_ = momentumECI_.norm();
    posECI_abs_ = posECI_.norm();
    velECI_abs_ = velECI_.norm();
    double specific_energy = velECI_abs_ * velECI_abs_ / 2 - mu / posECI_abs_;
    a = - mu / 2 / specific_energy;
    e = pow(1 - momentum_abs_ * momentum_abs_ / a / mu, 0.5);
    i = acos(momentumECI_[2] / momentum_abs_);
    Omega = atan2(momentumECI_[0], -momentumECI_[1]);
    double argument_of_latitude = atan2(posECI_[2] / sin(i), posECI_[0] * cos(Omega) + posECI_[1] * sin(Omega));
    nu = acos((a * (1 - e*e) - posECI_abs_) / e / posECI_abs_);
    omega = argument_of_latitude - nu;
    element.semi_major = a;
    element.eccentricity = e;
    element.inclination = i;
    element.raan = Omega;
    element.arg_perigee = omega;
    element.true_anomaly = nu;
    return element;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x120, %rsp            # imm = 0x120
movq %rdx, -0x120(%rbp)
movq %rsi, -0x108(%rbp)
movq %rdi, -0xe0(%rbp)
movq %rdi, %rax
movq %rax, -0xd8(%rbp)
movq %rsi, -0x8(%rbp)
movq %rdx, -0x10(%rbp)
movsd 0x9206(%rip), %xmm0     # 0x52040
xorps %xmm5, %xmm5
movaps %xmm5, %xmm1
movaps %xmm5, %xmm2
movaps %xmm5, %xmm3
movaps %xmm5, %xmm4
callq 0x49b50
movsd 0x1090a(%rip), %xmm0    # 0x59760
movsd %xmm0, -0x48(%rbp)
leaq -0x60(%rbp), %rdi
callq 0x1d670
movq -0x108(%rbp), %rsi
movsd 0x108e5(%rip), %xmm0    # 0x59758
movsd %xmm0, -0x90(%rbp)
leaq -0x88(%rbp), %rdi
leaq -0x90(%rbp), %rdx
callq 0x205f0
movq -0x108(%rbp), %rdi
leaq -0x88(%rbp), %rsi
callq 0x49c00
movq -0x120(%rbp), %rsi
movsd 0x108a8(%rip), %xmm0    # 0x59758
movsd %xmm0, -0xa8(%rbp)
leaq -0xa0(%rbp), %rdi
leaq -0xa8(%rbp), %rdx
callq 0x205f0
movq -0x120(%rbp), %rdi
leaq -0xa0(%rbp), %rsi
callq 0x49c00
movq -0x120(%rbp), %rdx
movq -0x108(%rbp), %rsi
leaq -0xc0(%rbp), %rdi
callq 0x49c30
leaq -0x60(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
callq 0x20420
leaq -0x60(%rbp), %rdi
callq 0x205d0
movq -0x108(%rbp), %rdi
movsd %xmm0, -0x68(%rbp)
callq 0x205d0
movq -0x120(%rbp), %rdi
movsd %xmm0, -0x70(%rbp)
callq 0x205d0
movsd %xmm0, -0x78(%rbp)
movsd -0x78(%rbp), %xmm0
mulsd -0x78(%rbp), %xmm0
movsd 0x9f36(%rip), %xmm1     # 0x52e80
divsd %xmm1, %xmm0
movsd -0x48(%rbp), %xmm1
divsd -0x70(%rbp), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0xc8(%rbp)
movsd -0x48(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd 0x9ef8(%rip), %xmm1     # 0x52e80
divsd %xmm1, %xmm0
divsd -0xc8(%rbp), %xmm0
movsd %xmm0, -0x18(%rbp)
movsd -0x68(%rbp), %xmm1
mulsd -0x68(%rbp), %xmm1
divsd -0x18(%rbp), %xmm1
divsd -0x48(%rbp), %xmm1
movsd 0x908b(%rip), %xmm0     # 0x52040
subsd %xmm1, %xmm0
movsd 0x9e9f(%rip), %xmm1     # 0x52e60
callq 0x6790
movsd %xmm0, -0x20(%rbp)
leaq -0x60(%rbp), %rdi
movl $0x2, %esi
callq 0x1de40
movsd (%rax), %xmm0
divsd -0x68(%rbp), %xmm0
callq 0x6430
movsd %xmm0, -0x28(%rbp)
leaq -0x60(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x118(%rbp)
leaq -0x60(%rbp), %rdi
movl $0x1, %esi
callq 0x1de40
movsd -0x118(%rbp), %xmm0
movsd (%rax), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
callq 0x60c0
movq -0x108(%rbp), %rdi
movsd %xmm0, -0x30(%rbp)
movl $0x2, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x110(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x6120
movq -0x108(%rbp), %rdi
movaps %xmm0, %xmm1
movsd -0x110(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0xe8(%rbp)
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0xf8(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x6810
movq -0x108(%rbp), %rdi
movsd %xmm0, -0xf0(%rbp)
movl $0x1, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x100(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x6120
movsd -0x100(%rbp), %xmm2
movsd -0xf8(%rbp), %xmm1
movsd -0xf0(%rbp), %xmm3
movaps %xmm0, %xmm4
movsd -0xe8(%rbp), %xmm0
mulsd %xmm4, %xmm2
mulsd %xmm3, %xmm1
addsd %xmm2, %xmm1
callq 0x60c0
movsd %xmm0, -0xd0(%rbp)
movsd -0x18(%rbp), %xmm0
movsd -0x20(%rbp), %xmm2
movsd -0x20(%rbp), %xmm1
movq %xmm2, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm2
mulsd %xmm1, %xmm2
movsd 0x8eff(%rip), %xmm1     # 0x52040
addsd %xmm1, %xmm2
movsd -0x70(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x20(%rbp), %xmm0
divsd -0x70(%rbp), %xmm0
callq 0x6430
movq -0xe0(%rbp), %rdi
movq -0xd8(%rbp), %rax
movsd %xmm0, -0x40(%rbp)
movsd -0xd0(%rbp), %xmm0
subsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x38(%rbp)
movsd -0x18(%rbp), %xmm0
movsd %xmm0, 0x58(%rdi)
movsd -0x20(%rbp), %xmm0
movsd %xmm0, 0x10(%rdi)
movsd -0x28(%rbp), %xmm0
movsd %xmm0, 0x8(%rdi)
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x18(%rdi)
movsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x20(%rdi)
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x48(%rdi)
addq $0x120, %rsp            # imm = 0x120
popq %rbp
retq
nopw %cs:(%rax,%rax)