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

// converted BRAWLER v7.5 alt35.f
// Replicates the force overshoot 3,5,6,1 alternative behavior

behavior alt3561_force_overshoot

   script_debug_writes off

   script_variables

      WsfQuantumTaskerProcessor processor;
      WsfPerceptionProcessor    perception;
      WsfBrawlerPlatform        brawlerPlatform;

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

      //**********************************************************************//
      //** alternative parameters                                           **//
      //**********************************************************************//
      // Flag used to enable/disable this alternative
      bool mAlternative3561Enabled = true;

      // Alternative ID
      int ilevel = 3;
      int kalt   = 5;
      int icall  = 6;
      int lcall  = 1;

      // Maneuver Alternative flight values
      Vec3 mDir0;
      double mGMX = 1.0;
      double mSpd0 = 3.0;

      //**********************************************************************//
      //********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
      //**********************************************************************//
      WsfDraw  mDraw             = WsfDraw();
      double   mLastTime         = 0.0;

   end_script_variables

   on_init
      if (PROCESSOR.IsA_TypeOf("WSF_QUANTUM_TASKER_PROCESSOR"))
      {
         processor = (WsfQuantumTaskerProcessor)PROCESSOR;
      }
      perception = (WsfPerceptionProcessor)PLATFORM.Processor("perception");
      brawlerPlatform = (WsfBrawlerPlatform)PLATFORM;
   end_on_init

   precondition
      #writeln_d(PLATFORM.Name(), " precondition behavior_alt3561_force_overshoot, T=", TIME_NOW);

      ### Evaluate conditions that would prevent behavior alternative from running

      if (!PROCESSOR.IsA_TypeOf("WSF_QUANTUM_TASKER_PROCESSOR"))
      {
         writeln_d("not a quantum tasker!");
         return Failure("behavior not attached to a WSF_QUANTUM_TASKER_PROCESSOR");
      }

      if (!mAlternative3561Enabled)
      {
         writeln_d("behavior not enabled!");
         return Failure("behavior alternative not enabled");
      }

      WsfTaskList tasks = processor.TasksReceivedOfType("WEAPON");
      if(tasks.Count() <= 0)
      {
         return Failure("no weapon (hostile) tasks!");
      }
      WsfTask targetTask = tasks.Entry(0);
      WsfLocalTrack hostile = PLATFORM.MasterTrackList().Find(targetTask.LocalTrackId());
      if (!hostile.IsValid())
      {
         return Failure("no hostile track!");
         //TODO also validate location / veloctiy
      }

      // alt35.f line 247
#      if(rngun(iac).gt.3000.) go to 205
      double projectionTime = TIME_NOW + brawlerPlatform.ProjectedTimeDelta();
      WsfGeoPoint projectedHostileLocation = hostile.LocationAtTime(projectionTime);
      double rangeToHostile = PLATFORM.SlantRangeTo(projectedHostileLocation);

      if (rangeToHostile > 3000 * Math.FT_PER_M())
      {
         string msg = write_str("T=",TIME_NOW,", alt ", ilevel, kalt, icall, lcall, " hostile range greater than 3000 ft");
         writeln_d(msg);
         return Failure(msg);
      }

      // ALL CONDITIONS PASS
      ### Generate Maneuver Alternative
      // alt35.f line 249 - 256
#      iactn = 4
#      lenalt = lactn(iactn)
#      altdsc = altpk(3,5,icall,1,iacidt(iac),0)
#      call cros1(vp(1,me),vp(1,iac),dir0)
#      if(dir0(3).gt.0.) call vmult(-1.,dir0,dir0)
#      gmx = amin1(5.,gmxin)
#      spd0 = spdnow(me)+almin*tproj3*1.1
#      spdmod = desspd
      Vec3 myVdir = PLATFORM.VelocityNED();
      Vec3 tgtVdir = hostile.VelocityNED();
      mDir0 = Vec3.Cross(myVdir, tgtVdir);
      mDir0.Normalize();
      if (mDir0.Z() > 0)
      {
         mDir0.Scale(-1.0);
      }
      mGMX = Math.Min(5.0, brawlerPlatform.MaxAvailableGs());
      double almin  = brawlerPlatform.MinForwardAccelWithGravity();
      mSpd0 = PLATFORM.Speed() * MATH.FT_PER_M() + almin * brawlerPlatform.ProjectedTimeDelta() * 1.1;
      mSpd0 *= MATH.M_PER_FT();

      ### Evaluate [Projected] Maneuver Alternative
      double score = brawlerPlatform.EvaluateVectorWithSpeed(mDir0, mGMX, mSpd0, ilevel, kalt, icall, lcall);
      writeln_d("T=", TIME_NOW, ", 3561_force_overshoot   = ", score);
      return score;

   end_precondition

   execute
      writeln_d("3561_force_overshoot");
      ## what was evaluated should be actually performed now
      brawlerPlatform.FlyVectorWithSpeed(mDir0, mGMX, mSpd0);
   end_execute

end_behavior

