// File: /workspace/github_cpp_makefile_star2_O2/sus304[P]ForRocket/src/solver/trajectory_solver.cpp
// Function: forrocket::TrajectorySolver::Solve()
void forrocket::TrajectorySolver::Solve() {
    DynamicsBase::state x0;  // ECI pos, ECI vel, quat, angle vel, mass
    for (int i=0; i < stage_vector.size(); ++i) {
        RocketStage& stage = stage_vector[i];
        Rocket& rocket = stage.rocket;
        x0 = {rocket.position.ECI(0), rocket.position.ECI(1), rocket.position.ECI(2),
                rocket.velocity.ECI(0),rocket.velocity.ECI(1),rocket.velocity.ECI(2),
                rocket.attitude.quaternion(0),rocket.attitude.quaternion(1),rocket.attitude.quaternion(2),rocket.attitude.quaternion(3),
                rocket.angular_velocity(0),rocket.angular_velocity(1),rocket.angular_velocity(2),
                rocket.mass.propellant};
        stage.FlightSequence(&master_clock, &wind, x0);

        // Update next stage
        if (stage_vector.size() != i+1 && stage.enable_sepation) {
            RocketStage& next_stage = stage_vector[i+1];

            next_stage.time_start = stage.time_separation;

            next_stage.rocket.position.ECI(0) = x0[0];
            next_stage.rocket.position.ECI(1) = x0[1];
            next_stage.rocket.position.ECI(2) = x0[2];
            next_stage.rocket.velocity.ECI(0) = x0[3];
            next_stage.rocket.velocity.ECI(1) = x0[4];
            next_stage.rocket.velocity.ECI(2) = x0[5];
            next_stage.rocket.attitude.quaternion(0) = x0[6];
            next_stage.rocket.attitude.quaternion(1) = x0[7];
            next_stage.rocket.attitude.quaternion(2) = x0[8];
            next_stage.rocket.attitude.quaternion(3) = x0[9];
            next_stage.rocket.angular_velocity(0) = x0[10];
            next_stage.rocket.angular_velocity(1) = x0[11];
            next_stage.rocket.angular_velocity(2) = x0[12];
        }
    }
    
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x88, %rsp
movq %rdi, %rbx
leaq 0x40(%rdi), %rax
movq %rax, 0x8(%rsp)
leaq 0x88(%rdi), %r15
movq 0x28(%rdi), %r14
movq 0x30(%rdi), %rcx
xorl %r13d, %r13d
movl $0x1210, %ebp           # imm = 0x1210
xorl %r12d, %r12d
subq %r14, %rcx
movq %rcx, %rax
cqto
idivq %rbp
cmpq %r12, %rax
jbe 0x2abb1
leaq (%r14,%r13), %rdi
movsd 0x2c0(%r14,%r13), %xmm0
movsd 0x308(%r14,%r13), %xmm1
movsd 0x450(%r14,%r13), %xmm2
movsd 0x228(%r14,%r13), %xmm3
movups 0x2b0(%r14,%r13), %xmm4
movups 0x2f8(%r14,%r13), %xmm5
movups 0x400(%r14,%r13), %xmm6
movups 0x410(%r14,%r13), %xmm7
movups 0x440(%r14,%r13), %xmm8
movaps %xmm4, 0x10(%rsp)
movsd %xmm0, 0x20(%rsp)
movups %xmm5, 0x28(%rsp)
movsd %xmm1, 0x38(%rsp)
movaps %xmm6, 0x40(%rsp)
movaps %xmm7, 0x50(%rsp)
movaps %xmm8, 0x60(%rsp)
movsd %xmm2, 0x70(%rsp)
movsd %xmm3, 0x78(%rsp)
movq 0x8(%rsp), %rsi
movq %r15, %rdx
leaq 0x10(%rsp), %rcx
callq 0x26566
movq 0x28(%rbx), %rsi
movq 0x30(%rbx), %rcx
movq %rcx, %rax
subq %rsi, %rax
cqto
idivq %rbp
incq %r12
cmpq %rax, %r12
je 0x2aba2
cmpb $0x1, 0x1198(%r14,%r13)
jne 0x2aba2
movsd 0x11a0(%r14,%r13), %xmm0
movsd %xmm0, 0x2360(%rsi,%r13)
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x14c0(%rsi,%r13)
movsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x14c8(%rsi,%r13)
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0x14d0(%rsi,%r13)
movsd 0x28(%rsp), %xmm0
movsd %xmm0, 0x1508(%rsi,%r13)
movsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x1510(%rsi,%r13)
movsd 0x38(%rsp), %xmm0
movsd %xmm0, 0x1518(%rsi,%r13)
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x1610(%rsi,%r13)
movsd 0x48(%rsp), %xmm0
movsd %xmm0, 0x1618(%rsi,%r13)
movsd 0x50(%rsp), %xmm0
movsd %xmm0, 0x1620(%rsi,%r13)
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x1628(%rsi,%r13)
movsd 0x60(%rsp), %xmm0
movsd %xmm0, 0x1650(%rsi,%r13)
movsd 0x68(%rsp), %xmm0
movsd %xmm0, 0x1658(%rsi,%r13)
movsd 0x70(%rsp), %xmm0
movsd %xmm0, 0x1660(%rsi,%r13)
addq $0x1210, %r13           # imm = 0x1210
movq %rsi, %r14
jmp 0x2a9df
addq $0x88, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop