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


#no other easy way in WSF to get relative NED position
#here we calculate a rough approximation ourselves
script Vec3 RelativePositionNED(WsfGeoPoint from, WsfGeoPoint to)
   double deg2ft = 364812.76313;
   Vec3 vec = Vec3.Construct( (to.Latitude() -from.Latitude() )*deg2ft,
                              (to.Longitude()-from.Longitude())*deg2ft*MATH.Cos(from.Latitude()*MATH.RAD_PER_DEG()),
                             -(to.Altitude() -from.Altitude() )*MATH.FT_PER_M());
   return vec;
end_script

#replicates subroutine makecs(bxin,bzin,b)
script Array<double> makecs(Vec3 bxin, Vec3 bzin)
   Array<double> b = Array<double>();
   Vec3 bx;
   Vec3 by;
   Vec3 bz;
   // Normalize bxin, store in bx
   bx = bxin.Normal();
   // Cross product of bzin and Normalized bxin, stored in by
   by = Vec3.Cross(bzin, bx);
   // Normalize by and evaluate the magnitude of original by
   if (by.Normalize() == 0.0)
   {
      // If the magnitude of by is zero, replace by with cross of bx and unit Z vector
      Vec3 unitZ = Vec3.Construct(0.0, 0.0, 1.0);
      by = Vec3.Cross(unitZ, bx);
      by.Normalize();
   }
   //cross product of bx and by stored in bz
   bz = Vec3.Cross(bx, by);

   b[0] = bx.Get(0);  // b(1,1) = bx(1)
   b[3] = bx.Get(1);  // b(1,2) = bx(2)
   b[6] = bx.Get(2);  // b(1,3) = bx(3)
   b[1] = by.Get(0);  // b(2,1) = by(1)
   b[4] = by.Get(1);  // b(2,2) = by(2)
   b[7] = by.Get(2);  // b(2,3) = by(3)
   b[2] = bz.Get(0);  // b(3,1) = bz(1)
   b[5] = bz.Get(1);  // b(3,2) = bz(2)
   b[8] = bz.Get(2);  // b(3,3) = bz(3)
   return b;
end_script

#replicates subroutine makex(cxin,[return c])
# EQUIVALENT TO MAKECS WHEN ARGUMENT BZIN = (0,0,1)
script Array<double> makex(Vec3 cxin)
   Array<double> c = Array<double>();
   Vec3 unitZ = Vec3.Construct(0.0, 0.0, 1.0);
   c = makecs(cxin, unitZ);
   return c;
end_script

#replicates subroutine makeh([return matrix], dir)
#returns an array (rot) that represents a 3x3 rotation matrix
#where rot[0] = matrix(1,1) and rot[1] == matrix(1,2)
script Array<double> makeh(Vec3 dir)
   Array<double> rot = Array<double>();
   double temp = MATH.Sqrt(dir[0]*dir[0] + dir[1]*dir[1]);
   rot[0] = dir[0]/temp;   #c(1,1) = cxin(1)/sngl(temp)
   rot[1] = dir[1]/temp;   #c(1,2) = cxin(2)/sngl(temp)
   rot[2] = 0.0;           #c(1,3) = 0.
   rot[3] = -rot[1];       #c(2,1) = -c(1,2)
   rot[4] = rot[0];        #c(2,2) =  c(1,1)
   rot[5] = 0.0;           #c(2,3) = 0.
   rot[6] = 0.0;           #c(3,1) = 0.
   rot[7] = 0.0;           #c(3,2) = 0.
   rot[8] = 1.0;           #c(3,3) = 1.
   return rot;
end_script

# replicates:  subroutine vxfrmc(b,v, [return Vec3 vt],1)
# "b" represents a 3x3 rotation matrix
# where b[0] = matrix(1,1) and b[1] == matrix(1,2)
script Vec3 vxfrmc1(Array<double> b, Vec3 v)
   #b = 3x3 rotation matrix where [0] = (1,1) and [1] == (1,2)
   double x = v[0]*b[0]+v[1]*b[1]+v[2]*b[2];
   double y = v[0]*b[3]+v[1]*b[4]+v[2]*b[5];
   double z = v[0]*b[6]+v[1]*b[7]+v[2]*b[8];
   Vec3 vt = Vec3.Construct(x,y,z);
   return vt;
end_script

# replicates:  subroutine vxfrmc(b,[return Vec3],vt,2)
# "b" represents a 3x3 rotation matrix
# where b[0] = matrix(1,1) and b[1] == matrix(1,2)
script Vec3 vxfrmc2(Array<double> b, Vec3 vt)
   #b = 3x3 rotation matrix where [0] = (1,1) and [1] == (1,2)
   double x = vt[0]*b[0]+vt[1]*b[3]+vt[2]*b[6];
   double y = vt[0]*b[1]+vt[1]*b[4]+vt[2]*b[7];
   double z = vt[0]*b[2]+vt[1]*b[5]+vt[2]*b[8];
   Vec3 v = Vec3.Construct(x,y,z);
   return v;
end_script

# replicates: subroutine rotx(chi,vin,[return vout])
# FINDS VECTOR IN A FRAME ROTATED BY CHI ABOUT X-AXIS
script Vec3 rotx(double chi, Vec3 vin)
   double cchi = (Math.Cos(chi * Math.DEG_PER_RAD())) * Math.RAD_PER_DEG();
   double schi = (Math.Sin(chi * Math.DEG_PER_RAD())) * Math.RAD_PER_DEG();
   Vec3 vout = Vec3.Construct(vin[0], (cchi*vin[1]+schi*vin[2]), (-schi*vin[1]+cchi*vin[2]));
   return vout;
end_script

# replicates: subroutine rotv(chi,vaxis,vin,[return vout])
# ROTATE VECTOR BY ANGLE ABOUT SPECIFIED AXIS
#CIN  CHI    REAL - DESIRED ANGLE OF ROTATION (radians)
#CIN  VAXIS  3-VEC - AXIS OF ROTATION
#CIN  VIN    3-VEC - UNROTATED VECTOR
#COUT VOUT   3-VEC - RESULT OF ROTATING VIN BY CHI ABOUT VAXIS
script Vec3 rotv(double chi, Vec3 vaxis, Vec3 vin)
   Array<double> rot = Array<double>();
   if (vaxis[0]*vaxis[0] + vaxis[1]*vaxis[1] == 0.0)
   {
      Vec3 unitX = Vec3.Construct(1,0,0);
      rot = makecs(vaxis, unitX);
   }
   else
   {
      rot = makex(vaxis);
   }
   Vec3 vout = vxfrmc1(rot,vin);
   vout = rotx(-chi,vout);
   vout = vxfrmc2(rot,vout);
   return vout;
end_script

# replicates: vmake(a,vin,vout)
# a - desired vector norm
# vector - vector to rescale
script Vec3 vmake(double a, Vec3 vin)
   double b = a / vin.Magnitude();
   // TODO prevent div by zero?
   // b = a/sqrt(1.e-35 + vin(1)*vin(1) + vin(2)*vin(2) + vin(3)*vin(3))
   Vec3 vout = Vec3.Construct(b*vin.X(), b*vin.Y(), b*vin.Z());
   return vout;
end_script

# replicates:  subroutine ramp(xlo,xval,xhi)
script double ramp(double xlo, double xval, double xhi)
   double rampVal = (xval-xlo) / (xhi-xlo);
   if(rampVal <= 0.0)
   {
      rampVal = 0.0;
   }
   if(rampVal >= 1.0)
   {
      rampVal = 1.0;
   }
   return rampVal;
end_script

# replicates:  subroutine border(z, z0)
script double border(double z, double z0)
   double border = 0;
   if (z <= 0)
   {
      double t = (z / z0 - 1.0);
      border = 1.0 / (1.0 + (t * t));
   }
   else
   {
      double u = (z / z0 + 1.0) * (z / z0 + 1.0);
      border = u / (1 + u);
   }
   return border;
end_script

# replicates:  subroutine cauchy(z, z0)
script double cauchy(double z, double z0)
   double t = (z / z0);
   return 1.0 / (1.0 + (t * t));
end_script


script Vec3 vorth(Vec3 a, Vec3 b)
   double x = (a[0]*b[0]+a[1]*b[1]+a[2]*b[2])/(b[0]*b[0]+b[1]*b[1]+b[2]*b[2]);
   Vec3 temp = b;
   temp.Scale(x);
   Vec3 c = Vec3.Subtract(a,temp);
   return c;
end_script


script bool HaveWeapon(WsfPlatform plat)
   for(int i=0; i<plat.WeaponCount(); i+=1)
   {
      if (plat.WeaponEntry(i).QuantityRemaining() >= 1)
      {
         return true;
      }
   }
   return false;
end_script


script Vec3 unitv2(Vec3 a)
   double mag = MATH.Sqrt(a.X()*a.X() + a.Y()*a.Y());
   Vec3 val = Vec3.Construct(a.X()/mag, a.Y()/mag, 0.0);
   return val;
end_script


#replicates portion of desvvi that calculates "vuse" and "spduse"
#function arguments:
#   vdes   = normalized direction vector
#   spddes = speed in ft/sec
#returns a vec in ft/sec
script Vec3 desvvi(WsfBrawlerPlatform plat, Vec3 vdes, double spddes)
   bool lngtrn;
   Vec3 vuse = Vec3();
   double spduse;
   double cornrv = plat.CorneringVelocity();   //fps
   double   grav = 32.17405;                   //fps
   double gmxsut = plat.MaxSustainedGs();

   //C     --external declarations
#   integer rspace
#   realdot,ramp,xlimit
#   real arccos
   //C     --local declarations
   //real h(2),hdes(2);
   Vec3 h = Vec3();
   Vec3 hdes = Vec3();
   double d10 = 0.174533;   //radians (10 deg)
   double s30 = 0.5;
   double s45 = 0.707107;
   double s15 = 0.258819;
   double dsemax,hnorm,csenow,senow;
   //real arcsin
   //C*ENDDEC
   vuse = unitv2(vdes);
   h    = unitv2(plat.VelocityNED());

   //C  CURRENT HEADING COORDINATES:
   //C     X = [ H(1),H(2)]
   //C     Y = [-H(2),H(1)]
   //hdes(1) = vuse(1)*h(1)+vuse(2)*h(2)
   hdes.SetX(vuse.X()*h.X()+vuse.Y()*h.Y());

   //C        TO AVOID MESSING UP LATER ARCCOS(HDES(1)):
   if(MATH.Fabs(hdes.X()) > 1.0)
   {
      if(MATH.Fabs(hdes.X()) > 1.000001)
      {
         //call nabort('HDES(1) NORM');
         writeln("ABORT!!!! hdes(1) > 1.000001");
         return Vec3();
      }
      hdes.SetX(1.0);
      if (hdes.X() < 0)
      {
         hdes.SetX(-1.0);
      }
   }

   hdes.SetY(-vuse.X()*h.Y()+vuse.Y()*h.X());
   csenow = plat.Speed()*MATH.FT_PER_M()+0.01;
   csenow = Vec3.Dot(vdes,plat.VelocityNED())/csenow;
   senow = MATH.ACos(csenow);
   double tproj3 = plat.ProjectedTimeDelta();
   dsemax = tproj3*(gmxsut*grav/cornrv);
   //C  LNGTRN REQUIRES LARGE ERRORS IN BOTH MAGNITUDE AND HEADING
   lngtrn = MATH.ACos(hdes.X()) > dsemax && senow > 1.5*dsemax;
   if (!lngtrn)
   {
      //C
      //C  SHORTER TURN:
      //C
      vuse = vdes;
      spduse = spddes;
      //goto 9999
   }
   else
   {
      //C
      //C  LONGER TURN:
      //C
      if((plat.Speed()*MATH.FT_PER_M()) > 0.9*cornrv)
      {
         if((plat.Speed()*MATH.FT_PER_M()) < 1.1*cornrv)
         {
            //C     HERE SPEED NEAR CORNER - MAINTAIN MODEST DIVE TO HELP MAINTAIN SPD
            vuse.SetZ(s15);
         }
         else
         {
            //C     TOO FAST - PULL UP WHILE TURNING
            //format('DESVVI... TOO FAST, CORNER = ',f8.2,' MACH ',f5.1)
            vuse.SetZ(-s45*ramp(1.1*cornrv,plat.Speed()*MATH.FT_PER_M(),1.2*cornrv));
         }
      }
      else
      {
         //C     TOO SLOW - SET 30 DEG DIVE
         vuse.SetZ(s30);
      }

      hnorm = MATH.Sqrt(1.0-vuse.Z()*vuse.Z());
      if(hdes.X() > 0.0)
      {
         //C  RENORMALIZE HORIZONTAL FOR LESS THAN 90 DEG HEADING CHANGES
         vuse.SetX(vuse.X()*hnorm);
         vuse.SetY(vuse.Y()*hnorm);
      }
      else
      {
         //C     SET UP 90 DEG HEADING CHANGE AND DO ROTATION BACK TO EARTH COORDINATES
         //C     IMPLICIT IS HDES(1) = 0
         hdes.SetY(hnorm);
         if (hdes.Y() < 0)
         {
            hdes.SetY(-hnorm);
         }
         vuse.SetX(-h.Y()*hdes.Y());
         vuse.SetY(h.X()*hdes.Y());
      }
      spduse = cornrv;
      //goto 9999
   }
   //C
//9999
   vuse.Normalize();
   vuse.Scale(spduse);
   return vuse;
end_script