// File: DavidLBruce[P]sim3dof/build/../src/Guidance.cpp
// Function: Guidance::FrameUpdate()
void Guidance::FrameUpdate()
{
  double axl;     // m/sec^2 Absolute target acceleration in LOS frame
  double ayl;     // m/sec^2 Absolute target acceleration in LOS frame
  double cth;     // na      Cosine of LOS angle
  double den;     // na      Intermediate variable
  double estPosYdot; // m/sec Estimated cross range rate
  double jxl;     // m/sec^3 Absolute target jerk in LOS frame
  double jyl;     // m/sec^3 Absolute target jerk in LOS frame
  double mslAxl;  // m/sec^2 Missile acceleration in LOS frame
  double mslAyl;  // m/sec^2 Missile acceleration in LOS frame
  double num;     // na      Intermediate variable
  double omega;   // rad/sec Weave radian frequency
  double omega2;  // na      Square of omega        
  double omega3;  // na      Cube of omega
  double pxl;     // m       Relative position in LOS frame
  double pyl;     // m       Relative position in LOS frame
  double sth;     // na      Sine of LOS angle
  double tgtJxf;  // m/sec^3 Target absolute jerk
  double tgtJyf;  // m/sec^3 Target absolute jerk
  double vxl;     // m/sec   Relative velocity in LOS frame
  double vyl;     // m/sec   Relative velocity in LOS frame
  double xx;      // na      Intermediate variable
  double x2;      // na      Intermediate variable
  double x3;      // na      Intermediate variable

  simTime = GetSimTime();

  if ( useTruth )
  {
    estAccX = pMslTse->truAccX;
    estAccY = pMslTse->truAccY;
    estLosAng = pMslTse->truLosAng;
    estLosr = pMslTse->truLosr;
    estRng = pMslTse->truRng;
    estRdot = pMslTse->truRdot;

    tgtPxf = pTgt->pxf;
    tgtPyf = pTgt->pyf;

    tgtVxf = pTgt->vxf;
    tgtVyf = pTgt->vyf;

    tgtAxf = pTgt->axf;
    tgtAyf = pTgt->ayf;

    tgtJxf = pTgt->jxf;
    tgtJyf = pTgt->jxf;

    omega = pTgt->omega;
  }
  else
  {
    tgtPxf = pMslTse->estTgtPxf;
    tgtPyf = pMslTse->estTgtPyf;
    
    tgtVxf = pMslTse->estTgtVxf;
    tgtVyf = pMslTse->estTgtVyf;
    
    tgtAxf = pMslTse->estTgtAxf;
    tgtAyf = pMslTse->estTgtAxf;

    omega = pMslTse->dlOmega;

    omega2 = pMslTse->dlOmega2;

    estAccX = pMslTse->estAccX;
    estAccY = pMslTse->estAccY;
    estLosAng = pMslTse->estLosAng;
    estLosr = pMslTse->estLosr;
    estRng = pMslTse->estRng;
    estRdot = pMslTse->estRdot;

    tgtJxf = pMslTse->estTgtJxf;
    tgtJyf = pMslTse->estTgtJyf;
  }

  //
  // Calculate target state wrt missile
  //
  // Using truth for missile state
  //
  relPxf = tgtPxf - pMsl->pxf;
  relPyf = tgtPyf - pMsl->pyf;

  relVxf = tgtVxf - pMsl->vxf;
  relVyf = tgtVyf - pMsl->vyf;

  if ( estRdot < 0.0 )
  {
    velClose = -estRdot;

    tgo = -estRng / estRdot;
  }
  // Else leave tgo & Vc at the last calculation

  switch ( gdnLawSel )
  {
    case glPN:
    {
      //
      // Lateral proportional navigation command.  Note 
      // inertial losr = body losr in this planar sim.
      // The body frame and the inertial frame are in the same plane.
      //
      aCmdYb = kpn * velClose * estLosr;

      break;

    }  // end case glPN:

    case glWEAVE:
    {
      //
      // Optimal weaving target guide law from Zarchan
      //
 
      //
      // Rotate variables into LOS frame     
      //
      cth = cos( estLosAng );
      sth = sin( estLosAng );

      pxl = relPxf * cth - relPyf * sth;
      pyl = relPxf * sth + relPyf * cth;
      pyl = relPyf;

      vxl = relVxf * cth - relPyf * sth;
      vyl = relVxf * sth + relPyf * cth;
      vyl = relVyf;

      axl = tgtAxf * cth - tgtAyf * sth;
      ayl = tgtAxf * sth + tgtAyf * cth;

      jxl = tgtJxf * cth - tgtJyf * sth;
      jyl = tgtJxf * sth + tgtJyf * cth;

      mslAxl = pMsl->axf * cth - pMsl->ayf * sth;
      mslAyl = pMsl->axf * sth + pMsl->ayf * cth;

      xx = tgo / tauCtrl;

      x2 = xx * xx;
      x3 = x2 * xx;

      num = 6.0*x2 * ( exp(-xx) - 1 + xx );

      den = 2.0*x3 - 6.0*x2 + 6.0*xx + 3.0 - 
            12.0*xx * exp(-xx) - 3.0 * exp(-2.0*xx);

      kWeave = num / den;

      //
      // Position and velocity terms are relative.  Accelearation and jerk terms
      // are absolute.
      //
      aCmdYb = pyl + tgo * vyl;

      omega2 = omega * omega;
      omega3 = omega2 * omega;

      aCmdYb += ( 1.0 - cos(omega * tgo ) ) * ayl / omega2;

      aCmdYb += ( omega * tgo - sin(omega * tgo) ) * jyl / omega3;

      aCmdYb -= mslAyl * tauCtrl*tauCtrl * ( exp(-xx) - 1 + xx );

      aCmdYb *= kWeave / ( tgo * tgo );

      break;

    }  // end case glWEAVE:
  }  // end switch ( gdnLawSel )
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x110, %rsp            # imm = 0x110
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0xc8(%rbp)
callq 0x3c40
movq -0xc8(%rbp), %rax
movsd %xmm0, 0x198(%rax)
testb $0x1, 0x110(%rax)
je 0xb6ad
movq -0xc8(%rbp), %rax
movq 0x1f0(%rax), %rcx
movsd 0x1b0(%rcx), %xmm0
movsd %xmm0, 0x130(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x1b8(%rcx), %xmm0
movsd %xmm0, 0x138(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x1d0(%rcx), %xmm0
movsd %xmm0, 0x140(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x1d8(%rcx), %xmm0
movsd %xmm0, 0x148(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x1e0(%rcx), %xmm0
movsd %xmm0, 0x150(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x1e8(%rcx), %xmm0
movsd %xmm0, 0x158(%rax)
movq 0x200(%rax), %rcx
movsd 0x160(%rcx), %xmm0
movsd %xmm0, 0x1b0(%rax)
movq 0x200(%rax), %rcx
movsd 0x168(%rcx), %xmm0
movsd %xmm0, 0x1b8(%rax)
movq 0x200(%rax), %rcx
movsd 0x180(%rcx), %xmm0
movsd %xmm0, 0x1c0(%rax)
movq 0x200(%rax), %rcx
movsd 0x190(%rcx), %xmm0
movsd %xmm0, 0x1c8(%rax)
movq 0x200(%rax), %rcx
movsd 0x130(%rcx), %xmm0
movsd %xmm0, 0x1a0(%rax)
movq 0x200(%rax), %rcx
movsd 0x140(%rcx), %xmm0
movsd %xmm0, 0x1a8(%rax)
movq 0x200(%rax), %rcx
movsd 0x148(%rcx), %xmm0
movsd %xmm0, -0x90(%rbp)
movq 0x200(%rax), %rcx
movsd 0x148(%rcx), %xmm0
movsd %xmm0, -0x98(%rbp)
movq 0x200(%rax), %rax
movsd 0x158(%rax), %xmm0
movsd %xmm0, -0x60(%rbp)
jmp 0xb81e
movq -0xc8(%rbp), %rax
movq 0x1f0(%rax), %rcx
movsd 0x168(%rcx), %xmm0
movsd %xmm0, 0x1b0(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x170(%rcx), %xmm0
movsd %xmm0, 0x1b8(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x178(%rcx), %xmm0
movsd %xmm0, 0x1c0(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x180(%rcx), %xmm0
movsd %xmm0, 0x1c8(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x188(%rcx), %xmm0
movsd %xmm0, 0x1a0(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x188(%rcx), %xmm0
movsd %xmm0, 0x1a8(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x208(%rcx), %xmm0
movsd %xmm0, -0x60(%rbp)
movq 0x1f0(%rax), %rcx
movsd 0x210(%rcx), %xmm0
movsd %xmm0, -0x68(%rbp)
movq 0x1f0(%rax), %rcx
movsd 0x138(%rcx), %xmm0
movsd %xmm0, 0x130(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x140(%rcx), %xmm0
movsd %xmm0, 0x138(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x148(%rcx), %xmm0
movsd %xmm0, 0x140(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x150(%rcx), %xmm0
movsd %xmm0, 0x148(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x158(%rcx), %xmm0
movsd %xmm0, 0x150(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x160(%rcx), %xmm0
movsd %xmm0, 0x158(%rax)
movq 0x1f0(%rax), %rcx
movsd 0x198(%rcx), %xmm0
movsd %xmm0, -0x90(%rbp)
movq 0x1f0(%rax), %rax
movsd 0x1a0(%rax), %xmm0
movsd %xmm0, -0x98(%rbp)
movq -0xc8(%rbp), %rax
movsd 0x1b0(%rax), %xmm0
movq 0x1e8(%rax), %rcx
subsd 0x130(%rcx), %xmm0
movsd %xmm0, 0x178(%rax)
movsd 0x1b8(%rax), %xmm0
movq 0x1e8(%rax), %rcx
subsd 0x138(%rcx), %xmm0
movsd %xmm0, 0x180(%rax)
movsd 0x1c0(%rax), %xmm0
movq 0x1e8(%rax), %rcx
subsd 0x150(%rcx), %xmm0
movsd %xmm0, 0x188(%rax)
movsd 0x1c8(%rax), %xmm0
movq 0x1e8(%rax), %rcx
subsd 0x160(%rcx), %xmm0
movsd %xmm0, 0x190(%rax)
xorps %xmm0, %xmm0
ucomisd 0x158(%rax), %xmm0
jbe 0xb90b
movq -0xc8(%rbp), %rax
movsd 0x158(%rax), %xmm0
movq %xmm0, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm0
movsd %xmm0, 0x1d0(%rax)
movsd 0x150(%rax), %xmm0
movq %xmm0, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm0
divsd 0x158(%rax), %xmm0
movsd %xmm0, 0x128(%rax)
movq -0xc8(%rbp), %rax
movl 0x160(%rax), %eax
movl %eax, -0xcc(%rbp)
subl $0x1, %eax
je 0xb935
jmp 0xb925
movl -0xcc(%rbp), %eax
subl $0x2, %eax
je 0xb961
jmp 0xbf8c
movq -0xc8(%rbp), %rax
movsd 0x108(%rax), %xmm0
mulsd 0x1d0(%rax), %xmm0
mulsd 0x148(%rax), %xmm0
movsd %xmm0, 0x120(%rax)
jmp 0xbf8c
movq -0xc8(%rbp), %rax
movsd 0x140(%rax), %xmm0
callq 0x3430
movq -0xc8(%rbp), %rax
movsd %xmm0, -0x20(%rbp)
movsd 0x140(%rax), %xmm0
callq 0x30a0
movq -0xc8(%rbp), %rax
movsd %xmm0, -0x88(%rbp)
movsd 0x178(%rax), %xmm0
movsd -0x20(%rbp), %xmm2
movsd 0x180(%rax), %xmm1
mulsd -0x88(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x78(%rbp)
movsd 0x178(%rax), %xmm0
movsd -0x88(%rbp), %xmm2
movsd 0x180(%rax), %xmm1
mulsd -0x20(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x80(%rbp)
movsd 0x180(%rax), %xmm0
movsd %xmm0, -0x80(%rbp)
movsd 0x188(%rax), %xmm0
movsd -0x20(%rbp), %xmm2
movsd 0x180(%rax), %xmm1
mulsd -0x88(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xa0(%rbp)
movsd 0x188(%rax), %xmm0
movsd -0x88(%rbp), %xmm2
movsd 0x180(%rax), %xmm1
mulsd -0x20(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xa8(%rbp)
movsd 0x190(%rax), %xmm0
movsd %xmm0, -0xa8(%rbp)
movsd 0x1a0(%rax), %xmm0
movsd -0x20(%rbp), %xmm2
movsd 0x1a8(%rax), %xmm1
mulsd -0x88(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x10(%rbp)
movsd 0x1a0(%rax), %xmm0
movsd -0x88(%rbp), %xmm2
movsd 0x1a8(%rax), %xmm1
mulsd -0x20(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x18(%rbp)
movsd -0x90(%rbp), %xmm0
movsd -0x20(%rbp), %xmm2
movsd -0x98(%rbp), %xmm1
mulsd -0x88(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x38(%rbp)
movsd -0x90(%rbp), %xmm0
movsd -0x88(%rbp), %xmm2
movsd -0x98(%rbp), %xmm1
mulsd -0x20(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x40(%rbp)
movq 0x1e8(%rax), %rcx
movsd 0x118(%rcx), %xmm0
movsd -0x20(%rbp), %xmm2
movq 0x1e8(%rax), %rcx
movsd 0x128(%rcx), %xmm1
mulsd -0x88(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x48(%rbp)
movq 0x1e8(%rax), %rcx
movsd 0x118(%rcx), %xmm0
movsd -0x88(%rbp), %xmm2
movq 0x1e8(%rax), %rcx
movsd 0x128(%rcx), %xmm1
mulsd -0x20(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x50(%rbp)
movsd 0x128(%rax), %xmm0
divsd 0x118(%rax), %xmm0
movsd %xmm0, -0xb0(%rbp)
movsd -0xb0(%rbp), %xmm0
mulsd -0xb0(%rbp), %xmm0
movsd %xmm0, -0xb8(%rbp)
movsd -0xb8(%rbp), %xmm0
mulsd -0xb0(%rbp), %xmm0
movsd %xmm0, -0xc0(%rbp)
movsd 0x188c5(%rip), %xmm0    # 0x24508
mulsd -0xb8(%rbp), %xmm0
movsd %xmm0, -0x108(%rbp)
movsd -0xb0(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
callq 0x3400
movaps %xmm0, %xmm1
movsd -0x108(%rbp), %xmm0
movsd 0x1837e(%rip), %xmm2    # 0x24008
subsd %xmm2, %xmm1
addsd -0xb0(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x58(%rbp)
movsd -0xc0(%rbp), %xmm1
movsd 0x18859(%rip), %xmm0    # 0x24508
mulsd -0xb8(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
addsd %xmm1, %xmm1
addsd %xmm0, %xmm1
movsd -0xb0(%rbp), %xmm0
movsd 0x18822(%rip), %xmm2    # 0x24508
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd 0x1832a(%rip), %xmm1    # 0x24020
addsd %xmm1, %xmm0
movsd %xmm0, -0x100(%rbp)
movsd 0x187f6(%rip), %xmm0    # 0x24500
mulsd -0xb0(%rbp), %xmm0
movsd %xmm0, -0xf8(%rbp)
movsd -0xb0(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
callq 0x3400
movsd -0x100(%rbp), %xmm1
movaps %xmm0, %xmm2
movsd -0xf8(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xf0(%rbp)
movsd 0x184a8(%rip), %xmm0    # 0x24228
mulsd -0xb0(%rbp), %xmm0
callq 0x3400
movsd -0xf0(%rbp), %xmm1
movq -0xc8(%rbp), %rax
movsd 0x18754(%rip), %xmm2    # 0x244f8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x28(%rbp)
movsd -0x58(%rbp), %xmm0
divsd -0x28(%rbp), %xmm0
movsd %xmm0, 0x168(%rax)
movsd -0x80(%rbp), %xmm1
movsd 0x128(%rax), %xmm0
movsd -0xa8(%rbp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x120(%rax)
movsd -0x60(%rbp), %xmm0
mulsd -0x60(%rbp), %xmm0
movsd %xmm0, -0x68(%rbp)
movsd -0x68(%rbp), %xmm0
mulsd -0x60(%rbp), %xmm0
movsd %xmm0, -0x70(%rbp)
movsd -0x60(%rbp), %xmm0
mulsd 0x128(%rax), %xmm0
callq 0x3430
movq -0xc8(%rbp), %rax
movaps %xmm0, %xmm1
movsd 0x181de(%rip), %xmm0    # 0x24008
subsd %xmm1, %xmm0
mulsd -0x18(%rbp), %xmm0
divsd -0x68(%rbp), %xmm0
addsd 0x120(%rax), %xmm0
movsd %xmm0, 0x120(%rax)
movsd -0x60(%rbp), %xmm0
movsd %xmm0, -0xe0(%rbp)
movsd 0x128(%rax), %xmm0
movsd %xmm0, -0xe8(%rbp)
movsd -0x60(%rbp), %xmm0
mulsd 0x128(%rax), %xmm0
callq 0x30a0
movsd -0xe8(%rbp), %xmm2
movq -0xc8(%rbp), %rax
movaps %xmm0, %xmm1
movsd -0xe0(%rbp), %xmm0
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
mulsd -0x40(%rbp), %xmm0
divsd -0x70(%rbp), %xmm0
addsd 0x120(%rax), %xmm0
movsd %xmm0, 0x120(%rax)
movsd -0x50(%rbp), %xmm0
mulsd 0x118(%rax), %xmm0
mulsd 0x118(%rax), %xmm0
movsd %xmm0, -0xd8(%rbp)
movsd -0xb0(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
callq 0x3400
movq -0xc8(%rbp), %rax
movaps %xmm0, %xmm2
movsd -0xd8(%rbp), %xmm0
movsd 0x180e3(%rip), %xmm1    # 0x24008
subsd %xmm1, %xmm2
addsd -0xb0(%rbp), %xmm2
movsd 0x120(%rax), %xmm1
movq %xmm0, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x120(%rax)
movsd 0x168(%rax), %xmm0
movsd 0x128(%rax), %xmm1
mulsd 0x128(%rax), %xmm1
divsd %xmm1, %xmm0
mulsd 0x120(%rax), %xmm0
movsd %xmm0, 0x120(%rax)
addq $0x110, %rsp            # imm = 0x110
popq %rbp
retq
nopw %cs:(%rax,%rax)