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

   script_debug_writes off

   script_variables
      WsfTask                   targetTask;
      WsfLocalTrack             targetTrack;
      WsfQuantumTaskerProcessor processor;
      WsfBrawlerPlatform        brawlerPlatform;
      double                    root3            =  1.732050808;   # pcon.fi
      double                    splita           = 45.0;           # MIND file, TODO - use read in value
   end_script_variables


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


   precondition

      WsfTaskList tasks =  processor.TasksReceivedOfType("WEAPON");
      if(tasks.Count() <= 0)
      {
         string err = write_str("no weapon (target) tasks!");
         writeln_d(err);
         return Failure(err);
      }
      targetTask = tasks.Entry(0);
      targetTrack = PLATFORM.MasterTrackList().Find(targetTask.LocalTrackId());
      if (!targetTrack.IsValid() || targetTrack.TargetKilled())
      {
         string err = write_str("no target track!");
         writeln_d(err);
         return Failure(err);
      }

      return true;
   end_precondition


   execute

     writeln_d("executing pilot_posture");

      Vec3 xp = brawlerPlatform.LocationNED();
      Vec3 vp = PLATFORM.VelocityNED();
      double spdnow = PLATFORM.Speed()*MATH.FT_PER_M();
      Vec3 xp_iac = brawlerPlatform.LocationNED(targetTrack);
      Vec3 vp_iac =  targetTrack.VelocityNED();


      #aslct2()
         #spcial()
            #spbvri()


      //   "iac" is target index, get target track from WEAPON task, for now

      Vec3 hlvec = Vec3();
      double hlspd = 0.0;
      double hlval = 0.0;

      extern int kturn;
      //C     --GET KTURN VALUE
      //call ckrngi(myturn,-2,2,'~KTURN(IACID)~')
      double sgn = 1.0;
      if (kturn<0)
      {
         sgn = -1.0;
      }
     #double vsme = 0.0;   #TODO - speed of sound
     #double bvrmch = 0.0; #TODO - mach to use BVR (MIND2)
     #double bvrmch = PLATFORM.Speed()*MATH.FT_PER_M() / vsme;
     #double sa = vsme*bvrmch;
      double sa = brawlerPlatform.Speed()*MATH.FT_PER_M();

      //call asstgt(iacord,jacidord,virtl,nassign)
      //if(nassign.eq.0) goto 900
      //iac = iacord(1);
      if (kturn != 0)
      {
        writeln_d("kturn != 0");
         //C        --THIS CODE NOT NEEDED FOR POINT TACTIC:
         //C        --COMPUTE INTERCEPT PT AIMP, BASED ON MAX OF MACH 1 OR PRESENT MACH
         //C        --USE THE FIRST MEMBER OF THE ASSIGNED TARGET GROUP.
         #st = xmag(vp(1,iac));
         double st = vp_iac.Magnitude();

         //LBM - TODO - verify units used in WsfBrawlerPlatform AimPoint and Intercept() methods!!!
         #call aimpt(xp(1,me),amax1(sa,vsme),xp(1,iac),vp_iac,st,aimp,lsoln);
         #Vec3 aimp = brawlerPlatform.AimPoint(xp, MATH.Max(sa, vsme), xp_iac, vp_iac, st);
         Vec3 aimp = brawlerPlatform.AimPoint(xp, sa, xp_iac, vp_iac, st);

         #Vec3 dxpme = Vec3.Subtract(xp_iac, xp);
         #Vec3 dxt = dxpme;
         Vec3 dxt = Vec3.Subtract(xp_iac, xp);
         dxt.SetZ(0.0);
         dxt.Normalize();
         if(aimp.Magnitude() > 0)
         {
            #call vsub(aimp,xp(1,me),dxi);
            Vec3 dxi = Vec3.Subtract(aimp, xp);
            dxi.SetZ(0.0);
            dxi.Normalize();
         }
      }

#      if (kturn == 0)   //C     --POINT AT TGT
#      {
#         hdes = -xp(3,me);
#         if (lbit(pp_skrs,saskr) || pp_comg)
#         {
#            if (!ppmtrk)
#            {
#               hdes = amin1(hdes,-xp(3,iac));
#            }
#         }
##         //LBM - dont worry about rules right now
##         //C     --CHANGED RULFLG VALUE 18 NOV 87 DICKERSON
##         if (rulflg == 1510)
##         {
##            call pcode(-rulflg);
##            hdes = prdalt;
##         }
#
#           Vec3 aimpt = ((WsfBrawlerPlatform)PLATFORM).AimPoint(Vec3, double, Vec3, Vec3, double);
#
#          #call intcpt(xp(1,me),spdnow(me),xp(1,iac),vp(1,iac),root3/2.0,0.0,hdes,hlvec,hlspd);
#           hlvec = ((WsfBrawlerPlatform)PLATFORM).Intercept(Vec3, PLATFORM.Speed()*MATH.FT_PER_M(), Vec3, Vec3, double, double, double);
#           hlspd = hlvec.Magnitude();
#           hlvec.Normalize();
#
#         //goto 965;
#      }
#      //else
      if (kturn == 1 || kturn == -1)   //C     --END-RUN TGT
      {
         writeln_d("end run");
         double closmn = root3/2.0;
         double angint = splita*sgn;
         double hdes = PLATFORM.Altitude();
         //hdes = -xp(3,me);

         //LBM - dont worry about lower targets or flying lower, for now
#         if (lbit(pp_skrs,saskr) || pp_comg)
#         {
#            if (!ppmtrk)
#            {
#               hdes = amin1(hdes,-xp(3,iac));
#            }
#         }
          //LBM - dont worry about rules right now
#         //C     --CHANGED RULFLG VALUE 18 NOV 87 DICKERSON
#         if (rulflg == 1510)
#         {
#            call pcode(-rulflg);
#            hdes = prdalt;
#         }


         //LBM - TODO - verify units used in WsfBrawlerPlatform AimPoint and Intercept() methods!!!
         //call intcpt(xp(1,me),spdnow(me),xp(1,iac),vp(1,iac),root3/2.0,splita*sgn,hdes,hlvec,hlspd);
         Vec3 desVec = brawlerPlatform.Intercept(xp, spdnow, xp_iac, vp_iac, closmn, angint, hdes);
         hlvec = desVec.Normal();
         hlspd = desVec.Magnitude();

         //brawlerPlatform.InterceptLocation3D(

         //goto 965;
      }
#      else if (kturn == 2 || kturn == -2)   //C     --LOOP
#      {
#         if (lsoln) then
#            call xmit3(dxi,temp)
#         else
#            call xmit3(dxt,temp)
#         endif
#         call makex(vp(1,me),r)
#         call vxfrmc(r,temp,temp,1)
#         loopha = loopha .or. temp(1).lt.0.
#         loopnf = loopnf .or. (loopha .and. temp(1).gt.0. .and. (sign(1.,temp(2)).eq.sgn) )
#         if (loopnf) then
#            //C        --HERE NO LONGER LOOP - POINT
#            goto 100
#         endif
#         temp(1) = 0.
#         temp(2) = sgn
#         temp(3) = 0.
#         call vxfrmc(r,hlvec,temp,2)
#         hlval = valme*1.5
#         hlspd = bvrmch*spdnow(me)/fmachp
#         //C     --TURN OFF FORMATION VALUES DURING LOOP:
#         mdskpp = iand(mdskpp,6)
#         stkppp = stkppp*0.25
#         goto 970
#      }

#      //C
#//900   continue
#      //C     --HERE NO TGT - PRESUMABLY DEAD
#      hlval = 0.
#      goto 970
#      //C

//965   continue
      //C     --FINISH SETTING UP HIGH LEVEL STEERING VALUES
      #hlval = valeff(iac);
      hlval = brawlerPlatform.LastEngagementValue(targetTrack.Target());
      #writeln_d("hlvec = ", hlvec.ToString(), ", hlval = ", hlval, ", hlspd = ", hlspd);

//970   continue

      //call steer
         #steer()
#         if (dirspc.ne.0) then
#            //C        --SUPPRESS VECTORED FLIGHT VALUES - DIRECT HAS PRECEDENCE
#            valfpp= 0.0
#         elseif (vfspc.eq.0) then
            //C        --USE NORMAL HIGH_LEVEL VALUES FOR VECTORED_FLIGHT
            //call xmit(3,hlvec,vecfpp)

            extern Vec3   vecfpp;
            extern double valfpp;
            extern double sflypp;
            vecfpp = hlvec;
            valfpp = hlval;
            sflypp = hlspd;

            writeln_d("vecfpp = ", vecfpp.ToString(), ", valfpp = ", valfpp, ", sflypp = ", sflypp);


            brawlerPlatform.SuggestVectoredFlight(vecfpp, valfpp, sflypp);
#         else
#            //C        --USE FORCE_V VECTOR INSTEAD OF HL... VALUES
#            //C        --THESE MAY BE SET BY EITHER PRODUCTION RULES OR IP
#            call xmit(3,hlvfor,vecfpp)
#            valfpp = hlvalf
#            sflypp = hlspdf
#         endif


      //call sepset(10.0)   //LBM - not doing separation yet
//9999
      return;


   end_execute

end_behavior