// File: 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
movq %rsp, %rbp
subq $0x6b0, %rsp            # imm = 0x6B0
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movsd %xmm0, -0x20(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x610(%rbp)
leaq -0x2e8(%rbp), %rdi
movq %rdi, -0x608(%rbp)
callq 0x2c800
movq -0x610(%rbp), %rax
movq 0x10(%rax), %rdi
movsd -0x20(%rbp), %xmm0
callq 0x34640
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
movsd -0x20(%rbp), %xmm0
callq 0x34640
movq -0x608(%rbp), %rdi
movsd -0x20(%rbp), %xmm0
callq 0x2d3e0
movq -0x10(%rbp), %rdi
callq 0x5aab0
movq %rax, -0x600(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5aab0
addq $0x18, %rax
movq %rax, -0x5f8(%rbp)
leaq -0x311(%rbp), %rdi
movq %rdi, -0x5f0(%rbp)
callq 0x7ac0
movq -0x600(%rbp), %rsi
movq -0x5f8(%rbp), %rdx
movq -0x5f0(%rbp), %rcx
leaq -0x310(%rbp), %rdi
callq 0x5aad0
jmp 0x6379f
leaq -0x310(%rbp), %rdi
callq 0x5ab50
movq %rax, -0x618(%rbp)
leaq -0x326(%rbp), %rdi
callq 0x5ab70
jmp 0x637c0
movq -0x618(%rbp), %rsi
leaq -0x2f8(%rbp), %rdi
leaq -0x326(%rbp), %rdx
callq 0x5abb0
jmp 0x637dc
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
leaq -0x2f8(%rbp), %rsi
callq 0x5ac00
jmp 0x637fc
leaq -0x310(%rbp), %rdi
callq 0x7760
leaq -0x311(%rbp), %rdi
callq 0x87a0
movq -0x610(%rbp), %rax
leaq -0x2a8(%rbp), %rdi
movq 0x8(%rax), %rsi
addq $0x2a0, %rsi            # imm = 0x2A0
callq 0x3e710
movq %rax, %rcx
movq -0x610(%rbp), %rax
movq %rcx, -0x338(%rbp)
movq %rdx, -0x330(%rbp)
movq 0x8(%rax), %rdi
addq $0x2b8, %rdi            # imm = 0x2B8
leaq -0x338(%rbp), %rsi
callq 0x3e760
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rsi
addq $0x2b8, %rsi            # imm = 0x2B8
leaq -0x368(%rbp), %rdi
movq %rdi, -0x650(%rbp)
callq 0x2f760
movq -0x650(%rbp), %rdx
leaq -0x350(%rbp), %rdi
movq %rdi, -0x648(%rbp)
leaq -0x2e8(%rbp), %rsi
movq %rsi, -0x640(%rbp)
callq 0x2d910
movq -0x648(%rbp), %rsi
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2d0, %rdi            # imm = 0x2D0
callq 0x37c30
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rsi
addq $0x2d0, %rsi            # imm = 0x2D0
leaq -0x380(%rbp), %rdi
movq %rdi, -0x638(%rbp)
callq 0x2f760
movq -0x640(%rbp), %rdi
movq -0x638(%rbp), %rsi
callq 0x2d5e0
movq -0x10(%rbp), %rdi
callq 0x5aab0
addq $0x18, %rax
movq %rax, -0x630(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5aab0
addq $0x30, %rax
movq %rax, -0x628(%rbp)
leaq -0x3a9(%rbp), %rdi
movq %rdi, -0x620(%rbp)
callq 0x7ac0
movq -0x630(%rbp), %rsi
movq -0x628(%rbp), %rdx
movq -0x620(%rbp), %rcx
leaq -0x3a8(%rbp), %rdi
callq 0x5aad0
jmp 0x63962
leaq -0x3a8(%rbp), %rdi
callq 0x5ab50
movq %rax, -0x658(%rbp)
leaq -0x3ab(%rbp), %rdi
callq 0x5ab70
jmp 0x63983
movq -0x658(%rbp), %rsi
leaq -0x390(%rbp), %rdi
leaq -0x3ab(%rbp), %rdx
callq 0x5abb0
jmp 0x6399f
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
leaq -0x390(%rbp), %rsi
callq 0x5ac00
jmp 0x639bf
leaq -0x3a8(%rbp), %rdi
callq 0x7760
leaq -0x3a9(%rbp), %rdi
callq 0x87a0
movq -0x610(%rbp), %rax
leaq -0x2e8(%rbp), %rcx
addq $0x40, %rcx
movq %rcx, -0x6a8(%rbp)
movq 0x8(%rax), %rcx
addq $0x2e8, %rcx            # imm = 0x2E8
movq %rcx, -0x6b0(%rbp)
leaq -0x2e8(%rbp), %rdi
addq $0x40, %rdi
addq $0x240, %rdi            # imm = 0x240
movq 0x8(%rax), %rsi
addq $0x2a0, %rsi            # imm = 0x2A0
callq 0x3e710
movq -0x6b0(%rbp), %rsi
movq %rax, -0x408(%rbp)
movq %rdx, -0x400(%rbp)
leaq -0x3f8(%rbp), %rdi
leaq -0x408(%rbp), %rdx
callq 0x5ac80
movq -0x6a8(%rbp), %rsi
leaq -0x3d8(%rbp), %rdi
leaq -0x3f8(%rbp), %rdx
callq 0x5ac30
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
addq $0x18, %rdi
leaq -0x3d8(%rbp), %rsi
callq 0x5acf0
movq -0x610(%rbp), %rax
leaq -0x2e8(%rbp), %rdi
addq $0x40, %rdi
addq $0x48, %rdi
movq 0x8(%rax), %rsi
addq $0x2e8, %rsi            # imm = 0x2E8
addq $0x18, %rsi
callq 0x3e710
movq %rax, %rcx
movq -0x610(%rbp), %rax
movq %rcx, -0x418(%rbp)
movq %rdx, -0x410(%rbp)
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
addq $0x30, %rdi
leaq -0x418(%rbp), %rsi
callq 0x3e760
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
addq $0x30, %rdi
movl $0x2, %esi
callq 0x372d0
movq %rax, %rcx
movq -0x610(%rbp), %rax
movsd (%rcx), %xmm0
movsd %xmm0, -0x420(%rbp)
movq 0x8(%rax), %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
addq $0x30, %rdi
movl $0x2, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x428(%rbp)
movsd -0x428(%rbp), %xmm0
leaq -0x448(%rbp), %rdi
callq 0x35f30
leaq -0x468(%rbp), %rdi
xorps %xmm0, %xmm0
callq 0x35f30
xorps %xmm0, %xmm0
movsd %xmm0, -0x488(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x490(%rbp)
movsd -0x428(%rbp), %xmm0
callq 0x4a4a0
movsd %xmm0, -0x498(%rbp)
leaq -0x480(%rbp), %rdi
leaq -0x488(%rbp), %rsi
leaq -0x490(%rbp), %rdx
leaq -0x498(%rbp), %rcx
callq 0x2f6d0
xorps %xmm0, %xmm0
movsd %xmm0, -0x4b8(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x4c0(%rbp)
movsd 0x3da1b(%rip), %xmm0    # 0xa15e8
mulsd -0x438(%rbp), %xmm0
mulsd -0x420(%rbp), %xmm0
movsd %xmm0, -0x6a0(%rbp)
movsd -0x420(%rbp), %xmm0
callq 0x373a0
movq -0x610(%rbp), %rax
movaps %xmm0, %xmm1
movsd -0x6a0(%rbp), %xmm0
mulsd %xmm1, %xmm0
movq 0x8(%rax), %rax
mulsd 0x4e8(%rax), %xmm0
movsd %xmm0, -0x4c8(%rbp)
leaq -0x4b0(%rbp), %rdi
leaq -0x4b8(%rbp), %rsi
leaq -0x4c0(%rbp), %rdx
leaq -0x4c8(%rbp), %rcx
callq 0x2f6d0
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x210, %rdi            # imm = 0x210
callq 0x3cdc0
movsd %xmm0, -0x540(%rbp)
leaq -0x538(%rbp), %rdi
leaq -0x4b0(%rbp), %rsi
leaq -0x540(%rbp), %rdx
callq 0x5af70
leaq -0x518(%rbp), %rdi
leaq -0x538(%rbp), %rsi
leaq -0x480(%rbp), %rdx
callq 0x64110
leaq -0x4e0(%rbp), %rdi
leaq -0x518(%rbp), %rsi
callq 0x64180
leaq -0x2e8(%rbp), %rax
addq $0x40, %rax
addq $0x1b0, %rax            # imm = 0x1B0
movq %rax, -0x698(%rbp)
leaq -0x2e8(%rbp), %rdi
addq $0x40, %rdi
addq $0x168, %rdi            # imm = 0x168
leaq -0x4e0(%rbp), %rsi
callq 0x3e710
movq -0x698(%rbp), %rsi
movq %rax, -0x568(%rbp)
movq %rdx, -0x560(%rbp)
leaq -0x558(%rbp), %rdi
leaq -0x568(%rbp), %rdx
callq 0x5b0a0
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x358, %rdi            # imm = 0x358
leaq -0x558(%rbp), %rsi
callq 0x641c0
movq -0x610(%rbp), %rax
movq 0x18(%rax), %rsi
movsd -0x428(%rbp), %xmm0
leaq -0x580(%rbp), %rdi
callq 0x364d0
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
addq $0x30, %rdi
leaq -0x580(%rbp), %rsi
callq 0x641f0
movq -0x610(%rbp), %rax
leaq -0x2e8(%rbp), %rcx
addq $0x40, %rcx
addq $0x1b0, %rcx            # imm = 0x1B0
movq %rcx, -0x690(%rbp)
leaq -0x2e8(%rbp), %rdi
addq $0x40, %rdi
addq $0x168, %rdi            # imm = 0x168
movq 0x8(%rax), %rsi
addq $0x2e8, %rsi            # imm = 0x2E8
addq $0x30, %rsi
callq 0x3e710
movq -0x690(%rbp), %rsi
movq %rax, -0x5d8(%rbp)
movq %rdx, -0x5d0(%rbp)
leaq -0x5c8(%rbp), %rdi
leaq -0x5d8(%rbp), %rdx
callq 0x5b0a0
movq -0x610(%rbp), %rax
leaq -0x2e8(%rbp), %rdi
addq $0x40, %rdi
addq $0x240, %rdi            # imm = 0x240
movq 0x8(%rax), %rsi
addq $0x2a0, %rsi            # imm = 0x2A0
callq 0x3e710
movq %rax, -0x5e8(%rbp)
movq %rdx, -0x5e0(%rbp)
leaq -0x5b0(%rbp), %rdi
leaq -0x5c8(%rbp), %rsi
leaq -0x5e8(%rbp), %rdx
callq 0x64250
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
leaq -0x5b0(%rbp), %rsi
callq 0x642c0
movq -0x610(%rbp), %rax
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
xorl %eax, %eax
movl %eax, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x688(%rbp)
movq -0x18(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x5b120
movsd -0x688(%rbp), %xmm0
movq %rax, %rcx
movq -0x610(%rbp), %rax
movsd %xmm0, (%rcx)
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
movl $0x1, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x680(%rbp)
movq -0x18(%rbp), %rdi
movl $0x1, %esi
callq 0x5b120
movsd -0x680(%rbp), %xmm0
movq %rax, %rcx
movq -0x610(%rbp), %rax
movsd %xmm0, (%rcx)
movq 0x8(%rax), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
movl $0x2, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x678(%rbp)
movq -0x18(%rbp), %rdi
movl $0x2, %esi
callq 0x5b120
movsd -0x678(%rbp), %xmm0
movq %rax, %rcx
movq -0x610(%rbp), %rax
movsd %xmm0, (%rcx)
movq 0x8(%rax), %rdi
addq $0x358, %rdi            # imm = 0x358
xorl %eax, %eax
movl %eax, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x670(%rbp)
movq -0x18(%rbp), %rdi
movl $0x3, %esi
callq 0x5b120
movsd -0x670(%rbp), %xmm0
movq %rax, %rcx
movq -0x610(%rbp), %rax
movsd %xmm0, (%rcx)
movq 0x8(%rax), %rdi
addq $0x358, %rdi            # imm = 0x358
movl $0x1, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x668(%rbp)
movq -0x18(%rbp), %rdi
movl $0x4, %esi
callq 0x5b120
movsd -0x668(%rbp), %xmm0
movq %rax, %rcx
movq -0x610(%rbp), %rax
movsd %xmm0, (%rcx)
movq 0x8(%rax), %rdi
addq $0x358, %rdi            # imm = 0x358
movl $0x2, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x660(%rbp)
movq -0x18(%rbp), %rdi
movl $0x5, %esi
callq 0x5b120
movsd -0x660(%rbp), %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0x6, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0x7, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0x8, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0x9, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0xa, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0xb, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0xc, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq -0x18(%rbp), %rdi
movl $0xd, %esi
callq 0x5b120
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
addq $0x6b0, %rsp            # imm = 0x6B0
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x320(%rbp)
movl %eax, -0x324(%rbp)
jmp 0x640b8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x320(%rbp)
movl %eax, -0x324(%rbp)
leaq -0x310(%rbp), %rdi
callq 0x7760
leaq -0x311(%rbp), %rdi
callq 0x87a0
jmp 0x64104
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x320(%rbp)
movl %eax, -0x324(%rbp)
jmp 0x640f8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x320(%rbp)
movl %eax, -0x324(%rbp)
leaq -0x3a8(%rbp), %rdi
callq 0x7760
leaq -0x3a9(%rbp), %rdi
callq 0x87a0
movq -0x320(%rbp), %rdi
callq 0x6760