// File: DavidLBruce[P]sim3dof/build/../src/MdlBase.cpp
// Function: MdlBase::Integrate(double&)
void MdlBase::Integrate(double   &timeStep )
{
  //   
  // Determine step interval and step dependent values
  //
  double hh = timeStep;
  double h_half = hh / 2.0;
  double tempSimTime = *pSimTime;

  //
  // Define local derivaitve and state containers
  //
  std::vector<double *> pDeriv;
  std::vector<double *> pState;

  //
  // Get current states and store values
  //
  GetStates( pState );

  if ( !initComp )
  {
    initComp = true;

    //
    // Allocate container memory with sizes of state vector
    //
    xn.insert( xn.end(), pState.size(), 0.0 );
    k1.insert( k1.end(), pState.size(), 0.0 );
    k2.insert( k2.end(), pState.size(), 0.0 );
    k3.insert( k3.end(), pState.size(), 0.0 );
    k4.insert( k4.end(), pState.size(), 0.0 );
  }

  for( unsigned int idx = 0; idx < pState.size(); ++idx )
  {
     xn[idx] = *pState[idx];
  }

  //
  // Integrate
  // Step 1
  //
  UpdateDerivatives();
  GetDerivatives( pDeriv );

  for( unsigned int idx = 0; idx < pDeriv.size(); ++idx )
  {
    k1[idx] = *pDeriv[idx] * hh;

    //
    // Compute states for next step while we are here
    //
    *pState[idx] = xn[idx] + k1[idx] / 2.0;
  }

  // 
  // Advance clock half step for Stage 2 and Stage 3
  //
  *pSimTime += h_half;

  //
  // Step 2
  // Apply state step and get new derivatives
  //
  SetStates(pState);
  UpdateDerivatives();
  GetDerivatives(pDeriv);

  for( unsigned int idx = 0; idx < pDeriv.size(); ++idx )
  {
    k2[idx] = *pDeriv[idx] * hh;

    //
    // Compute states for next step while we are here
    //
    *pState[idx] = xn[idx] + k2[idx]/2.0;
  }

  //
  // Step 3
  // Apply state step and get new derivatives
  //
  SetStates(pState);
  UpdateDerivatives();
  GetDerivatives(pDeriv);

  for(unsigned int idx = 0; idx < pDeriv.size(); ++idx)
  {
    k3[idx] = *pDeriv[idx]*hh;

    //
    // Compute states for next step while we are here
    //
    *pState[idx] = xn[idx] + k3[idx];
  }

  //
  // Advance clock another half step (for a complete step) for Stage 4
  //
  *pSimTime += h_half;

  //
  // Step 4
  // Apply state step and get new derivatives
  //
  SetStates(pState);
  UpdateDerivatives();
  GetDerivatives(pDeriv);

  for(unsigned int idx = 0; idx < pDeriv.size(); ++idx)
  {
    k4[idx] = *pDeriv[idx]*hh;

    //
    // Compute states
    //
    *pState[idx] = xn[idx] + k1[idx]/6.0 + k2[idx]/3.0 + k3[idx]/3.0 + 
                                                               k4[idx]/6.0;
  }
  SetStates(pState);

  //
  // Reset clock for this module
  //
  *pSimTime = tempSimTime;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x230, %rsp            # imm = 0x230
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x130(%rbp)
movq -0x10(%rbp), %rcx
movsd (%rcx), %xmm0
movsd %xmm0, -0x18(%rbp)
movsd -0x18(%rbp), %xmm0
movsd 0x16b70(%rip), %xmm1    # 0x245d8
mulsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
movq 0x38(%rax), %rax
movsd (%rax), %xmm0
movsd %xmm0, -0x28(%rbp)
leaq -0x40(%rbp), %rdi
callq 0xe400
leaq -0x58(%rbp), %rdi
movq %rdi, -0x128(%rbp)
callq 0xe400
movq -0x130(%rbp), %rdi
movq -0x128(%rbp), %rsi
callq 0xd8e0
jmp 0xdaac
movq -0x130(%rbp), %rax
testb $0x1, 0xf0(%rax)
jne 0xdd43
movq -0x130(%rbp), %rdi
movb $0x1, 0xf0(%rdi)
addq $0x78, %rdi
movq %rdi, -0x140(%rbp)
callq 0xe8a0
movq %rax, -0x78(%rbp)
leaq -0x70(%rbp), %rdi
leaq -0x78(%rbp), %rsi
callq 0xe8d0
leaq -0x58(%rbp), %rdi
callq 0xe750
movq -0x140(%rbp), %rdi
movq %rax, %rdx
movq $0x0, -0x80(%rbp)
movq -0x70(%rbp), %rsi
leaq -0x80(%rbp), %rcx
callq 0xe800
movq %rax, -0x138(%rbp)
jmp 0xdb20
movq -0x130(%rbp), %rdi
movq -0x138(%rbp), %rax
movq %rax, -0x88(%rbp)
addq $0x90, %rdi
movq %rdi, -0x150(%rbp)
callq 0xe8a0
movq %rax, -0x98(%rbp)
leaq -0x90(%rbp), %rdi
leaq -0x98(%rbp), %rsi
callq 0xe8d0
leaq -0x58(%rbp), %rdi
callq 0xe750
movq -0x150(%rbp), %rdi
movq %rax, %rdx
movq $0x0, -0xa0(%rbp)
movq -0x90(%rbp), %rsi
leaq -0xa0(%rbp), %rcx
callq 0xe800
movq %rax, -0x148(%rbp)
jmp 0xdb9c
movq -0x130(%rbp), %rdi
movq -0x148(%rbp), %rax
movq %rax, -0xa8(%rbp)
addq $0xa8, %rdi
movq %rdi, -0x160(%rbp)
callq 0xe8a0
movq %rax, -0xb8(%rbp)
leaq -0xb0(%rbp), %rdi
leaq -0xb8(%rbp), %rsi
callq 0xe8d0
leaq -0x58(%rbp), %rdi
callq 0xe750
movq -0x160(%rbp), %rdi
movq %rax, %rdx
movq $0x0, -0xc0(%rbp)
movq -0xb0(%rbp), %rsi
leaq -0xc0(%rbp), %rcx
callq 0xe800
movq %rax, -0x158(%rbp)
jmp 0xdc18
movq -0x130(%rbp), %rdi
movq -0x158(%rbp), %rax
movq %rax, -0xc8(%rbp)
addq $0xc0, %rdi
movq %rdi, -0x170(%rbp)
callq 0xe8a0
movq %rax, -0xd8(%rbp)
leaq -0xd0(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0xe8d0
leaq -0x58(%rbp), %rdi
callq 0xe750
movq -0x170(%rbp), %rdi
movq %rax, %rdx
movq $0x0, -0xe0(%rbp)
movq -0xd0(%rbp), %rsi
leaq -0xe0(%rbp), %rcx
callq 0xe800
movq %rax, -0x168(%rbp)
jmp 0xdc94
movq -0x130(%rbp), %rdi
movq -0x168(%rbp), %rax
movq %rax, -0xe8(%rbp)
addq $0xd8, %rdi
movq %rdi, -0x180(%rbp)
callq 0xe8a0
movq %rax, -0xf8(%rbp)
leaq -0xf0(%rbp), %rdi
leaq -0xf8(%rbp), %rsi
callq 0xe8d0
leaq -0x58(%rbp), %rdi
callq 0xe750
movq -0x180(%rbp), %rdi
movq %rax, %rdx
movq $0x0, -0x100(%rbp)
movq -0xf0(%rbp), %rsi
leaq -0x100(%rbp), %rcx
callq 0xe800
movq %rax, -0x178(%rbp)
jmp 0xdd10
movq -0x178(%rbp), %rax
movq %rax, -0x108(%rbp)
jmp 0xdd43
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0x58(%rbp), %rdi
callq 0xe4a0
leaq -0x40(%rbp), %rdi
callq 0xe4a0
jmp 0xe3f5
movl $0x0, -0x10c(%rbp)
movl -0x10c(%rbp), %eax
movq %rax, -0x188(%rbp)
leaq -0x58(%rbp), %rdi
callq 0xe750
movq %rax, %rcx
movq -0x188(%rbp), %rax
cmpq %rcx, %rax
jae 0xddc7
movl -0x10c(%rbp), %eax
movl %eax, %esi
leaq -0x58(%rbp), %rdi
callq 0xe770
movq -0x130(%rbp), %rdi
movq (%rax), %rax
movsd (%rax), %xmm0
movsd %xmm0, -0x190(%rbp)
addq $0x78, %rdi
movl -0x10c(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x190(%rbp), %xmm0
movsd %xmm0, (%rax)
movl -0x10c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x10c(%rbp)
jmp 0xdd4d
movq -0x130(%rbp), %rdi
movq (%rdi), %rax
movq 0x18(%rax), %rax
callq *%rax
jmp 0xddd9
movq -0x130(%rbp), %rdi
leaq -0x40(%rbp), %rsi
callq 0xd910
jmp 0xddeb
movl $0x0, -0x110(%rbp)
movl -0x110(%rbp), %eax
movq %rax, -0x198(%rbp)
leaq -0x40(%rbp), %rdi
callq 0xe750
movq %rax, %rcx
movq -0x198(%rbp), %rax
cmpq %rcx, %rax
jae 0xdf01
movl -0x110(%rbp), %eax
movl %eax, %esi
leaq -0x40(%rbp), %rdi
callq 0xe770
movq -0x130(%rbp), %rdi
movq (%rax), %rax
movsd (%rax), %xmm0
mulsd -0x18(%rbp), %xmm0
movsd %xmm0, -0x1b0(%rbp)
addq $0x90, %rdi
movl -0x110(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x1b0(%rbp), %xmm0
movq -0x130(%rbp), %rdi
movsd %xmm0, (%rax)
addq $0x78, %rdi
movl -0x110(%rbp), %eax
movl %eax, %esi
callq 0xe910
movq -0x130(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x1a8(%rbp)
addq $0x90, %rdi
movl -0x110(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x1a8(%rbp), %xmm0
movsd (%rax), %xmm1
movsd 0x1679b(%rip), %xmm2    # 0x24658
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x1a0(%rbp)
movl -0x110(%rbp), %eax
movl %eax, %esi
leaq -0x58(%rbp), %rdi
callq 0xe770
movsd -0x1a0(%rbp), %xmm0
movq (%rax), %rax
movsd %xmm0, (%rax)
movl -0x110(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x110(%rbp)
jmp 0xddf5
movq -0x130(%rbp), %rdi
movsd -0x20(%rbp), %xmm1
movq 0x38(%rdi), %rax
movsd (%rax), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rax)
leaq -0x58(%rbp), %rsi
callq 0xd940
movq -0x130(%rbp), %rdi
movq (%rdi), %rax
movq 0x18(%rax), %rax
callq *%rax
jmp 0xdf38
movq -0x130(%rbp), %rdi
leaq -0x40(%rbp), %rsi
callq 0xd910
jmp 0xdf4a
movl $0x0, -0x114(%rbp)
movl -0x114(%rbp), %eax
movq %rax, -0x1b8(%rbp)
leaq -0x40(%rbp), %rdi
callq 0xe750
movq %rax, %rcx
movq -0x1b8(%rbp), %rax
cmpq %rcx, %rax
jae 0xe060
movl -0x114(%rbp), %eax
movl %eax, %esi
leaq -0x40(%rbp), %rdi
callq 0xe770
movq -0x130(%rbp), %rdi
movq (%rax), %rax
movsd (%rax), %xmm0
mulsd -0x18(%rbp), %xmm0
movsd %xmm0, -0x1d0(%rbp)
addq $0xa8, %rdi
movl -0x114(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x1d0(%rbp), %xmm0
movq -0x130(%rbp), %rdi
movsd %xmm0, (%rax)
addq $0x78, %rdi
movl -0x114(%rbp), %eax
movl %eax, %esi
callq 0xe910
movq -0x130(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x1c8(%rbp)
addq $0xa8, %rdi
movl -0x114(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x1c8(%rbp), %xmm0
movsd (%rax), %xmm1
movsd 0x1663c(%rip), %xmm2    # 0x24658
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x1c0(%rbp)
movl -0x114(%rbp), %eax
movl %eax, %esi
leaq -0x58(%rbp), %rdi
callq 0xe770
movsd -0x1c0(%rbp), %xmm0
movq (%rax), %rax
movsd %xmm0, (%rax)
movl -0x114(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x114(%rbp)
jmp 0xdf54
movq -0x130(%rbp), %rdi
leaq -0x58(%rbp), %rsi
callq 0xd940
movq -0x130(%rbp), %rdi
movq (%rdi), %rax
movq 0x18(%rax), %rax
callq *%rax
jmp 0xe082
movq -0x130(%rbp), %rdi
leaq -0x40(%rbp), %rsi
callq 0xd910
jmp 0xe094
movl $0x0, -0x118(%rbp)
movl -0x118(%rbp), %eax
movq %rax, -0x1d8(%rbp)
leaq -0x40(%rbp), %rdi
callq 0xe750
movq %rax, %rcx
movq -0x1d8(%rbp), %rax
cmpq %rcx, %rax
jae 0xe19a
movl -0x118(%rbp), %eax
movl %eax, %esi
leaq -0x40(%rbp), %rdi
callq 0xe770
movq -0x130(%rbp), %rdi
movq (%rax), %rax
movsd (%rax), %xmm0
mulsd -0x18(%rbp), %xmm0
movsd %xmm0, -0x1f0(%rbp)
addq $0xc0, %rdi
movl -0x118(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x1f0(%rbp), %xmm0
movq -0x130(%rbp), %rdi
movsd %xmm0, (%rax)
addq $0x78, %rdi
movl -0x118(%rbp), %eax
movl %eax, %esi
callq 0xe910
movq -0x130(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x1e8(%rbp)
addq $0xc0, %rdi
movl -0x118(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x1e8(%rbp), %xmm0
addsd (%rax), %xmm0
movsd %xmm0, -0x1e0(%rbp)
movl -0x118(%rbp), %eax
movl %eax, %esi
leaq -0x58(%rbp), %rdi
callq 0xe770
movsd -0x1e0(%rbp), %xmm0
movq (%rax), %rax
movsd %xmm0, (%rax)
movl -0x118(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x118(%rbp)
jmp 0xe09e
movq -0x130(%rbp), %rdi
movsd -0x20(%rbp), %xmm1
movq 0x38(%rdi), %rax
movsd (%rax), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rax)
leaq -0x58(%rbp), %rsi
callq 0xd940
movq -0x130(%rbp), %rdi
movq (%rdi), %rax
movq 0x18(%rax), %rax
callq *%rax
jmp 0xe1d1
movq -0x130(%rbp), %rdi
leaq -0x40(%rbp), %rsi
callq 0xd910
jmp 0xe1e3
movl $0x0, -0x11c(%rbp)
movl -0x11c(%rbp), %eax
movq %rax, -0x1f8(%rbp)
leaq -0x40(%rbp), %rdi
callq 0xe750
movq %rax, %rcx
movq -0x1f8(%rbp), %rax
cmpq %rcx, %rax
jae 0xe3b6
movl -0x11c(%rbp), %eax
movl %eax, %esi
leaq -0x40(%rbp), %rdi
callq 0xe770
movq -0x130(%rbp), %rdi
movq (%rax), %rax
movsd (%rax), %xmm0
mulsd -0x18(%rbp), %xmm0
movsd %xmm0, -0x228(%rbp)
addq $0xd8, %rdi
movl -0x11c(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x228(%rbp), %xmm0
movq -0x130(%rbp), %rdi
movsd %xmm0, (%rax)
addq $0x78, %rdi
movl -0x11c(%rbp), %eax
movl %eax, %esi
callq 0xe910
movq -0x130(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x220(%rbp)
addq $0x90, %rdi
movl -0x11c(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x220(%rbp), %xmm0
movq -0x130(%rbp), %rdi
movsd (%rax), %xmm1
movsd 0x1624c(%rip), %xmm2    # 0x24508
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x218(%rbp)
addq $0xa8, %rdi
movl -0x11c(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x218(%rbp), %xmm0
movq -0x130(%rbp), %rdi
movsd (%rax), %xmm1
movsd 0x15d25(%rip), %xmm2    # 0x24020
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x210(%rbp)
addq $0xc0, %rdi
movl -0x11c(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x210(%rbp), %xmm0
movq -0x130(%rbp), %rdi
movsd (%rax), %xmm1
movsd 0x15ce6(%rip), %xmm2    # 0x24020
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x208(%rbp)
addq $0xd8, %rdi
movl -0x11c(%rbp), %eax
movl %eax, %esi
callq 0xe910
movsd -0x208(%rbp), %xmm0
movsd (%rax), %xmm1
movsd 0x16196(%rip), %xmm2    # 0x24508
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x200(%rbp)
movl -0x11c(%rbp), %eax
movl %eax, %esi
leaq -0x58(%rbp), %rdi
callq 0xe770
movsd -0x200(%rbp), %xmm0
movq (%rax), %rax
movsd %xmm0, (%rax)
movl -0x11c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x11c(%rbp)
jmp 0xe1ed
movq -0x130(%rbp), %rdi
leaq -0x58(%rbp), %rsi
callq 0xd940
movq -0x130(%rbp), %rax
movsd -0x28(%rbp), %xmm0
movq 0x38(%rax), %rax
movsd %xmm0, (%rax)
leaq -0x58(%rbp), %rdi
callq 0xe4a0
leaq -0x40(%rbp), %rdi
callq 0xe4a0
addq $0x230, %rsp            # imm = 0x230
popq %rbp
retq
movq -0x60(%rbp), %rdi
callq 0x33d0
nop