# ****************************************************************************
# 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 alt31.f
// Replicates the straight and level (current speed) 3,1,2,1 alternative behavior

behavior alt3121_straight_level

   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 mAlternative3121Enabled = true;

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

      Vec3 mDir0;
      double mGMX = 1.0;
      double mSpd0;


      //**********************************************************************//
      //********* 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_alt3121_straight_levelt, T=", TIME_NOW);


      #writeln_d("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 (!mAlternative3121Enabled)
      {
         writeln_d("behavior not enabled!");
         return Failure("behavior alternative not enabled");
      }

      // ALL CONDITIONS PASS
      ### Generate Maneuver Alternative
#      C  STRAIGHT-AND-LEVEL
#20    iactn = 4
#      lenalt = lactn(iactn)
#      altdsc=indpk(3,1,2,1)
#      call xmit(2,vp(1,me),dir0)
#      dir0(3) = 0.
#      call vnorm(dir0,dir0)
#      gmx = gmxsu
#      spd0 = spdnow(me)
#      spdmod = desspd

      mDir0 = PLATFORM.VelocityNED();
      mDir0.SetZ(0.0);
      mDir0.Normalize();
      mGMX = brawlerPlatform.MaxSustainedGs();
      mSpd0 = PLATFORM.Speed();

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

   end_precondition


   execute

      writeln_d("3121_straight_level,  dir0 = ", mDir0.ToString());
      ## what was evaluated should be actually performed now
      brawlerPlatform.FlyVectorWithSpeed(mDir0, mGMX, mSpd0);
   end_execute

end_behavior