# ****************************************************************************
# 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 escort

   script_debug_writes off

   script_variables

      //**********************************************************************//
      //** debugging parameters                                             **//
      //**********************************************************************//
      bool  mDrawEscortData    =  false;

      //**********************************************************************//
      //** control / mode of operation parameters                           **//
      //**********************************************************************//
      bool  mCheckOnlyJobs     =  true;
      bool  mCheckMasterTracks =  false;

      //**********************************************************************//
      //** escort parameters (who to escort, and what ranges)               **//
      //**********************************************************************//
      Array<string> mEscortNames          = Array<string>();
      double        mEscortProtectDistance = 100.0 * MATH.M_PER_NM();  #engage threats within this range of the escort package
      double        mEscortChaseDistance   =  15.0 * MATH.M_PER_NM();  #allowed to chase a threat this far out of protect area if it entered
      double        mWeaponRangeToInclude  =  50.0 * MATH.M_PER_NM();

      //**********************************************************************//
      //** flying parameters, for offset and tightness                      **//
      //**********************************************************************//
      double   mFormationPositionX =  0;     #meters in front of of package
      double   mFormationPositionY =  0;     #meters off right wing of package
      double   mGoodFormationRatio =  0.15;  #percentage of total offset
      double   mFormationLookAhead = 30.0;   #seconds
      double   mFormationAltitude  = -1.0;   #used if positive

      //**********************************************************************//
      //********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
      //**********************************************************************//
      WsfDraw  mDraw           =  WsfDraw();
      bool     mLastInPosition =  false;  #boolean flag, utility, don't change

   end_script_variables


   ### find escorted platform, return it
   script WsfPlatform GetEscortPackage()
      WsfPlatform escortPlatform;
      foreach ( string sEscortName in mEscortNames )
      {
         escortPlatform = WsfSimulation.FindPlatform(sEscortName);
         if (escortPlatform.IsValid() )
         {
            break;
         }
      }
      return escortPlatform;
   end_script


   script bool IsAThreatToEscortPackage(WsfTrack aTrack, WsfPlatform aPackage)
      double slantRange        = aPackage.SlantRangeTo(aTrack);
      double currentlyTargeted = 0;
      if (((WsfRIPRProcessor)PROCESSOR).GetTargetName() == aTrack.TargetName())
      {
         currentlyTargeted = 1.0;
      }
      #  determine if the target is a threat to your escort package, check if it lies within the protect radius
      #  if already engaging the threat, allow a little chase
      if (slantRange < (mEscortProtectDistance + mWeaponRangeToInclude + (currentlyTargeted * mEscortChaseDistance)))
      {
          writeln_d("Target ", aTrack.TargetName()," is a threat to escort package!  Stop formation flying, go engage!!!");
          return true;
      }
      return false;
   end_script

   script void DrawThreat(WsfTrack track)
      if (!track.IsValid())
      {
         return;
      }
      if (mDrawEscortData == true)
      {
         mDraw.SetLayer("behavior_escort");
         mDraw.SetDuration(PROCESSOR.UpdateInterval());
         // draw line to desired formation position
         mDraw.SetColor(1, 0, 0);
         mDraw.SetLineSize(5);
         mDraw.SetLineStyle("dashed");
         mDraw.BeginLines();
            mDraw.Vertex(PLATFORM.Location());
            mDraw.Vertex(track.CurrentLocation());
         mDraw.End();
         mDraw.SetLineSize(2);
         mDraw.SetLineStyle("solid");
         mDraw.SetEllipseMode("line");
         mDraw.SetColor(0, 1, 1);
         mDraw.BeginEllipse(0,20,20);
            mDraw.Vertex(track.CurrentLocation());
         mDraw.End();
      }
   end_script

  script bool HaveAPursueTargetJob()
    WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
    if (commander.IsValid())
    {
      for (int channel = 0; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
      {
        WsfRIPRJob job = commander.GetJobFor(((WsfRIPRProcessor)PROCESSOR), channel);
        if (job.IsValid() && job.Name() == "pursue-target")
        {
          return true;
        }
      }
    }
    return false;
  end_script


   precondition
      writeln_d("precondition escort");

      if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
      {
         return Failure("behavior not attached to a RIPR processor!");
      }

      WsfPlatform escortPlatform = GetEscortPackage();
      if (!escortPlatform.IsValid() )
      {
         return Failure("no listed escort packages found to escort");
      }

      //check jobs?
      //check tracks?
      if (mCheckOnlyJobs == true &&
          ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid())
      {
         bool packageInDanger = false;
         Array<WsfRIPRJob> jobs = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobs();
         Array<WsfTrack> threats = Array<WsfTrack>();
         foreach (WsfRIPRJob job in jobs)
         {
            if (job.Name() == "pursue-target")
            {
               string name = (string)job.GetData("targetTrackName");
              #extern WsfTrack GetTrackByName(WsfPlatform, string);
               WsfTrack track = GetTrackByName(PLATFORM, name);
               if (track.IsValid())
               {
                  if (IsAThreatToEscortPackage(track, escortPlatform))
                  {
                     threats.PushBack(track);
                     packageInDanger = true;
                  }
                  else if (track.AirDomain())
                  {
                     #unbid job, so we cant go after anything thats not a threat to our package
                     for (int channel = 0; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
                     {
                        job.UnbidJob( ((WsfRIPRProcessor)PROCESSOR), channel);
                     }
                  }
               }
            }
         }
         if (packageInDanger == true && HaveAPursueTargetJob())
         {
            foreach (WsfTrack threat in threats)
            {
               DrawThreat(threat);
            }
            return Failure("target is threat to package");  //pre-condition not met, can't just fly in formation
         }
      }
      else if (mCheckMasterTracks == true)
      {
         foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
         {
            //check if this track is a threat
            if (t.SideValid() &&
                t.Side() == PLATFORM.Side())
            {
               continue;
            }

            if (IsAThreatToEscortPackage(t, escortPlatform))
            {
               DrawThreat(t);
               return Failure("track is threat to package");  //pre-condition not met, can't just fly in formation
            }
         }
      }

      writeln_d("--- agent ", PLATFORM.Name(), " is escorting ", escortPlatform.Name());
      return true;
   end_precondition



   script bool AmInFormationPosition(WsfGeoPoint formationPoint, double radius)
      //make it fly to the inner 50% of the acceptable radius on re-entry
      if (!mLastInPosition)
      {
         radius = 0.5 * radius;
      }
      mLastInPosition = false;
      double range = PLATFORM.GroundRangeTo(formationPoint);
      if (range <= radius)
      {
         mLastInPosition = true;
      }
      return mLastInPosition;
   end_script



   execute
      writeln_d(PLATFORM.Name(), " executing escort, T=", TIME_NOW);
      //PLATFORM.Comment("escort");

      //make sure escorted platform exists, find it, save it
      WsfPlatform escortPlatform = GetEscortPackage();
      if (!escortPlatform.IsValid() )
      {
         writeln_d("--- fighter ", PLATFORM.Name(), " no valid platform to escort!");
         return;
      }

      //check if I am approximately in my formation position
      WsfGeoPoint formationPoint = escortPlatform.Location();

      if (mFormationAltitude > 0)
      {
        formationPoint.Set(formationPoint.Latitude(), formationPoint.Longitude(), mFormationAltitude);
      }

      if (mFormationPositionX > 0)
      {
         formationPoint.Extrapolate(escortPlatform.Heading(), mFormationPositionX);
      }
      else
      {
         formationPoint.Extrapolate(escortPlatform.Heading()+180, -mFormationPositionX);
      }

      if (mFormationPositionY > 0)
      {
         formationPoint.Extrapolate(escortPlatform.Heading()+90.0, mFormationPositionY);
      }
      else
      {
         formationPoint.Extrapolate(escortPlatform.Heading()-90.0, -mFormationPositionY);
      }

      if (!formationPoint.IsValid())
      {
         writeln_d("--- fighter ", PLATFORM.Name(), " invalid formation point from ", escortPlatform.Name());
         return;
      }

      if (mDrawEscortData == true)
      {
         mDraw.SetLayer("behavior_escort");
         mDraw.SetDuration(((WsfRIPRProcessor)PROCESSOR).UpdateInterval());

         // draw protection bubble
         mDraw.SetColor(0.5, 0.5, 0.5, 0.33);   //gray with 0.33 alpha
         mDraw.SetEllipseMode("fill");
         double R = mEscortProtectDistance + mWeaponRangeToInclude;
         mDraw.BeginEllipse(0, R, R);
            mDraw.Vertex(escortPlatform.Location());
         mDraw.End();

         // draw line to desired formation position
         mDraw.SetColor(0, 1, 0);   //green
         mDraw.SetLineSize(4);
         mDraw.BeginLines();
            mDraw.Vertex(escortPlatform.Location());
            mDraw.Vertex(formationPoint);
         mDraw.End();
      }

      double radius = mGoodFormationRatio * escortPlatform.SlantRangeTo(formationPoint);

      double formAlt   = escortPlatform.Altitude();
      double formSpeed = escortPlatform.Speed();
      double formYaw   = escortPlatform.Heading();
      if (formAlt < PLATFORM.Altitude())
      {
         formAlt = PLATFORM.Altitude();
      }
      WsfGeoPoint tgt = WsfGeoPoint.Construct(formationPoint.Latitude(), formationPoint.Longitude(), formAlt);
      double lookAheadDistance  = mFormationLookAhead * escortPlatform.Speed();
      tgt.Extrapolate(formYaw, lookAheadDistance);

      if (! AmInFormationPosition(formationPoint, radius))
      {
         //fly towards formation position, extrapolated ahead X seconds, in order to catch up to the point in X seconds
         formSpeed = PLATFORM.GroundRangeTo(tgt) / mFormationLookAhead;
      }

     #extern bool FlyTarget (WsfPlatform, WsfGeoPoint,    double);
      FlyTarget (PLATFORM, tgt, formSpeed);

   end_execute

end_behavior

