// File: /workspace/github_cpp_makefile_star2_O1/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
pushq %rbx
subq $0x88, %rsp
movq %rdi, %rbx
movq 0x38(%rdi), %rax
movsd (%rax), %xmm0
movsd %xmm0, 0x198(%rdi)
movq 0x1f0(%rdi), %rax
cmpb $0x1, 0x110(%rdi)
jne 0x8d89
movsd 0x1b0(%rax), %xmm0
movsd %xmm0, 0x130(%rbx)
movsd 0x1b8(%rax), %xmm0
movsd %xmm0, 0x138(%rbx)
movsd 0x1d0(%rax), %xmm0
movsd %xmm0, 0x140(%rbx)
movsd 0x1d8(%rax), %xmm0
movsd %xmm0, 0x148(%rbx)
movsd 0x1e0(%rax), %xmm0
movsd %xmm0, 0x150(%rbx)
movsd 0x1e8(%rax), %xmm0
movsd %xmm0, 0x158(%rbx)
movq 0x200(%rbx), %rax
movsd 0x160(%rax), %xmm0
movsd %xmm0, 0x1b0(%rbx)
movsd 0x168(%rax), %xmm0
movsd %xmm0, 0x1b8(%rbx)
movsd 0x180(%rax), %xmm0
movsd %xmm0, 0x1c0(%rbx)
movsd 0x190(%rax), %xmm0
movsd %xmm0, 0x1c8(%rbx)
movsd 0x130(%rax), %xmm0
movsd %xmm0, 0x1a0(%rbx)
movsd 0x140(%rax), %xmm0
movsd %xmm0, 0x1a8(%rbx)
movsd 0x148(%rax), %xmm5
movsd 0x158(%rax), %xmm3
movapd %xmm5, %xmm4
jmp 0x8e59
movsd 0x168(%rax), %xmm0
movsd %xmm0, 0x1b0(%rbx)
movsd 0x170(%rax), %xmm0
movsd %xmm0, 0x1b8(%rbx)
movsd 0x178(%rax), %xmm0
movsd %xmm0, 0x1c0(%rbx)
movsd 0x180(%rax), %xmm0
movsd %xmm0, 0x1c8(%rbx)
movsd 0x188(%rax), %xmm0
movsd %xmm0, 0x1a0(%rbx)
movsd %xmm0, 0x1a8(%rbx)
movsd 0x138(%rax), %xmm0
movsd 0x208(%rax), %xmm3
movsd %xmm0, 0x130(%rbx)
movsd 0x140(%rax), %xmm0
movsd %xmm0, 0x138(%rbx)
movsd 0x148(%rax), %xmm0
movsd %xmm0, 0x140(%rbx)
movsd 0x150(%rax), %xmm0
movsd %xmm0, 0x148(%rbx)
movsd 0x158(%rax), %xmm0
movsd %xmm0, 0x150(%rbx)
movsd 0x160(%rax), %xmm0
movsd %xmm0, 0x158(%rbx)
movsd 0x198(%rax), %xmm4
movsd 0x1a0(%rax), %xmm5
movsd 0x158(%rbx), %xmm0
movsd 0x1b0(%rbx), %xmm1
movq 0x1e8(%rbx), %rax
subsd 0x130(%rax), %xmm1
movsd %xmm1, 0x178(%rbx)
movsd 0x1b8(%rbx), %xmm1
subsd 0x138(%rax), %xmm1
movsd %xmm1, 0x180(%rbx)
movsd 0x1c0(%rbx), %xmm1
subsd 0x150(%rax), %xmm1
movsd %xmm1, 0x188(%rbx)
movsd 0x1c8(%rbx), %xmm1
subsd 0x160(%rax), %xmm1
movsd %xmm1, 0x190(%rbx)
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x8f02
movapd 0xd136(%rip), %xmm1     # 0x16010
movapd %xmm0, %xmm2
xorpd %xmm1, %xmm2
movlpd %xmm2, 0x1d0(%rbx)
movsd 0x150(%rbx), %xmm2
xorpd %xmm1, %xmm2
divsd %xmm0, %xmm2
movsd %xmm2, 0x128(%rbx)
movl 0x160(%rbx), %eax
cmpl $0x2, %eax
je 0x8f3b
cmpl $0x1, %eax
jne 0x91ec
movsd 0x108(%rbx), %xmm0
mulsd 0x1d0(%rbx), %xmm0
mulsd 0x148(%rbx), %xmm0
movsd %xmm0, 0x120(%rbx)
jmp 0x91ec
movsd 0x140(%rbx), %xmm0
movsd %xmm3, -0x10(%rbp)
movsd %xmm4, -0x28(%rbp)
movsd %xmm5, -0x50(%rbp)
callq 0x3310
movsd %xmm0, -0x60(%rbp)
movsd 0x140(%rbx), %xmm0
callq 0x3080
movsd 0x128(%rbx), %xmm2
movsd 0x180(%rbx), %xmm1
movsd %xmm1, -0x20(%rbp)
movsd 0x190(%rbx), %xmm1
movsd %xmm1, -0x18(%rbp)
movsd 0x1a8(%rbx), %xmm1
movsd -0x60(%rbp), %xmm4
mulsd %xmm4, %xmm1
movsd 0x1a0(%rbx), %xmm3
mulsd %xmm0, %xmm3
addsd %xmm1, %xmm3
movsd %xmm3, -0x70(%rbp)
movsd -0x50(%rbp), %xmm3
mulsd %xmm4, %xmm3
movsd -0x28(%rbp), %xmm1
mulsd %xmm0, %xmm1
movq 0x1e8(%rbx), %rax
mulsd 0x128(%rax), %xmm4
mulsd 0x118(%rax), %xmm0
addsd %xmm3, %xmm1
movsd %xmm1, -0x28(%rbp)
addsd %xmm4, %xmm0
movsd %xmm0, -0x78(%rbp)
divsd 0x118(%rbx), %xmm2
movapd %xmm2, %xmm0
mulsd %xmm2, %xmm0
movapd %xmm2, %xmm1
movapd %xmm2, -0x50(%rbp)
mulsd %xmm0, %xmm1
movapd %xmm1, -0x40(%rbp)
mulsd 0xd4a2(%rip), %xmm0     # 0x164b8
movapd %xmm0, -0x90(%rbp)
movapd 0xcfea(%rip), %xmm0     # 0x16010
xorpd %xmm2, %xmm0
movapd %xmm0, -0x60(%rbp)
callq 0x32f0
addsd 0xcff4(%rip), %xmm0     # 0x16030
movapd -0x50(%rbp), %xmm1
addsd %xmm1, %xmm0
movapd -0x90(%rbp), %xmm2
mulsd %xmm2, %xmm0
movsd %xmm0, -0x68(%rbp)
movapd -0x40(%rbp), %xmm0
addsd %xmm0, %xmm0
subsd %xmm2, %xmm0
movapd %xmm0, %xmm2
movsd 0xd449(%rip), %xmm0     # 0x164b8
mulsd %xmm1, %xmm0
addsd %xmm2, %xmm0
addsd 0xcfa1(%rip), %xmm0     # 0x16020
movsd %xmm0, -0x40(%rbp)
movapd -0x60(%rbp), %xmm0
callq 0x32f0
movsd 0xd42a(%rip), %xmm2     # 0x164c0
movapd -0x50(%rbp), %xmm1
mulsd %xmm1, %xmm2
mulsd %xmm0, %xmm2
addsd -0x40(%rbp), %xmm2
movsd %xmm2, -0x40(%rbp)
movsd 0xd173(%rip), %xmm0     # 0x16228
mulsd %xmm1, %xmm0
callq 0x32f0
mulsd 0xd402(%rip), %xmm0     # 0x164c8
addsd -0x40(%rbp), %xmm0
movsd -0x68(%rbp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x168(%rbx)
movsd 0x128(%rbx), %xmm0
movsd -0x18(%rbp), %xmm1
mulsd %xmm0, %xmm1
addsd -0x20(%rbp), %xmm1
movsd %xmm1, 0x120(%rbx)
movsd -0x10(%rbp), %xmm1
movapd %xmm1, %xmm3
mulsd %xmm1, %xmm3
movsd %xmm3, -0x18(%rbp)
movapd %xmm1, %xmm2
mulsd %xmm3, %xmm2
movsd %xmm2, -0x20(%rbp)
mulsd %xmm1, %xmm0
callq 0x3310
movsd 0xcf0e(%rip), %xmm1     # 0x16038
subsd %xmm0, %xmm1
mulsd -0x70(%rbp), %xmm1
divsd -0x18(%rbp), %xmm1
addsd 0x120(%rbx), %xmm1
movsd %xmm1, 0x120(%rbx)
movsd -0x10(%rbp), %xmm0
mulsd 0x128(%rbx), %xmm0
movsd %xmm0, -0x10(%rbp)
callq 0x3080
movsd -0x10(%rbp), %xmm1
subsd %xmm0, %xmm1
mulsd -0x28(%rbp), %xmm1
divsd -0x20(%rbp), %xmm1
addsd 0x120(%rbx), %xmm1
movsd %xmm1, 0x120(%rbx)
movsd 0x118(%rbx), %xmm0
movsd %xmm0, -0x10(%rbp)
movapd -0x60(%rbp), %xmm0
callq 0x32f0
addsd 0xce8f(%rip), %xmm0     # 0x16030
addsd -0x50(%rbp), %xmm0
movsd -0x78(%rbp), %xmm1
movsd -0x10(%rbp), %xmm2
mulsd %xmm2, %xmm1
mulsd %xmm2, %xmm1
mulsd %xmm0, %xmm1
movsd 0x120(%rbx), %xmm0
subsd %xmm1, %xmm0
movsd 0x128(%rbx), %xmm1
movsd 0x168(%rbx), %xmm2
mulsd %xmm1, %xmm1
divsd %xmm1, %xmm2
mulsd %xmm0, %xmm2
movsd %xmm2, 0x120(%rbx)
addq $0x88, %rsp
popq %rbx
popq %rbp
retq