# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************





behavior avoid_overshoot

   script_variables
      double      mLagPursuitTime  =  5.0;   #when flying lag pursuit, fly to where the target was this many seconds ago

      double      mTurnRadius = -1.0;
      WsfGeoPoint mTurnPost = WsfGeoPoint();
      bool        mDrawPost = false;
      bool        mDrawLag  = false;
      WsfDraw     mDraw = WsfDraw();

      double   cGRAVITY_ACCEL          =          9.80665;        # 1 G
      double   cDEFAULT_ACCEL          = (cGRAVITY_ACCEL * 7.5);  # m/s^2  ~7.5 Gs
   end_script_variables


   precondition
      writeln("precondition avoid_overshoot");

      //check relative position and rates against target

      WsfTrack targetTrack = PROCESSOR.GetTarget();
      WsfMover mover = PLATFORM.Mover();

      if (targetTrack.IsNull() || !targetTrack.IsValid() || mover.IsNull() || !mover.IsValid())
      {
         writeln("target or mover not valid for executing avoid_overshoot");
         return false;
      }

      //check if target is turning & I'm in pursuit
      //check if I'm in danger of overshooting the target
      //if inside the turn circle: fly lag
      //if outside the turn circle, check closing speed and range: fly to post

      WsfPlatform tPlatform   = targetTrack.Target();
      Vec3        tAccel      = tPlatform.AccelerationWCS();
                  mTurnRadius = tPlatform.VelocityWCS().MagnitudeSquared() / tAccel.Magnitude();   //equation of circular motion
      Vec3        tTurnPost   = tAccel.Normal();
                  tTurnPost   . Scale(mTurnRadius);                                                //turning post is center of turning circle
                  tTurnPost   = Vec3.Add(tPlatform.Location().LocationWCS(), tTurnPost);           //(one radius in the direction of acceleration)
                  mTurnPost   . Set(tTurnPost);

      if (!mTurnPost.IsValid() || mTurnPost.Altitude() < 0 || mTurnPost.Altitude() != mTurnPost.Altitude())
      {
         writeln("mTurnPost is NOT VALID!");
         return false;
      }

      if (PLATFORM.SlantRangeTo(mTurnPost) > mTurnRadius)
      {
         //we are outside the target's turning circle
         double tCloseSpeed = PLATFORM.ClosingSpeedOf(tPlatform);


         //don't want to swing it too wide


      }
      else
      {
         //we are inside the target's turning circle
         return true;
      }

      return false;
   end_precondition


   execute
      WsfTrack    targetTrack  = PROCESSOR.GetTarget();
      if (targetTrack.IsNull() || !targetTrack.IsValid())
      {
         writeln("target track not valid for executing avoid_overshoot");
         return;
      }

      writeln("executing avoid_overshoot");
      PLATFORM.Comment("avoid_overshoot");

      //fly lag &/or climb
      double maxLagDist = 0.40 * PLATFORM.SlantRangeTo(targetTrack);
      double maxLagTime = maxLagDist / targetTrack.Speed();
      double lagTime = mLagPursuitTime;
      if (lagTime > maxLagTime)
      {
         lagTime = maxLagTime;
      }
      WsfGeoPoint targetLagPos = targetTrack.LocationAtTime(TIME_NOW - lagTime);

      //use track's old position (just extrapolated with velocity, backwards)

      double tAlt = targetTrack.Altitude();
      if (PLATFORM.Altitude() > tAlt)
      {
         tAlt = PLATFORM.Altitude();
      }
      PLATFORM.GoToAltitude(tAlt, 100);

      PLATFORM.TurnToHeading(PLATFORM.TrueBearingTo(targetLagPos), cDEFAULT_ACCEL);

      if (mDrawPost)
      {
         mDraw.SetLineSize(2);
         mDraw.SetColor(0.2, 1.0, 0.2); //greenish
         mDraw.SetDuration(PROCESSOR.UpdateInterval());
         mDraw.BeginLines();
            mDraw.Vertex(PLATFORM.Location());
            mDraw.Vertex(mTurnPost);
         mDraw.End();
      }

      if (mDrawLag)
      {
         mDraw.SetLineSize(2);
         mDraw.SetColor(0.8, 1.0, 0.2); //yellow-ish
         mDraw.SetDuration(PROCESSOR.UpdateInterval());
         mDraw.BeginLines();
            mDraw.Vertex(PLATFORM.Location());
            mDraw.Vertex(targetLagPos);
         mDraw.End();
      }

      double tSpeed = targetTrack.Speed();
      //double percentSpeed = 1.0 + (PLATFORM.SlantRangeTo(mTurnPost) / (mTurnRadius * 100));   //vary speed based on distance from from post
      double percentSpeed = 0.95; //match the targets speed for now

      //writeln("tSpeed=", tSpeed, ", mTurnPost=", mTurnPost.ToString(), ", SlantRangeTo(mTurnPost)=", PLATFORM.SlantRangeTo(mTurnPost), ", mTurnRadius=", mTurnRadius, ", percentSpeed=", percentSpeed);

      PLATFORM.GoToSpeed(tSpeed*percentSpeed, cDEFAULT_ACCEL, true);

      //use out of plan manuevers if necessary to slow down & preserve energy (climb)
      ###PLATFORM.GoToAltitude(mBiggestThreat.Altitude(), cDEFAULT_ACCEL, true);
   end_execute


end_behavior