#ifndef AuSIM_UTIL_H
 #define AuSIM_UTIL_H

#ifndef TRIG
 #define TRIG
 #include <math.h>
 #define pi        3.14159265358979323846
 #define pi2      (2.0*pi)
 #ifndef  M_PI
  #define  M_PI     pi
  #define  M_PI_2     (0.5 * M_PI)
  #define  M_1_PI     (1.0 / M_PI)
 #endif
 #define epsilon   0.000001
 #define _EPSILON  0.0001
 #define EPSILON   0.01
 #define QUADRANT  (0.25 * M_PI)
 #define RADS2DGRS (180.0/M_PI)
 #define RADS2DEGS (180/M_PI)
 #define DEGS2RADS (M_PI/180.0)
 #define DGRS2RADS (M_PI/180.0)
 #define XAXIS 0
 #define YAXIS 1
 #define ZAXIS 2
#endif

#ifndef VCTRTYPES
 #define VCTRTYPES
  struct vctr2 {
      float x, y;
      vctr2(float a=0.0) : x(a),y(a) { }
      vctr2(float a, float b) : x(a),y(b) { }
      vctr2(const vctr2 &a) : x(a.x),y(a.y) { }

      float length(void)         const { return sqrt(x*x + y*y); }
      float dot(const vctr2 &a)  const { return (a.x*x + a.y*y); }

      vctr2 &operator+=(float inc)        { x+=inc; y+=inc;  return *this; }
      vctr2 &operator*=(float fac)        { x*=fac; y*=fac;  return *this; }
      vctr2 &operator+=(const vctr2 &v)   { x+=v.x; y+=v.y;  return *this; }
      int    operator==(const vctr2 &v) const { return ((x==v.x)&&(y==v.y)); }

      friend vctr2 operator*(const vctr2 &a, float fac);
      friend vctr2 operator/(const vctr2 &a, float div);
      friend vctr2 operator+(const vctr2 &a, const vctr2 &b);
      friend vctr2 operator-(const vctr2 &a, const vctr2 &b);
      friend vctr2 operator*(const vctr2 &a, const vctr2 &b);
      friend vctr2 operator/(const vctr2 &a, const vctr2 &b);
      
      vctr2 unit(void) const
      {
         float len = this->length();
         if (len>0.0) return *this / len;
         return vctr2(1.0f);
      }
  };
  struct vctr3 : public vctr2 {
	    float z;
	    vctr3(float u=0.0, float v=0.0, float w=0.0) : vctr2(u,v),z(w) { }
	    vctr3(const vctr3 &v) : vctr2(v.x,v.y),z(v.z) { }
	    vctr3(const vctr2    &a, float c=0.0) : vctr2(a.x,a.y),z(c) { }
	    int    operator==(const vctr3 &v) const
          { return ((x==v.x)&&(y==v.y)&&(z==v.z)); }
	    vctr3 &operator= (const vctr3 &v) { x=v.x; y=v.y; z=v.z;  return *this; }
	    vctr3 &operator+=(float inc) { x+=inc; y+=inc; z+=inc;  return *this; }
	    vctr3 &operator-=(float dec) { x-=dec; y-=dec; z-=dec;  return *this; }
	    vctr3 &operator+=(const vctr3 &v) { x+=v.x; y+=v.y; z+=v.z;  return *this; }
	    vctr3 &operator-=(const vctr3 &v) { x-=v.x; y-=v.y; z-=v.z;  return *this; }
	    vctr3 &operator*=(float fac) { x*=fac; y*=fac; z*=fac;  return *this; }
	    vctr3 &operator/=(float div) { if (div!=0) *this*=(1/div);  return *this; }
	    float dot(const vctr3 &a) const { return (a.x*x + a.y*y + a.z*z); }
	    float length(void) const { return sqrt(x*x + y*y + z*z); }
	    vctr3 cross(const vctr3 &a) const
          { return vctr3( (y*a.z - z*a.y), (z*a.x - x*a.z), (x*a.y - y*a.x) ); }
	    friend vctr3 operator*(const vctr3 &a, float fac);
	    friend vctr3 operator/(const vctr3 &a, float div);
	    friend vctr3 operator*(const vctr3 &a, const vctr3 &b);
	    friend vctr3 operator/(const vctr3 &a, const vctr3 &b);
	    friend vctr3 operator+(const vctr3 &a, const vctr3 &b);
	    friend vctr3 operator-(const vctr3 &a, const vctr3 &b);
	    vctr3 unit(void) const
          { float len = this->length();
            if (len>0.0) return *this / len;  return vctr3(1.0f); }
  };
  struct vctr4 : public vctr3 {
      float w;
      vctr4(float a=0.0, float b=0.0, float c=0.0, float d=1.0) :
						      vctr3(a,b,c),w(d) { }
      vctr4(const vctr4 &v) : vctr3(v.x,v.y,v.z),w(v.w) { }
      vctr4(const vctr3 &a, float d=1.0) : vctr3(a.x,a.y,a.z),w(d) { }
  };
  struct euler {
      float a, e, r;
      euler(float u=0.0, float v=0.0, float w=0.0) : a(u), e(v), r(w) { }
      euler(const euler &v) : a(v.a), e(v.e), r(v.r) { }
      int    operator==(const euler &v) const
          { return ((a==v.a)&&(e==v.e)&&(r==v.r)); }
      vctr3 sin(void) const { return vctr3(::sin(r),::sin(e),::sin(a)); }
      vctr3 cos(void) const { return vctr3(::cos(r),::cos(e),::cos(a)); }
      vctr3 bore(void) const
          { float ecs = ::cos(e);
            return vctr3(ecs*::cos(a),ecs*::sin(a),-::sin(e)); }
  };
  struct polar {
      float az, el, rg;
      polar(float u=0.0, float v=0.0, float w=0.0) : az(u), el(v), rg(w) { }
      polar(const polar &v) : az(v.az), el(v.el), rg(v.rg) { }
      polar(const vctr3 &v) : az(), el(), rg()
          { if ((  rg = v.length()) < epsilon) return;
            if (v.vctr2::length() < epsilon) el = (float) M_PI;
            else  el = asin(v.z/rg);  az = atan2(v.y,v.x); }
	    vctr3 unit(void) const
          { double cse = cos(el);
            return vctr3((float)(cos(az)*cse),(float)(sin(az)*cse),sin(el)); }
  };
  struct vctr6 : public vctr3, public euler {
      vctr6(float u=0.0, float v=0.0, float w=0.0,
            float i=0.0, float j=0.0, float k=0.0) :
		      vctr3(u,v,w), euler(i,j,k) { }
      vctr6(const vctr3 &v) : vctr3(v.x,v.y,v.z), euler() { }
      vctr6(const vctr3 &v, const vctr3 &n) : vctr3(v.x,v.y,v.z), euler()
            { if (n.vctr2::length()==0) e = (float) M_PI;
              else { double l=n.length();
                if (l!=0) { a = (float) atan2(n.y,n.x);  e = (float) asin(n.z/l); }
              } }
      vctr6(const vctr6 &v) : vctr3(v.x,v.y,v.z), euler(v.a,v.e,v.r) { }
      vctr6(const euler &v) : vctr3(), euler(v.a,v.e,v.r) { }
      vctr6 &operator=(const vctr6 &v)
                  { x=v.x; y=v.y; z=v.z; a=v.a; e=v.e; r=v.r;  return *this; }
      vctr6 &operator=(const vctr3 &v) { x=v.x; y=v.y; z=v.z;  return *this; }
      vctr6 &operator=(const euler &v) { a=v.a; e=v.e; r=v.r;  return *this; }
      int    operator==(const vctr6 &v) const
          { return ( (x==v.x)&&(y==v.y)&&(z==v.z)
                   &&(a==v.a)&&(e==v.e)&&(r==v.r) ); }
      vctr3  normal() const
      {
         double ce=::cos(e);
         return vctr3((float) (::cos(a)*ce),(float) (::sin(a)*ce),::sin(e));
      }
  };

  //  friend inline functions
  inline vctr2 operator*(const vctr2 &a, float fac)
  { return vctr2(a.x*fac,a.y*fac); }

  inline vctr2 operator/(const vctr2 &a, float div)
  { return (div==0.0)?vctr2(0.0f):vctr2(a.x/div,a.y/div); }

  inline vctr2 operator+(const vctr2 &a, const vctr2 &b)
  { return vctr2(a.x+b.x,a.y+b.y); }

  inline vctr2 operator-(const vctr2 &a, const vctr2 &b)
  { return vctr2(a.x-b.x,a.y-b.y); }

  inline vctr2 operator*(const vctr2 &a, const vctr2 &b)
  { return vctr2(a.x*b.x,a.y*b.y); }

  inline vctr2 operator/(const vctr2 &a, const vctr2 &b)
  { return vctr2((b.x==0.0)?0.0f:a.x/b.x,
					  (b.y==0.0)?0.0f:a.y/b.y); }

  inline vctr3 operator*(const vctr3 &a, float fac)
  { return vctr3(a.x*fac,a.y*fac,a.z*fac); }

  inline vctr3 operator/(const vctr3 &a, float div)
  { if (div==0.0) return a;  float fac = 1.0f/div;  return (a*fac); }

  inline vctr3 operator+(const vctr3 &a, const vctr3 &b)
  { return vctr3(a.x+b.x,a.y+b.y,a.z+b.z); }

  inline vctr3 operator-(const vctr3 &a, const vctr3 &b)
  { return vctr3(a.x-b.x,a.y-b.y,a.z-b.z); }

  inline vctr3 operator*(const vctr3 &a, const vctr3 &b)
  { return vctr3(a.x*b.x,a.y*b.y,a.z*b.z); }

  inline vctr3 operator/(const vctr3 &a, const vctr3 &b)
  { return vctr3((b.x==0.0)?0.0f:a.x/b.x,
                 (b.y==0.0)?0.0f:a.y/b.y,
                 (b.z==0.0)?0.0f:a.z/b.z); }

  template <class T> T &vrot(T &v) { T t=v;  v.x=-v.y;  v.y=t.x;  return v; }
  template <class T> T &urot(T &v) { T t=v;  v.y=-v.x;  v.x=t.y;  return v; }
 #endif   // VCTRTYPES

polar  Convert2Polar(const vctr3 *pnt, const vctr3 *ecs, const vctr3 *esn);
double AngleFromXaxis(double x, double y);
double AngleFromYaxis(double x, double y);
double AngleFromPlane(double z, double d);
double FBound(double val, double min, double max);
long   IBound(  long val,   long min,   long max);
double FWrap (double val, double min, double max);
int    IWrap (   int val,    int min,    int max);

#endif    //  AuSIM_UTIL_H