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

   script_debug_writes off

   script_variables

      WsfTrack mTargetTrack;

      // these values suggested for air to air fighters & jets
      double mMatchSpeedDistanceMin =   1 * 1852;        #  1 mile
      double mMatchSpeedDistanceMax =  20 * 1852;        # 20 miles
      double mWaitSpeed             = 293.941;  //mach 0.95 at 25200 ft altitude
      double mInterceptSpeed        = 600 * MATH.MPS_PER_NMPH();

      #double mWaitSpeed             = 500 * MATH.MPS_PER_NMPH();
      #double mInterceptSpeed        = 800 * MATH.MPS_PER_NMPH();
      #double mInterceptSpeed        = 293.941;  //mach 0.95 at 25200 ft altitude

   end_script_variables

   precondition
      writeln_d(PLATFORM.Name(), " precondition pursue_speed, T=", TIME_NOW);
      if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
      {
         return Failure("behavior not attached to a RIPR processor!");
      }   //   ((WsfRIPRProcessor)PROCESSOR)
      //get target from ripr processor, to be sure
      mTargetTrack = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
      if (mTargetTrack.IsNull() || !mTargetTrack.IsValid())
      {
         writeln_d("    No valid target track found to pursue!");
         return Failure("target track not found");
      }
      return true;
   end_precondition


   execute
      string comment = write_str(PLATFORM.Name(), " executing pursue_speed, T=", TIME_NOW);
      writeln_d(comment);
      #PLATFORM.Comment(comment);

      extern double mDesiredSpeed;

      mDesiredSpeed = mInterceptSpeed;
      if (mTargetTrack.VelocityValid())
      {
         if (mTargetTrack.AirDomain())
         {
            extern double EffectiveRange(WsfPlatform, WsfTrack);

            double speedOfTarget = mTargetTrack.Speed();
            double effRange      = EffectiveRange(PLATFORM, mTargetTrack);
            double distanceWindow = mMatchSpeedDistanceMax - mMatchSpeedDistanceMin;
            double speedWindow   = mInterceptSpeed - speedOfTarget;

            if(effRange < mMatchSpeedDistanceMax &&  effRange > mMatchSpeedDistanceMin)
            {
               double rangeScale = (effRange - mMatchSpeedDistanceMin) / distanceWindow;
               mDesiredSpeed = speedOfTarget + (speedWindow * rangeScale);
               writeln_d(PLATFORM.Name(), " pursue-target, speed scaled down in matching window!");
            }
            else if (effRange <= mMatchSpeedDistanceMin)
            {
               mDesiredSpeed = speedOfTarget * 0.99;
               writeln_d(PLATFORM.Name(), " pursue-target, speed set to match target!");
            }

            if (mDesiredSpeed < mWaitSpeed)
            {
               mDesiredSpeed = mWaitSpeed;
               writeln_d(PLATFORM.Name(), " pursue-target, speed was lower than wait speed, adjust!");
            }
         }
         else if (mTargetTrack.LandDomain())
         {
            writeln_d(PLATFORM.Name(), " pursue-target, target is land domain, adjust speed!");
            double speedOfTarget = mTargetTrack.Speed();
            double range      = PLATFORM.GroundRangeTo(mTargetTrack);
            double distanceWindow = mMatchSpeedDistanceMax - mMatchSpeedDistanceMin;
            double speedWindow   = mInterceptSpeed - speedOfTarget;

            if(range < mMatchSpeedDistanceMax &&  range > mMatchSpeedDistanceMin)
            {
               double rangeScale = (range - mMatchSpeedDistanceMin) / distanceWindow;
               mDesiredSpeed = speedOfTarget + (speedWindow * rangeScale);
            }
            else if (range <= mMatchSpeedDistanceMin)
            {
               mDesiredSpeed = speedOfTarget * 0.99;
            }

            if (mDesiredSpeed < mWaitSpeed)
            {
               mDesiredSpeed = mWaitSpeed;
            }
         }
      }

   end_execute

end_behavior