// File: 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
movq %rsp, %rbp
subq $0x180, %rsp            # imm = 0x180
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x110(%rbp)
movl $0x0, -0x7c(%rbp)
movq -0x110(%rbp), %rdi
movslq -0x7c(%rbp), %rax
movq %rax, -0x118(%rbp)
addq $0x28, %rdi
callq 0x87a70
movq %rax, %rcx
movq -0x118(%rbp), %rax
cmpq %rcx, %rax
jae 0x871f6
movq -0x110(%rbp), %rdi
addq $0x28, %rdi
movslq -0x7c(%rbp), %rsi
callq 0x87890
movq %rax, -0x88(%rbp)
movq -0x88(%rbp), %rax
addq $0x10, %rax
movq %rax, -0x90(%rbp)
movq -0x90(%rbp), %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
xorl %eax, %eax
movl %eax, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0x100(%rbp)
movq -0x90(%rbp), %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x1, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xf8(%rbp)
movq -0x90(%rbp), %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x2, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xf0(%rbp)
movq -0x90(%rbp), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
xorl %eax, %eax
movl %eax, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xe8(%rbp)
movq -0x90(%rbp), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
movl $0x1, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xe0(%rbp)
movq -0x90(%rbp), %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
movl $0x2, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xd8(%rbp)
movq -0x90(%rbp), %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x2f870
movsd (%rax), %xmm0
movsd %xmm0, -0xd0(%rbp)
movq -0x90(%rbp), %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
movl $0x1, %esi
callq 0x2f870
movsd (%rax), %xmm0
movsd %xmm0, -0xc8(%rbp)
movq -0x90(%rbp), %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
movl $0x2, %esi
callq 0x2f870
movsd (%rax), %xmm0
movsd %xmm0, -0xc0(%rbp)
movq -0x90(%rbp), %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
movl $0x3, %esi
callq 0x2f870
movsd (%rax), %xmm0
movsd %xmm0, -0xb8(%rbp)
movq -0x90(%rbp), %rdi
addq $0x430, %rdi            # imm = 0x430
xorl %eax, %eax
movl %eax, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xb0(%rbp)
movq -0x90(%rbp), %rdi
addq $0x430, %rdi            # imm = 0x430
movl $0x1, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xa8(%rbp)
movq -0x90(%rbp), %rdi
addq $0x430, %rdi            # imm = 0x430
movl $0x2, %esi
callq 0x372d0
movsd (%rax), %xmm0
movsd %xmm0, -0xa0(%rbp)
movq -0x90(%rbp), %rax
movsd 0x218(%rax), %xmm0
movsd %xmm0, -0x98(%rbp)
leaq -0x78(%rbp), %rdi
leaq -0x100(%rbp), %rsi
movl $0x70, %edx
callq 0x63a0
movq -0x110(%rbp), %rdx
movq -0x88(%rbp), %rdi
movq %rdx, %rsi
addq $0x40, %rsi
addq $0x88, %rdx
leaq -0x78(%rbp), %rcx
callq 0x7a760
movq -0x110(%rbp), %rdi
addq $0x28, %rdi
callq 0x87a70
movl -0x7c(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
cmpq %rcx, %rax
je 0x871e6
movq -0x88(%rbp), %rax
testb $0x1, 0x1198(%rax)
je 0x871e6
movq -0x110(%rbp), %rdi
addq $0x28, %rdi
movl -0x7c(%rbp), %eax
addl $0x1, %eax
movslq %eax, %rsi
callq 0x87890
movq %rax, -0x108(%rbp)
movq -0x88(%rbp), %rax
movsd 0x11a0(%rax), %xmm0
movq -0x108(%rbp), %rax
movsd %xmm0, 0x1150(%rax)
leaq -0x78(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x180(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
xorl %eax, %eax
movl %eax, %esi
callq 0x372d0
movsd -0x180(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x1, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x178(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x1, %esi
callq 0x372d0
movsd -0x178(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x2, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x170(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x2, %esi
callq 0x372d0
movsd -0x170(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x3, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x168(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
xorl %eax, %eax
movl %eax, %esi
callq 0x372d0
movsd -0x168(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x4, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x160(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
movl $0x1, %esi
callq 0x372d0
movsd -0x160(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x5, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x158(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x2e8, %rdi            # imm = 0x2E8
movl $0x2, %esi
callq 0x372d0
movsd -0x158(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x6, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x150(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x2f870
movsd -0x150(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x7, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x148(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
movl $0x1, %esi
callq 0x2f870
movsd -0x148(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x8, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x140(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
movl $0x2, %esi
callq 0x2f870
movsd -0x140(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0x9, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x138(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
addq $0x20, %rdi
movl $0x3, %esi
callq 0x2f870
movsd -0x138(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0xa, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x130(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x430, %rdi            # imm = 0x430
xorl %eax, %eax
movl %eax, %esi
callq 0x372d0
movsd -0x130(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0xb, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x128(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x430, %rdi            # imm = 0x430
movl $0x1, %esi
callq 0x372d0
movsd -0x128(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x78(%rbp), %rdi
movl $0xc, %esi
callq 0x5b120
movsd (%rax), %xmm0
movsd %xmm0, -0x120(%rbp)
movq -0x108(%rbp), %rdi
addq $0x10, %rdi
addq $0x430, %rdi            # imm = 0x430
movl $0x2, %esi
callq 0x372d0
movsd -0x120(%rbp), %xmm0
movsd %xmm0, (%rax)
jmp 0x871e8
movl -0x7c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x7c(%rbp)
jmp 0x86b81
addq $0x180, %rsp            # imm = 0x180
popq %rbp
retq
nop