// File: /workspace/github_cpp_makefile_star2_O2/sus304[P]ForRocket/src/dynamics/dynamics_3dof_onlauncher.cpp
// Function: forrocket::Dynamics3dofOnLauncher::operator()(std::array<double, 14ul> const&, std::array<double, 14ul>&, double)
void forrocket::Dynamics3dofOnLauncher::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.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;
    #ifdef DEBUG_NO
    std::cout << p_rocket->velocity.ECI << std::endl;
    std::cout << coordinate.dcm.EarthRotate * p_rocket->position.ECI << std::endl;
    std::cout << p_rocket->velocity.ECEF << std::endl;
    std::cout << p_rocket->velocity.NED << std::endl;
    #endif

    p_rocket->attitude.quaternion = Eigen::Map<Eigen::Vector4d>(std::vector<double>(x.begin()+6, x.begin()+10).data()).normalized();

    coordinate.setNED2Body(p_rocket->attitude.quaternion);
    
    p_rocket->attitude.euler_angle = coordinate.EulerAngle();

    // p_rocket->angular_velocity = Eigen::Map<Eigen::Vector3d>(std::vector<double>(x.begin()+10, x.begin()+13).data());

    p_rocket->mass.propellant = x[13];


    // 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));

    // Update Airspeed
    p_rocket->velocity.air_body = coordinate.dcm.NED2body * p_rocket->velocity.NED;
    p_rocket->dynamic_pressure = 0.5 * air.density * std::pow(p_rocket->velocity.air_body.norm(), 2);
    p_rocket->velocity.mach_number = p_rocket->velocity.air_body.norm() / air.speed_of_sound;

    // Update time and mach parameter
    p_rocket->inertia_tensor = p_rocket->getInertiaTensor();

    p_rocket->length_CG = p_rocket->getLengthCG();    
    p_rocket->length_CP = p_rocket->getLengthCP(p_rocket->velocity.mach_number);
    p_rocket->CA = p_rocket->getCA(p_rocket->velocity.mach_number);
    p_rocket->CNa = p_rocket->getCNa(p_rocket->velocity.mach_number);
    p_rocket->Cld = p_rocket->getCld(p_rocket->velocity.mach_number);
    p_rocket->Clp = p_rocket->getClp(p_rocket->velocity.mach_number);
    p_rocket->Cmq = p_rocket->getCmq(p_rocket->velocity.mach_number);
    p_rocket->Cnr = p_rocket->getCnr(p_rocket->velocity.mach_number);

    // Calculate AoA
    // p_rocket->angle_of_attack = std::atan2(p_rocket->velocity.air_body[2], p_rocket->velocity.air_body[0]);
    if (p_rocket->velocity.air_body.norm() <= 0.0) {
        p_rocket->angle_of_attack = 0.0;
        p_rocket->sideslip_angle = 0.0;
    } else {
        p_rocket->angle_of_attack = std::asin(p_rocket->velocity.air_body[2] / p_rocket->velocity.air_body.norm());
        p_rocket->sideslip_angle = std::asin(-p_rocket->velocity.air_body[1] / p_rocket->velocity.air_body.norm());
    }

    // Calculate Force
    p_rocket->force.thrust = p_rocket->getThrust(air.pressure);
    p_rocket->force.thrust(1) = 0.0; p_rocket->force.thrust(2) = 0.0;
    p_rocket->force.aero = AeroForce(p_rocket);
    p_rocket->force.gravity = (coordinate.dcm.NED2body * gravity_NED) * (p_rocket->mass.Sum());
    p_rocket->force.gravity(1) = 0.0; p_rocket->force.gravity(2) = 0.0;

    // Calculate Acceleration
    p_rocket->acceleration.body = p_rocket->force.Sum() / (p_rocket->mass.Sum());
    p_rocket->acceleration.ECI = coordinate.dcm.ECEF2ECI * (coordinate.dcm.NED2ECEF * (coordinate.dcm.body2NED * p_rocket->acceleration.body));
    if (p_rocket->acceleration.body(0) < 0.0) {
        p_rocket->acceleration.body << 0.0, 0.0, 0.0;
        p_rocket->acceleration.ECI << 0.0, 0.0, 0.0;
    }

    // // Calculate Moment
    // p_rocket->moment.gyro = GyroEffectMoment();
    // p_rocket->moment.thrust = ThrustMoment();
    // p_rocket->moment.aero_force = AeroForceMoment();
    // p_rocket->moment.aero_dumping = AeroDampingMoment();
    // p_rocket->moment.jet_dumping = JetDampingMoment();
    
    // // Calculate Angle Velocity
    // p_rocket->angular_acceleration = p_rocket->getInertiaTensor().inverse() * p_rocket->moment.Sum();

    // // Calculate Quaternion
    // p_rocket->quaternion_dot = 0.5 * QuaternionDiff();


    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] = -p_rocket->engine.mdot_prop;  // massdot => mass_prop

    #ifdef DEBUG_NO
    std::cout << p_rocket->force.thrust << std::endl;
    std::cout << p_rocket->CA << std::endl;
    #endif
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x418, %rsp            # imm = 0x418
movsd %xmm0, 0x8(%rsp)
movq %rdx, %rbx
movq %rsi, %r12
movq %rdi, %r14
leaq 0x150(%rsp), %r15
movq %r15, %rdi
callq 0x10fe4
movq 0x10(%r14), %rdi
movsd 0x8(%rsp), %xmm0
callq 0x13326
movq 0x8(%r14), %rdi
movsd 0x8(%rsp), %xmm0
callq 0x13326
movq %r15, %rdi
movsd 0x8(%rsp), %xmm0
callq 0x1187c
leaq 0x18(%r12), %r15
leaq 0x30(%rsp), %rdi
leaq 0xd0(%rsp), %rcx
movq %r12, %rsi
movq %r15, %rdx
callq 0x1d514
movq 0x30(%rsp), %rax
leaq 0x78(%rsp), %rsi
movq %rax, (%rsi)
movl $0x2a0, %edi            # imm = 0x2A0
addq 0x8(%r14), %rdi
callq 0x1e0f2
leaq 0x30(%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 0x30(%rsp), %rsi
callq 0x15ab6
movq 0x8(%r14), %rax
movq 0x2c8(%rax), %rcx
leaq 0x110(%rsp), %rdx
movq %rcx, 0x10(%rdx)
movups 0x2b8(%rax), %xmm0
movaps %xmm0, (%rdx)
leaq 0x30(%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 0xf0(%rsp), %rsi
movq %rcx, 0x10(%rsi)
movupd 0x2d0(%rax), %xmm0
movapd %xmm0, (%rsi)
movq %rbp, %rdi
callq 0x119c8
leaq 0x30(%r12), %r13
leaq 0x30(%rsp), %rdi
leaq 0xd0(%rsp), %rcx
movq %r15, %rsi
movq %r13, %rdx
callq 0x1d514
movq %rbx, 0x8(%rsp)
movq 0x30(%rsp), %rax
leaq 0x78(%rsp), %rsi
movq %rax, (%rsi)
movl $0x2e8, %edi            # imm = 0x2E8
addq 0x8(%r14), %rdi
callq 0x1e0f2
leaq 0x30(%rsp), %r15
movq %r15, %rdi
callq 0x6002
movq 0x8(%r14), %rdi
leaq 0x2e8(%rdi), %rax
leaq 0x3d0(%rsp), %rcx
leaq 0x2a0(%rdi), %rdx
leaq 0x190(%rsp), %rsi
movq %rsi, (%r15)
movq %rax, 0x8(%r15)
movq %rcx, 0x10(%r15)
movq %rdx, 0x18(%r15)
movl $0x300, %ebx            # imm = 0x300
addq %rbx, %rdi
leaq 0x30(%rsp), %r15
movq %r15, %rsi
callq 0x1e163
leaq 0x1d8(%rsp), %rax
movq 0x8(%r14), %rdi
addq %rdi, %rbx
movq %rax, (%r15)
movq %rbx, 0x8(%r15)
addq $0x318, %rdi            # imm = 0x318
leaq 0x30(%rsp), %rsi
callq 0x15ab6
leaq 0x50(%r12), %rdx
leaq 0x78(%rsp), %rdi
leaq 0xb0(%rsp), %rbx
movq %r13, %rsi
movq %rbx, %rcx
callq 0x1d514
movq 0x78(%rsp), %rax
leaq 0xd0(%rsp), %rsi
movq %rax, (%rsi)
leaq 0x30(%rsp), %rdi
callq 0x1d546
movl $0x3f0, %edi            # imm = 0x3F0
addq 0x8(%r14), %rdi
leaq 0x30(%rsp), %rsi
callq 0x14c6c
leaq 0x78(%rsp), %rdi
callq 0x6002
movq 0x8(%r14), %rax
movaps 0x3f0(%rax), %xmm0
movaps 0x400(%rax), %xmm1
leaq 0x130(%rsp), %rsi
movaps %xmm1, 0x10(%rsi)
movaps %xmm0, (%rsi)
leaq 0x150(%rsp), %r13
movq %r13, %rdi
callq 0x115bc
leaq 0x30(%rsp), %rbp
movq %rbp, %rdi
movq %r13, %rsi
callq 0x122fc
movl $0x3d0, %edi            # imm = 0x3D0
addq 0x8(%r14), %rdi
movq %rbp, %rsi
callq 0x1496a
movsd 0x68(%r12), %xmm0
movq 0x8(%r14), %rax
movsd %xmm0, 0x218(%rax)
movsd 0x2e0(%rax), %xmm0
movaps %xmm0, 0x10(%rsp)
leaq 0x78(%rsp), %r13
movq %r13, %rdi
callq 0x13b30
leaq 0xd0(%rsp), %rdi
xorps %xmm0, %xmm0
callq 0x13b30
movsd 0x1777a(%rip), %xmm0    # 0x34770
movapd 0x10(%rsp), %xmm3
movapd %xmm3, %xmm1
addsd %xmm0, %xmm1
cmpltsd 0x158e3(%rip), %xmm3    # 0x328f0
movapd %xmm3, %xmm2
andnpd %xmm1, %xmm2
andpd %xmm0, %xmm3
orpd %xmm2, %xmm3
mulsd %xmm3, %xmm3
movsd 0x1774f(%rip), %xmm0    # 0x34778
divsd %xmm3, %xmm0
xorpd %xmm1, %xmm1
movapd %xmm1, 0xb0(%rsp)
movsd %xmm0, 0xc0(%rsp)
leaq 0x220(%rsp), %r15
movq 0x8(%r14), %rdi
leaq 0x318(%rdi), %rax
movq %r15, (%rbp)
movq %rax, 0x8(%rbp)
movl $0x330, %r12d           # imm = 0x330
addq %r12, %rdi
leaq 0x30(%rsp), %rsi
callq 0x15ab6
movsd 0x10(%r13), %xmm0
mulsd 0x148c9(%rip), %xmm0    # 0x31948
movsd %xmm0, 0x10(%rsp)
movq 0x8(%r14), %rdi
addq %r12, %rdi
callq 0x1c7d2
mulsd %xmm0, %xmm0
mulsd 0x10(%rsp), %xmm0
movq 0x8(%r14), %rdi
movsd %xmm0, 0x350(%rdi)
addq %r12, %rdi
callq 0x1c7d2
divsd 0x18(%r13), %xmm0
movq 0x8(%r14), %rsi
movsd %xmm0, 0x348(%rsi)
leaq 0x30(%rsp), %r13
movq %r13, %rdi
callq 0x1b6f2
movl $0x220, %edi            # imm = 0x220
addq 0x8(%r14), %rdi
movq %r13, %rsi
callq 0x12760
movq 0x8(%r14), %rdi
callq 0x1b480
movq 0x8(%r14), %rdi
movsd %xmm0, 0x200(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x1b4b0
movq 0x8(%r14), %rdi
movsd %xmm0, 0x208(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x1b4ca
movq 0x8(%r14), %rdi
movsd %xmm0, 0x268(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x1b4f4
movq 0x8(%r14), %rdi
movsd %xmm0, 0x270(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x1b50e
movq 0x8(%r14), %rdi
movsd %xmm0, 0x278(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x1b528
movq 0x8(%r14), %rdi
movsd %xmm0, 0x280(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x1b556
movq 0x8(%r14), %rdi
movsd %xmm0, 0x288(%rdi)
movsd 0x348(%rdi), %xmm0
callq 0x1b584
movq 0x8(%r14), %rdi
movsd %xmm0, 0x290(%rdi)
addq %r12, %rdi
callq 0x1c7d2
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
movq 0x8(%r14), %rsi
movq %rbx, %rbp
jae 0x1d236
movsd 0x340(%rsi), %xmm0
movsd %xmm0, 0x10(%rsp)
addq %r12, %rsi
movq %rsi, %rdi
callq 0x1c7d2
movsd 0x10(%rsp), %xmm1
divsd %xmm0, %xmm1
movapd %xmm1, %xmm0
callq 0x5550
movq 0x8(%r14), %rax
movsd %xmm0, 0x460(%rax)
addq %rax, %r12
movsd 0x338(%rax), %xmm0
xorpd 0x14729(%rip), %xmm0    # 0x31930
movapd %xmm0, 0x10(%rsp)
movq %r12, %rdi
callq 0x1c7d2
movapd 0x10(%rsp), %xmm1
divsd %xmm0, %xmm1
movapd %xmm1, %xmm0
callq 0x5550
movq 0x8(%r14), %rsi
movsd %xmm0, 0x468(%rsi)
jmp 0x1d242
xorpd %xmm0, %xmm0
movapd %xmm0, 0x460(%rsi)
movsd 0x80(%rsp), %xmm0
leaq 0x30(%rsp), %r12
movq %r12, %rdi
callq 0x1b5b2
movl $0x388, %ebx            # imm = 0x388
movq 0x8(%r14), %rdi
addq %rbx, %rdi
movq %r12, %rsi
callq 0x1496a
movq 0x8(%r14), %rdx
xorpd %xmm0, %xmm0
movupd %xmm0, 0x390(%rdx)
leaq 0x30(%rsp), %r12
movq %r12, %rdi
callq 0x1cc88
movl $0x3a0, %edi            # imm = 0x3A0
addq 0x8(%r14), %rdi
movq %r12, %rsi
callq 0x1496a
leaq 0x98(%rsp), %r13
movq %r15, (%r13)
movq %rbp, 0x8(%r13)
movl $0x210, %r15d           # imm = 0x210
movq 0x8(%r14), %rdi
addq %r15, %rdi
callq 0x154c4
leaq 0x28(%rsp), %r12
movsd %xmm0, (%r12)
leaq 0x30(%rsp), %rbp
movq %rbp, %rdi
movq %r13, %rsi
movq %r12, %rdx
callq 0x1d5c8
movl $0x3b8, %edi            # imm = 0x3B8
addq 0x8(%r14), %rdi
movq %rbp, %rsi
callq 0x1e5df
movq 0x8(%r14), %rsi
xorpd %xmm0, %xmm0
movupd %xmm0, 0x3c0(%rsi)
addq %rbx, %rsi
leaq 0x98(%rsp), %r13
movq %r13, %rdi
callq 0x14e8e
addq 0x8(%r14), %r15
movq %r15, %rdi
callq 0x154c4
movsd %xmm0, (%r12)
leaq 0x30(%rsp), %r15
leaq 0x28(%rsp), %rdx
movq %r15, %rdi
movq %r13, %rsi
callq 0x1d618
movl $0x370, %ebx            # imm = 0x370
movq 0x8(%r14), %rdi
addq %rbx, %rdi
movq %r15, %rsi
callq 0x1e6e4
leaq 0x340(%rsp), %rax
leaq 0x2f8(%rsp), %rcx
leaq 0x2b0(%rsp), %rdx
movq 0x8(%r14), %rdi
leaq (%rdi,%rbx), %rsi
movq %rax, (%r15)
movq %rcx, 0x8(%r15)
movq %rdx, 0x10(%r15)
movq %rsi, 0x18(%r15)
addq $0x358, %rdi            # imm = 0x358
leaq 0x30(%rsp), %rsi
callq 0x1e786
movq 0x8(%r14), %rax
xorpd %xmm0, %xmm0
ucomisd 0x370(%rax), %xmm0
jbe 0x1d44c
addq %rbx, %rax
leaq 0x30(%rsp), %rdi
movq %rax, (%rdi)
andq $0x0, 0x8(%rdi)
pushq $0x1
popq %rbx
movq %rbx, 0x10(%rdi)
movq %rbx, 0x18(%rdi)
andq $0x0, (%rax)
leaq 0x98(%rsp), %rsi
andq $0x0, (%rsi)
callq 0x126a8
leaq 0x28(%rsp), %rsi
andq $0x0, (%rsi)
movq %rax, %rdi
callq 0x126a8
leaq 0x30(%rsp), %r15
movq %r15, %rdi
callq 0x12750
movq 0x8(%r14), %rax
leaq 0x358(%rax), %rcx
movq %rcx, (%r15)
andq $0x0, 0x8(%r15)
movq %rbx, 0x10(%r15)
movq %rbx, 0x18(%r15)
andq $0x0, 0x358(%rax)
leaq 0x98(%rsp), %rsi
andq $0x0, (%rsi)
leaq 0x30(%rsp), %rdi
callq 0x126a8
leaq 0x28(%rsp), %rsi
andq $0x0, (%rsi)
movq %rax, %rdi
callq 0x126a8
movq 0x8(%rsp), %rbx
leaq 0x30(%rsp), %rdi
callq 0x12750
movq 0x8(%r14), %rax
jmp 0x1d451
movq 0x8(%rsp), %rbx
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)
andq $0x0, 0x60(%rbx)
movsd 0x60(%rax), %xmm0
xorps 0x14472(%rip), %xmm0    # 0x31930
movlps %xmm0, 0x68(%rbx)
addq $0x418, %rsp            # imm = 0x418
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x1d4da
jmp 0x1d4da
jmp 0x1d4da
movq %rax, %rbx
leaq 0x30(%rsp), %rdi
callq 0x12750
jmp 0x1d50b
jmp 0x1d4f9
jmp 0x1d4ef
jmp 0x1d4f9
movq %rax, %rbx
leaq 0x30(%rsp), %rdi
jmp 0x1d506
movq %rax, %rbx
jmp 0x1d50b
movq %rax, %rbx
leaq 0x78(%rsp), %rdi
callq 0x6002
movq %rbx, %rdi
callq 0x55b0
nop