// File: /workspace/github_cpp_makefile_star2_O2/sus304[P]ForRocket/src/dynamics/dynamics_3dof_parachute.cpp
// Function: forrocket::Dynamics3dofParachute::operator()(std::array<double, 14ul> const&, std::array<double, 14ul>&, double)
void forrocket::Dynamics3dofParachute::operator()(const state& x, state& dx, const double t) {
    Coordinate coordinate;

    // Countup Time
    p_clock->SyncSolverTime(t);
    p_rocket->burn_clock.SyncSolverTime(t);

    // Update Flight Infomation
    coordinate.setECI2ECEF(t);

    p_rocket->position.ECI = Eigen::Map<Eigen::Vector3d>(std::vector<double>(x.begin()+0, x.begin()+3).data());
    p_rocket->position.ECEF = coordinate.dcm.ECI2ECEF * p_rocket->position.ECI;
    p_rocket->position.LLH = coordinate.ECEF2LLH(p_rocket->position.ECEF);

    coordinate.setECEF2NED(p_rocket->position.LLH);

    p_rocket->velocity.ECI = Eigen::Map<Eigen::Vector3d>(std::vector<double>(x.begin()+3, x.begin()+6).data());
    p_rocket->velocity.ECEF = coordinate.dcm.ECI2ECEF * (p_rocket->velocity.ECI - coordinate.dcm.EarthRotate * p_rocket->position.ECI);
    p_rocket->velocity.NED = coordinate.dcm.ECEF2NED * p_rocket->velocity.ECEF;
    double decent_velosity = p_rocket->velocity.NED(2);

    // Update Environment
    double altitude = p_rocket->position.LLH[2];
    EnvironmentAir air(altitude);
    EnvironmentAir air_sea_level(0.0);
    Eigen::Vector3d gravity_NED(0.0, 0.0, gravity(altitude));

    Eigen::Vector3d drag_NED(0.0, 0.0, -0.5 * air.density * decent_velosity * abs(decent_velosity) * p_rocket->CdS_parachute);
    Eigen::Vector3d acceleration_NED = drag_NED / (p_rocket->mass.Sum()) + gravity_NED;
    p_rocket->acceleration.ECI = coordinate.dcm.ECEF2ECI * (coordinate.dcm.NED2ECEF * acceleration_NED);

    Eigen::Vector3d wind_NED = p_wind->getNED(altitude);
    p_rocket->velocity.NED += wind_NED;
    p_rocket->velocity.ECI = coordinate.dcm.ECEF2ECI * (coordinate.dcm.NED2ECEF * p_rocket->velocity.NED) + coordinate.dcm.EarthRotate * p_rocket->position.ECI;

    dx[0] = p_rocket->velocity.ECI[0];  // vel_ECI => pos_ECI
    dx[1] = p_rocket->velocity.ECI[1];  // 
    dx[2] = p_rocket->velocity.ECI[2];  // 
    dx[3] = p_rocket->acceleration.ECI[0];  // acc_ECI => vel_ECI
    dx[4] = p_rocket->acceleration.ECI[1];  // 
    dx[5] = p_rocket->acceleration.ECI[2];  // 
    dx[6] = 0.0;  // quatdot => quat
    dx[7] = 0.0;  // 
    dx[8] = 0.0;  // 
    dx[9] = 0.0;  // 
    dx[10] = 0.0;  // angular_acceleration => angular_velocity
    dx[11] = 0.0;  // 
    dx[12] = 0.0;  // 
    dx[13] = 0.0;  // massdot => mass_prop
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x418, %rsp            # imm = 0x418
movsd %xmm0, (%rsp)
movq %rdx, %rbx
movq %rsi, %r15
movq %rdi, %r14
leaq 0x150(%rsp), %r12
movq %r12, %rdi
callq 0x10fe4
movq 0x10(%r14), %rdi
movsd (%rsp), %xmm0
callq 0x13326
movq 0x8(%r14), %rdi
movsd (%rsp), %xmm0
callq 0x13326
movq %r12, %rdi
movsd (%rsp), %xmm0
callq 0x1187c
leaq 0x18(%r15), %r12
leaq 0x8(%rsp), %rdi
leaq 0x60(%rsp), %rcx
movq %r15, %rsi
movq %r12, %rdx
callq 0x1d514
movq 0x8(%rsp), %rax
leaq 0x80(%rsp), %rsi
movq %rax, (%rsi)
movl $0x2a0, %edi            # imm = 0x2A0
addq 0x8(%r14), %rdi
callq 0x1e0f2
leaq 0x8(%rsp), %r13
movq %r13, %rdi
callq 0x6002
leaq 0x190(%rsp), %rcx
movq 0x8(%r14), %rdi
leaq 0x2a0(%rdi), %rax
movq %rcx, (%r13)
movq %rax, 0x8(%r13)
addq $0x2b8, %rdi            # imm = 0x2B8
leaq 0x8(%rsp), %rsi
callq 0x15ab6
movq 0x8(%r14), %rax
movq 0x2c8(%rax), %rcx
leaq 0x120(%rsp), %rdx
movq %rcx, 0x10(%rdx)
movups 0x2b8(%rax), %xmm0
movaps %xmm0, (%rdx)
leaq 0x8(%rsp), %r13
leaq 0x150(%rsp), %rbp
movq %r13, %rdi
movq %rbp, %rsi
callq 0x11be4
movl $0x2d0, %edi            # imm = 0x2D0
addq 0x8(%r14), %rdi
movq %r13, %rsi
callq 0x1496a
movq 0x8(%r14), %rax
movq 0x2e0(%rax), %rcx
leaq 0x100(%rsp), %rsi
movq %rcx, 0x10(%rsi)
movups 0x2d0(%rax), %xmm0
movaps %xmm0, (%rsi)
movq %rbp, %rdi
callq 0x119c8
addq $0x30, %r15
leaq 0x8(%rsp), %rdi
leaq 0x60(%rsp), %rcx
movq %r12, %rsi
movq %r15, %rdx
callq 0x1d514
movq 0x8(%rsp), %rax
leaq 0x80(%rsp), %rsi
movq %rax, (%rsi)
movl $0x2e8, %r12d           # imm = 0x2E8
movq 0x8(%r14), %rdi
addq %r12, %rdi
callq 0x1e0f2
leaq 0x8(%rsp), %r15
movq %r15, %rdi
callq 0x6002
movq 0x8(%r14), %rdi
leaq (%rdi,%r12), %rax
leaq 0x3d0(%rsp), %rdx
movl $0x2a0, %ecx            # imm = 0x2A0
addq %rdi, %rcx
leaq 0x190(%rsp), %rsi
movq %rsi, (%r15)
movq %rax, 0x8(%r15)
movq %rdx, 0x10(%r15)
movq %rcx, 0x18(%r15)
movl $0x300, %r12d           # imm = 0x300
addq %r12, %rdi
leaq 0x8(%rsp), %r15
movq %r15, %rsi
callq 0x1e163
leaq 0x1d8(%rsp), %rax
movq 0x8(%r14), %rdi
addq %rdi, %r12
movq %rax, (%r15)
movq %r12, 0x8(%r15)
movl $0x318, %ebp            # imm = 0x318
addq %rbp, %rdi
leaq 0x8(%rsp), %rsi
callq 0x15ab6
movq 0x8(%r14), %rax
movsd 0x2e0(%rax), %xmm1
movsd %xmm1, (%rsp)
movsd 0x328(%rax), %xmm0
movaps %xmm0, 0x40(%rsp)
leaq 0x80(%rsp), %r12
movq %r12, %rdi
movaps %xmm1, %xmm0
callq 0x13b30
leaq 0x60(%rsp), %rdi
xorps %xmm0, %xmm0
callq 0x13b30
movsd 0x1592e(%rip), %xmm0    # 0x34770
movsd (%rsp), %xmm2
movapd %xmm2, %xmm1
addsd %xmm0, %xmm1
cmpltsd 0x13a98(%rip), %xmm2    # 0x328f0
movapd %xmm2, %xmm3
andnpd %xmm1, %xmm3
andpd %xmm0, %xmm2
orpd %xmm3, %xmm2
mulsd %xmm2, %xmm2
movsd 0x15904(%rip), %xmm0    # 0x34778
divsd %xmm2, %xmm0
xorpd %xmm2, %xmm2
leaq 0xe0(%rsp), %r15
movapd %xmm2, (%r15)
movsd 0x10(%r12), %xmm1
mulsd 0x15a50(%rip), %xmm1    # 0x348e8
movsd %xmm0, 0x10(%r15)
movapd 0x40(%rsp), %xmm0
mulsd %xmm0, %xmm1
andpd 0x13a50(%rip), %xmm0    # 0x32900
mulsd %xmm1, %xmm0
movq 0x8(%r14), %rdi
mulsd 0x4e8(%rdi), %xmm0
leaq 0xc0(%rsp), %r12
movapd %xmm2, (%r12)
movsd %xmm0, 0x10(%r12)
addq $0x210, %rdi            # imm = 0x210
callq 0x154c4
leaq 0x58(%rsp), %rdx
movsd %xmm0, (%rdx)
leaq 0xa0(%rsp), %r13
movq %r13, %rdi
movq %r12, %rsi
callq 0x1d618
movq (%r13), %rax
leaq 0x8(%rsp), %rcx
movq %rax, 0x8(%rcx)
movsd 0x10(%r13), %xmm0
movsd %xmm0, 0x18(%rcx)
movq %r15, 0x28(%rcx)
leaq 0x138(%rsp), %r15
leaq 0x8(%rsp), %r12
movq %r15, %rdi
movq %r12, %rsi
callq 0x1f082
leaq 0x340(%rsp), %r13
leaq 0x2f8(%rsp), %rax
movq %r13, (%r12)
movq %rax, 0x8(%r12)
movq %r15, 0x10(%r12)
movl $0x358, %edi            # imm = 0x358
addq 0x8(%r14), %rdi
leaq 0x8(%rsp), %r15
movq %r15, %rsi
callq 0x1f199
movq 0x18(%r14), %rsi
leaq 0xa0(%rsp), %r12
movq %r12, %rdi
movsd (%rsp), %xmm0
callq 0x13e80
movq 0x8(%r14), %rdi
addq %rbp, %rdi
movq %r12, %rsi
callq 0x1f066
movq 0x8(%r14), %rdi
addq %rdi, %rbp
movl $0x2a0, %eax            # imm = 0x2A0
addq %rdi, %rax
movq %r13, (%r15)
leaq 0x2f8(%rsp), %rcx
movq %rcx, 0x8(%r15)
movq %rbp, 0x10(%r15)
leaq 0x3d0(%rsp), %rcx
movq %rcx, 0x18(%r15)
movq %rax, 0x20(%r15)
movl $0x2e8, %eax            # imm = 0x2E8
addq %rax, %rdi
leaq 0x8(%rsp), %rsi
callq 0x1f25f
movq 0x8(%r14), %rax
movsd 0x2e8(%rax), %xmm0
movsd %xmm0, (%rbx)
movsd 0x2f0(%rax), %xmm0
movsd %xmm0, 0x8(%rbx)
movsd 0x2f8(%rax), %xmm0
movsd %xmm0, 0x10(%rbx)
movsd 0x358(%rax), %xmm0
movsd %xmm0, 0x18(%rbx)
movsd 0x360(%rax), %xmm0
movsd %xmm0, 0x20(%rbx)
movsd 0x368(%rax), %xmm0
movsd %xmm0, 0x28(%rbx)
xorps %xmm0, %xmm0
movups %xmm0, 0x30(%rbx)
movups %xmm0, 0x40(%rbx)
movups %xmm0, 0x50(%rbx)
movups %xmm0, 0x60(%rbx)
addq $0x418, %rsp            # imm = 0x418
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x1f04b
jmp 0x1f05a
movq %rax, %rbx
leaq 0x8(%rsp), %rdi
callq 0x6002
jmp 0x1f05d
movq %rax, %rbx
movq %rbx, %rdi
callq 0x55b0
nop