template< typename T >
void Aftr::QuatT<T>::enforce_imaginary_comp_is_always_positive() noexcept
{
   return;
   if( this->a < 0 )
   {
      this->a = -this->a;
      this->b = -this->b;
      this->c = -this->c;
      this->d = -this->d;
   }
}

template< typename T >
Aftr::QuatT<T>::QuatT() noexcept {}

template< typename T >
Aftr::QuatT<T>::QuatT( T a, T b, T c, T d ) noexcept : a( a ), b( b ), c( c ), d( d )
{
   this->enforce_imaginary_comp_is_always_positive();
}

template< typename T >
Aftr::QuatT<T>::QuatT( Aftr::AxisAngleT<T> const& aa ) noexcept
{
   (*this) = QuatImpl::fromAxisAngle( aa );
}

template< typename T >
Aftr::QuatT<T>::QuatT( Aftr::QuatT<T> const& toCopy ) noexcept
{
   *this = toCopy;
}

template< typename T >
Aftr::QuatT<T>& Aftr::QuatT<T>::operator=( const Aftr::QuatT<T>& q )
{
   if( this != &q )
   {
      this->a = q.a;
      this->b = q.b;
      this->c = q.c;
      this->d = q.d;
      this->enforce_imaginary_comp_is_always_positive();
   }
   return *this;
}

template< typename T >
Aftr::QuatT<T>::QuatT( const Mat4T<T>& m ) noexcept
{
   *this = QuatImpl::fromDCM( m );
}

template< typename T >
Aftr::QuatT<T>::QuatT( const T* m ) : Aftr::QuatT<T>( Mat4T<T>(m) ) {}

template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::negation() const noexcept
{
   Aftr::QuatT<T> temp(-a, -b, -c, -d);
   return temp;
}

template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::conjugate() const noexcept
{
   Aftr::QuatT<T> temp(a, -b, -c, -d);
   return temp;
}

template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::operator +(const Aftr::QuatT<T> &q) const
{
   Aftr::QuatT<T> temp(a + q.a, b + q.b, c + q.c, d + q.d);
   return temp;
}

template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::operator -(const Aftr::QuatT<T> &q) const
{
   Aftr::QuatT<T> temp(a - q.a, b - q.b, c - q.c, d - q.d);
   return temp;
}

template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::operator *(const Aftr::QuatT<T> &q) const
{
   //Exactly the same operations as a grassProduct
   //Qr = Qa * Qb
   //Calculates the combined rotation

   return this->grassmanProduct( q );
}

template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::grassmanProduct(const Aftr::QuatT<T> &q) const
{     
   Aftr::QuatT<T> temp( a * q.a - b * q.b - c * q.c - d * q.d,
                        a * q.b + b * q.a + c * q.d - d * q.c,
                        a * q.c - b * q.d + c * q.a + d * q.b,
                        a * q.d + b * q.c - c * q.b + d * q.a );
   return temp;
}

template< typename T >
T Aftr::QuatT<T>::innerProduct(const Aftr::QuatT<T> &q) const
{
   return a * q.a + b * q.b + c * q.c + d * q.d;
}

//template< typename T >
//Aftr::QuatT<T> Aftr::QuatT<T>::outerProduct(const Aftr::QuatT<T> &q) const
//{
//   Vector a(this->b, this->c, this->d);
//   Vector b(q.b, q.c, q.d);
//   Vector c;
//
//   c = b * this->a;
//   c = c - (a * q.a);
//   c = c - a.crossProduct(b);
//
//   Aftr::QuatT<T> temp(0, c.x, c.y, c.z);
//   return temp;
//}

template< typename T >
T Aftr::QuatT<T>::magnitude() const
{
   return this->length();
}

template< typename T >
bool Aftr::QuatT<T>::isEqual( QuatT const& a, T epsilon ) const noexcept
{
   return QuatImpl::isEqual( *this, a, epsilon );
}

template< typename T >
std::optional< Aftr::QuatT<T> > Aftr::QuatT<T>::fromString_opt( const std::string& str )
{
   return QuatImpl::fromString_opt<T>( str );
}

//template< typename T >
//Aftr::QuatT<T> Aftr::QuatT<T>::crossProduct(const Aftr::QuatT<T> &q) const
//{
//   Vector a(this->b, this->c, this->d);
//   Vector b(q.b, q.c, q.d);
//   Vector c = a.crossProduct(b);
//
//   Aftr::QuatT<T> temp(0, c.x, c.y, c.z);
//   return temp;
//}
//
//template< typename T >
//Aftr::QuatT<T> Aftr::QuatT<T>::evenProduct(const Aftr::QuatT<T> &q) const
//{
//
//   QuatT temp(a * q.a - b * q.b - c * q.c - d * q.d,
//      a * q.b + q.a * b,
//      a * q.c + q.a * c,
//      a * q.d + q.a * d);
//   return temp;
//}
//
//template< typename T >
//void Aftr::QuatT<T>::toRepresentationMatrix(T m[]) const
//{
//   m[0] = m[5] = m[10] = m[15] = a;
//   m[1] = m[14] = b;
//   m[2] = m[7] = c;
//   m[3] = m[9] = d;
//   m[4] = m[11] = -b;
//   m[6] = m[12] = -d;
//   m[8] = m[13] = -c;
//}

template< typename T >
void Aftr::QuatT<T>::toRotationMatrix(T m[]) const
{
   Mat4T<T> mat = QuatImpl::toRotationMatrix( *this );
   std::copy( mat.cbegin(), mat.cend(), m );
}

template< typename T >
Aftr::Mat4T<T> Aftr::QuatT<T>::toRotationMatrix() const
{
   return Aftr::QuatImpl::toRotationMatrix( *this );
}

template< typename T >
std::string Aftr::QuatT<T>::toString( size_t length) const
{
   return Aftr::QuatImpl::toString( *this, length );
}

template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::normalize() const
{
   T mag = this->magnitude();
   T w = a / mag;
   T x = b / mag;
   T y = c / mag;
   T z = d / mag;

   return QuatT( w, x, y, z );
}

template< typename T >
T Aftr::QuatT<T>::length() const noexcept
{
   T len = std::sqrt( a * a + b * b + c * c + d * d );
   return len;
}

template< typename T >
Aftr::VectorT<T> Aftr::QuatT<T>::getVectorXYZ() const noexcept
{
   return VectorT<T>{b, c, d};
}

template< typename T >
Aftr::VectorT<T> Aftr::QuatT<T>::getAxisOfRotation() const noexcept
{
   return this->toAxisAngle().axis();
   //auto q = this->normalize(); //Code adapted from Eigen3 library
   //T n = q.getVectorXYZ().length();
   //if( q.a < T( 0 ) )
   //   n = -n;
   //VectorT<T> axis = q.getVectorXYZ() / n;
   //return axis;
}

template< typename T >
T Aftr::QuatT<T>::getAngleOfRotationRads() const noexcept
{
   return this->toAxisAngle().rad();
   //auto q = this->normalize(); //Code adapted from Eigen3 library
   //T n = q.getVectorXYZ().length();
   //T angleRad = T( 2 ) * std::atan2( n, std::abs( q.a ) );// m_angle = Scalar( 2 ) * atan2( n, abs( q.w() ) );
   //return angleRad;
}

template< typename T >
Aftr::AxisAngleT<T> Aftr::QuatT<T>::toAxisAngle() const noexcept
{
   return Aftr::QuatImpl::toAxisAngle( *this );
}

template< typename T >
T& Aftr::QuatT<T>::operator[]( uint8_t i ) noexcept
{
   switch( i )
   {
   case 0:return this->a;
   case 1:return this->b;
   case 2:return this->c;
   case 3:return this->d;
   default:
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " - Out of bounds quat access [0,3], cannot access " << i << "\n";
      std::abort();
   }
}

template< typename T >
T Aftr::QuatT<T>::operator[]( uint8_t i ) const noexcept
{
   switch( i )
   {
   case 0:return this->a;
   case 1:return this->b;
   case 2:return this->c;
   case 3:return this->d;
   default:
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " - Out of bounds quat access [0,3], cannot access " << i << "\n";
      std::abort();
   }
}


template< typename T >
Aftr::QuatT<T> Aftr::QuatT<T>::slerp( const Aftr::QuatT<T>& startOrient, const Aftr::QuatT<T>& endOrient, T t )
{
   //T srcm[16];
   //T daftr[16];
   //aftrSetIdentityMatrix( srcm );
   //aftrSetIdentityMatrix( daftr );

   //AftrCoordinateTransforms::rotateAboutArbitraryAxis( Vector(0,1,0), 35.0f * Aftr::DEGtoRAD, daftr );
   //AftrCoordinateTransforms::rotateAboutArbitraryAxis( Vector(0,0,1), 90.0f * Aftr::DEGtoRAD, daftr );



   //QuatT thisQuat( srcm );
   //QuatT input( daftr );
   //std::cout << "src   quat " << thisQuat.toString() << "...\n";
   //std::cout << "input quat " << input.toString() << "...\n";

   //for( size_t i = 0; i < 101; ++i )
   //{
   //   T t = (T)i / (T)100;

   Aftr::QuatT<T> a = startOrient.normalize();
   Aftr::QuatT<T> b = endOrient.normalize();


   //std::cout << "A norm is " << a.toString() << "...\n";
   //std::cout << "B norm is " << b.toString() << "...\n";

   //compute "cosine of the angle" between quaternions using dot product
   T cosOmega = a.innerProduct( b );

   //if negative dot prod, negate one of input quaternions
   if( cosOmega < 0.0f )
   {
      b = b.negation();
      cosOmega *= -1.0f;
   }

   T k0 = 0.0f;
   T k1 = 1.0f;

   if( cosOmega > 0.9999f )
   {
      //very close to 1 (causes a divide by zero) - just use linear interpolation
      k0 = 1.0f - t;
      k1 = t;
   }
   else
   {
      //compute the sin of the angle using
      //trig identity sin^2 x + cos^2 x = 1
      T sinOmega = std::sqrt( 1.0f - cosOmega * cosOmega );

      //Compute the angle from its sin and cosine
      T omega = atan2f( sinOmega, cosOmega );

      //compute inverse of denominator, so we only divide once
      T oneOverSinOmega = 1.0f / sinOmega;

      //Compute interpolation parameters
      k0 = sinf( (1.0f - t) * omega ) * oneOverSinOmega;
      k1 = sinf( t * omega ) * oneOverSinOmega;
   }

   //Interpolate
   Aftr::QuatT<T> c( a.a * k0 + b.a * k1,
      a.b * k0 + b.b * k1,
      a.c * k0 + b.c * k1,
      a.d * k0 + b.d * k1 );
   c = c.normalize();
   //std::cout << "Interpolated quat is " << c.toString() << "...\n";

   //T m[16];
   //aftrSetIdentityMatrix( m );
   //c.toRotationMatrix( m );
   ////std::cout << "matrix version of quat is \n" << print4x4MatrixColumn( m ) << "\n";
   //T v[4] = {1,0,0,0};
   //T vout[4] = {0,0,0,0};

   //multiply4x4MatrixBy4x1Vector( m, v, vout );

   ////AftrAffine

   //Vector xdir( vout[0], vout[1], vout[2] );
   //Vector rpy = AftrCoordinateTransforms::transformFromDisplayMatrixToRollPitchYaw( m ).toVecS();

   //std::cout << "t " << t << ": " << xdir.toString() << " length is " << xdir.magnitude() << ", rpy is " << rpy.toString()  << "...\n";
   //}

   return c;
}