//using namespace Aftr;

//template bool Mat4::is_T_Equal<float>( float x, float y, float epsilon );
//template bool Mat4::is_T_Equal<double>( double x, double y, double epsilon );
template< typename T >
bool Aftr::Mat4T<T>::is_T_Equal( T x, T y, T epsilon ) noexcept
{
   if( std::abs( x - y ) <= epsilon )
      return true;
   return false;
}

template< typename T >
void Aftr::Mat4T<T>::setToIdentity4x4( std::array<T, NUM_ELEMS>& m ) noexcept
{
   m[0]  = 1; m[4]  = 0; m[8]  = 0; m[12] = 0;
   m[1]  = 0; m[5]  = 1; m[9]  = 0; m[13] = 0;
   m[2]  = 0; m[6]  = 0; m[10] = 1; m[14] = 0;
   m[3]  = 0; m[7]  = 0; m[11] = 0; m[15] = 1;
}

template< typename T >
Aftr::Mat4T<T>::Mat4T() noexcept
{   
   Aftr::Mat4T<T>::setToIdentity4x4( this->m );

   static_assert( sizeof( Mat4T<T> ) == sizeof( T ) * 16,
                  "Aftr Compiler Halt: Mat4T<T> MUST not change its size of 16*T.\n" \
                  "This would cause float* mat to have a different layout than std::vector<Mat4<float>>.\n" \
                  "This would ultimately hurt performance." );
}


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

template< typename T >
Aftr::Mat4T<T>::Mat4T( const T* mat4ToBeCopied ) noexcept
{
   if( mat4ToBeCopied != nullptr )
      std::copy( mat4ToBeCopied, mat4ToBeCopied + this->m.size(), this->m.begin() );
   else
      Aftr::Mat4T<T>::setToIdentity4x4( this->m );
}

template< typename T >
Aftr::Mat4T<T>::Mat4T( Aftr::AxisAngleT<T> const& a ) noexcept
{   
   *this = Mat4T<T>::rotateIdentityMat( a.axis(), a.rad() );
}

template< typename T >
Aftr::Mat4T<T>::Mat4T( Aftr::QuatT<T> const& q ) noexcept
{
   *this = q.toRotationMatrix();
}

template< typename T >
Aftr::Mat4T<T>::Mat4T( const VectorT<T>& x, const VectorT<T>& y, const VectorT<T>& z ) noexcept
{
   this->m[0] = x.x; this->m[4] = y.x; this->m[8]  = z.x; this->m[12] = 0;
   this->m[1] = x.y; this->m[5] = y.y; this->m[9]  = z.y; this->m[13] = 0;
   this->m[2] = x.z; this->m[6] = y.z; this->m[10] = z.z; this->m[14] = 0;
   this->m[3] = 0;   this->m[7] = 0;   this->m[11] = 0;   this->m[15] = 1;
}

template< typename T >
Aftr::Mat4T<T>::Mat4T( const VectorT<T>& x, const VectorT<T>& y, const VectorT<T>& z, const VectorT<T>& translation ) noexcept
{
   this->m[0] = x.x; this->m[4] = y.x; this->m[8] = z.x;  this->m[12] = translation.x;
   this->m[1] = x.y; this->m[5] = y.y; this->m[9] = z.y;  this->m[13] = translation.y;
   this->m[2] = x.z; this->m[6] = y.z; this->m[10] = z.z; this->m[14] = translation.z;
   this->m[3] = 0;   this->m[7] = 0;   this->m[11] = 0;   this->m[15] = 1;
}

template< typename T >
Aftr::Mat4T<T>::Mat4T( std::array<T, NUM_ELEMS> const& mat ) noexcept
{
   std::copy( mat.cbegin(), mat.cend(), this->m.begin());
}

template< typename T >
Aftr::Mat4T<T>::Mat4T( const std::string& str, bool inputStringIsColumnMajor ) noexcept
{
   Mat4T<T> mat = Mat4T::fromString( str, inputStringIsColumnMajor );
   *this = mat;
}

template< typename T >
void Aftr::Mat4T<T>::set( const T* mat4x4 )
{
   std::copy( mat4x4, mat4x4 + this->m.size(), this->m.begin() );
}

template< typename T >
void Aftr::Mat4T<T>::set( Mat4T<T> const& m ) noexcept
{
   this->m = m.m;
}

template< typename T >
void Aftr::Mat4T<T>::setMeToIdentity() noexcept
{
   Mat4T::setToIdentity4x4( this->m );
}

template< typename T >
Aftr::VectorT<T> Aftr::Mat4T<T>::operator *( const Aftr::VectorT<T>& v ) const noexcept
{
   Aftr::VectorT<T> w = { 0, 0, 0 };

   w.x = m[0] * v.x + m[4] * v.y + m[8]  * v.z;
   w.y = m[1] * v.x + m[5] * v.y + m[9]  * v.z;
   w.z = m[2] * v.x + m[6] * v.y + m[10] * v.z;

   return w;
}

template< typename T >
std::pair< Aftr::VectorT<T>, T > Aftr::Mat4T<T>::operator *( const std::pair< const Aftr::VectorT<T>&, T >& homogeniusCoord ) const noexcept
{
   //return std::make_pair( 1, 1.3f );

   const VectorT<T>& v = homogeniusCoord.first;
   T h = homogeniusCoord.second;
   Aftr::VectorT<T> w = { 0, 0, 0 };
   T s = 0;

   w.x = m[0] * v.x + m[4] * v.y + m[8]  * v.z + m[12] * h;
   w.y = m[1] * v.x + m[5] * v.y + m[9]  * v.z + m[13] * h;
   w.z = m[2] * v.x + m[6] * v.y + m[10] * v.z + m[14] * h;
     s = m[3] * v.x + m[7] * v.y + m[11] * v.z + m[15] * h;

   return std::make_pair( w, s );
   //return std::make_pair( w, h );
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::operator *( const Aftr::Mat4T<T>& m ) const noexcept
{
   Aftr::Mat4T<T> result;
   T* p = result.m.data();
   const T* b = this->m.data();
   const T* a = m.m.data();
   
   //we want to post multiply a by b. So we swap the order the matrices are multiplied above (ie, b is "this" and a is "m")
   for( size_t r = 0; r < 4; ++r )
      for( size_t c = 0; c < 4; ++c )
         p[r * 4 + c] = a[r * 4 + 0] * b[0 * 4 + c] +
                        a[r * 4 + 1] * b[1 * 4 + c] +
                        a[r * 4 + 2] * b[2 * 4 + c] +
                        a[r * 4 + 3] * b[3 * 4 + c];
   return result;
}

template<typename T>
Aftr::Mat4T<T> Aftr::Mat4T<T>::operator*( T const scalar ) const noexcept
{
   Mat4T<T> scaled;
   std::transform( this->m.cbegin(), this->m.cend(), scaled.begin(), [scalar](auto const v) {return v * scalar; });
   return scaled;
}

template<typename T>
Aftr::Mat4T<T> Aftr::Mat4T<T>::operator/( T const scalar ) const noexcept
{
   Mat4T<T> divided;
   std::transform( this->m.cbegin(), this->m.cend(), divided.begin(), [scalar]( auto const v ) {return v / scalar; } );
   return divided;
}

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

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::operator +(const Aftr::Mat4T<T>& m) const noexcept
{
   Aftr::Mat4T<T> result;
   std::transform( this->m.cbegin(), this->m.cend(), m.cbegin(), result.begin(), std::plus{} );
   return result;
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::operator -( const Aftr::Mat4T<T>& m ) const noexcept
{
   Aftr::Mat4T<T> result;
   std::transform( this->m.cbegin(), this->m.cend(), m.cbegin(), result.begin(), std::minus{} );
   return result;
}

template< typename T >
Aftr::Mat4T<T>& Aftr::Mat4T<T>::operator =( const Aftr::Mat4T<T>& m ) noexcept
{
   if( this != &m )
      this->m = m.m;
   return *this;
}

template< typename T >
bool Aftr::Mat4T<T>::operator ==( const Aftr::Mat4T<T>& m ) const noexcept
{
   for( size_t i = 0; i < this->m.size(); ++i )
   {
      if( std::abs( this->m[i] - m.m[i] ) > ZERO_THRESHOLD() )
         return false;
   }
   return true;
}

template< typename T >
bool Aftr::Mat4T<T>::operator !=( const Aftr::Mat4T<T>& m ) const noexcept
{
   if( (*this) == m )
      return false;
   return true;
}

template< typename T >
T& Aftr::Mat4T<T>::operator [] ( size_t i ) noexcept
{
   return this->m[i];
}

template< typename T >
T Aftr::Mat4T<T>::operator [] ( size_t i ) const noexcept
{
   return this->m[i];
}

template< typename T >
T& Aftr::Mat4T<T>::at( size_t i ) noexcept
{
   if( i > 15 )
   {
      //ERROR, replace with exception at some point
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " User attempted to access a component outside the interval [0,15].  Exiting. \n";
      std::abort();
   }
   return this->m[i];
}

template< typename T >
T Aftr::Mat4T<T>::at( size_t i ) const noexcept
{
   if( i > 15 )
   {
      //ERROR, replace with exception at some point
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " User attempted to access a component outside the interval [0,15].  Exiting. \n";
      std::abort();
   }
   return this->m[i];
}

template< typename T >
T& Aftr::Mat4T<T>::at( int row, int col ) noexcept
{
   if( row < 0 || row > 3 || col < 0 || col > 3 )
   {
      //ERROR, replace with exception at some point
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " User attempted to access a component ("<<row<<","<<col<<"). This is outside Mat4x4.  Exiting. \n";
      std::abort();
   }
   return this->m[ row * 4 + col];
}

template< typename T >
T Aftr::Mat4T<T>::at( int row, int col ) const noexcept
{
   if( row < 0 || row > 3 || col < 0 || col > 3 )
   {
      //ERROR, replace with exception at some point
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " User attempted to access a component (" << row << "," << col << "). This is outside Mat4x4.  Exiting. \n";
      std::abort();
   }
   return this->m[row * 4 + col];
}

template< typename T >
bool Aftr::Mat4T<T>::isEqual( const Aftr::Mat4T<T>& m, T tolerance ) const noexcept
{
   for( size_t i = 0; i < this->m.size(); ++i )
   {
      if( std::abs( this->m[i] - m[i] ) > tolerance )
         return false;
   }
   return true;
}

template< typename T >
bool Aftr::Mat4T<T>::isValidForEachComponent() const noexcept
{
   for( size_t i = 0; i < 16; ++i )
      if( std::isinf( this->m[i] ) || std::isnan( this->m[i] ) )
         return false;

   return true;
}

template< typename T >
T Aftr::Mat4T<T>::trace3x3() const noexcept
{
   return this->m[0] + this->m[5] + this->m[10];
}

template< typename T >
T Aftr::Mat4T<T>::trace4x4() const noexcept
{
   return this->m[0] + this->m[5] + this->m[10] + this->m[15];
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::rotate( const Aftr::VectorT<T>& axisNormalized, T angleRads ) const noexcept
{
   return Aftr::Mat4T<T>::rotateIdentityMat( axisNormalized, angleRads ) * ( *this ); // transform current matrix by rotated identity
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::rotateAboutRelX( T angleRads ) const noexcept
{
   VectorT<T> relX{ this->m[0], this->m[1], this->m[2] };
   return Aftr::Mat4T<T>::rotateIdentityMat( relX, angleRads ) * (*this);
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::rotateAboutRelY( T angleRads ) const noexcept
{
   VectorT<T> relY{ this->m[4], this->m[5], this->m[6] };
   return Aftr::Mat4T<T>::rotateIdentityMat( relY, angleRads ) * (*this);
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::rotateAboutRelZ( T angleRads ) const noexcept
{
   VectorT<T> relZ{ this->m[8], this->m[9], this->m[10] };
   return Aftr::Mat4T<T>::rotateIdentityMat( relZ, angleRads ) * (*this);
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::rotateIdentityMat( const Aftr::VectorT<T>& axisNormalized, T angleRads ) noexcept
{
   if( ! Aftr::Mat4T<T>::is_T_Equal( axisNormalized.magnitudeSquared(), static_cast<T>(1.0), static_cast<T>(0.0001) ) )
   {
      std::cout << "ERROR:" << AFTR_FILE_LINE_STR << ": Expected rotation axis must be unit length, but was passed "
         << axisNormalized.toString( 5 ) << " (magnitude " << axisNormalized.magnitude() << ")...\n"
         << "Ensure rotation axis is unit length.";
      std::abort();
   }

   Aftr::Mat4T<T> r;
   T* m = r.m.data();
   T u = axisNormalized.x;
   T v = axisNormalized.y;
   T w = axisNormalized.z;
   T t = angleRads;

   T cost = std::cos( t );
   T sint = std::sin( t );

   m[0] = u*u + ( 1.0f - u*u ) * cost;    m[4] = u*v*( 1.0f - cost ) - w * sint; m[8] = u*w*( 1.0f - cost ) + v * sint; m[12] = 0.0f;
   m[1] = u*v*( 1.0f - cost ) + w * sint; m[5] = v*v + ( 1.0f - v*v ) * cost;    m[9] = v*w*( 1.0f - cost ) - u * sint; m[13] = 0.0f;
   m[2] = u*w*( 1.0f - cost ) - v * sint; m[6] = v*w*( 1.0f - cost ) + u*sint;   m[10] = w*w + ( 1.0f - w*w )* cost;    m[14] = 0.0f;
   m[3] = 0.0f;                           m[7] = 0.0f;                           m[11] = 0.0f;                          m[15] = 1.0f;

   return r; //Since this static method only rotates the identity matrix, we do not need to accumulate r with the current matrix
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::translate( const Aftr::VectorT<T>& t ) const noexcept
{
   Aftr::Mat4T<T> trM( *this );
   VectorT<T> tr = ( *this ) * t; //transform t through upper 3x3 matrix
   trM.setPosition( trM.getPosition() + tr ); //add new translate amount to the existing translate amount
   return trM;
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::translateIdentityMat( const Aftr::VectorT<T>& t ) noexcept
{
   Aftr::Mat4T<T> tr;
   tr[12] = t.x;
   tr[13] = t.y;
   tr[14] = t.z;
   return tr;
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::scale( const Aftr::VectorT<T>& s ) const noexcept
{
   Aftr::Mat4T<T> sc( *this );
   sc.setX( sc.getX() * s.x );
   sc.setY( sc.getY() * s.y );
   sc.setZ( sc.getZ() * s.z );
   return sc;
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::scaleIdentityMat( const Aftr::VectorT<T>& s ) noexcept
{
   Aftr::Mat4T<T> sc;
   sc[0] = s.x;
   sc[5] = s.y;
   sc[10] = s.z;
   return sc;
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::transposeUpperLeft3x3() const noexcept
{
   Aftr::Mat4T<T> transpose(*this);
   T* t = transpose.m.data();
   const T* s = this->m.data();
   
   t[0] = s[0]; t[4] = s[1]; t[8]  = s[2];
   t[1] = s[4]; t[5] = s[5]; t[9]  = s[6];
   t[2] = s[8]; t[6] = s[9]; t[10] = s[10];

   return transpose;
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::transpose4x4() const noexcept
{
   Aftr::Mat4T<T> t;
   for( size_t r = 0; r < 4; ++r )
      for( size_t c = 0; c < 4; ++c )
         t[ r * 4 + c ] = ( *this )[r + c * 4 ];

   return t;
}

template< typename T >
std::string Aftr::Mat4T<T>::toString() const
{
   return this->toString( 3 );
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::fromString( const std::string& str, bool inputStringIsColumnMajor ) noexcept
{
   return Aftr::Mat4Impl::make_Mat4T<T>( str, inputStringIsColumnMajor );
}

template< typename T >
std::optional< Aftr::Mat4T<T> > Aftr::Mat4T<T>::fromString_opt( const std::string& str, bool inputStringIsColumnMajor ) noexcept
{
   return Aftr::Mat4Impl::make_Mat4T_opt<T>( str, inputStringIsColumnMajor );
}

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

template< typename T >
std::string Aftr::Mat4T<T>::toStringWithVecMagnitudes( size_t length ) const
{
   return Aftr::Mat4Impl::toStringWithVecMagnitudes( *this, length );
}

template< typename T >
Aftr::Mat4T<double> Aftr::Mat4T<T>::toMatD() const noexcept
{
   Aftr::Mat4T<double> dm;
   for( size_t i = 0; i < 16; ++i )
      dm.m[i] = static_cast<double>( this->m[i] );
   return dm;
}

template< typename T >
Aftr::Mat4T<float> Aftr::Mat4T<T>::toMatS() const noexcept
{
   Aftr::Mat4T<float> fm;
   for( size_t i = 0; i < 16; ++i )
      fm.m[i] = static_cast<float>( this->m[i] );
   return fm;
}

template< typename T >
Aftr::VectorT<T> Aftr::Mat4T<T>::getX() const noexcept
{
   Aftr::VectorT<T> x( this->m[0], this->m[1], this->m[2] );
   return x;
}

template< typename T >
Aftr::VectorT<T> Aftr::Mat4T<T>::getY() const noexcept
{
   Aftr::VectorT<T> y( this->m[4], this->m[5], this->m[6] );
   return y;
}

template< typename T >
Aftr::VectorT<T> Aftr::Mat4T<T>::getZ() const noexcept
{
   Aftr::VectorT<T> z( this->m[8], this->m[9], this->m[10] );
   return z;
}

template< typename T >
Aftr::VectorT<T> Aftr::Mat4T<T>::getPosition() const noexcept
{
   Aftr::VectorT<T> trans( this->m[12], this->m[13], this->m[14] );
   return trans;
}

template< typename T >
Aftr::VectorT<T> Aftr::Mat4T<T>::getVecCol( int i ) const noexcept
{
   if( i < 0 || i > 3 )
   {
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << ": cannot extract column " << i << ", out of bounds...\n";
      std::abort();
   }
   if( i == 0 )
      return this->getX();
   else if( i == 1 )
      return this->getY();
   else if( i == 2 )
      return this->getZ();
   else //( i == 3 );
      return this->getPosition();
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::getUpperLeft3x3AsMat4() const noexcept
{
   Aftr::Mat4T<T> mat( *this );
   T* t = mat.m.data();   
                                  t[12] = 0;
                                  t[13] = 0;
                                  t[14] = 0;
   t[3] = 0; t[7] = 0; t[11] = 0; t[15] = 1;

   return mat;
}

template< typename T >
void Aftr::Mat4T<T>::setX( const Aftr::VectorT<T>& xVec ) noexcept
{
   this->m[0] = xVec.x;
   this->m[1] = xVec.y;
   this->m[2] = xVec.z;
}

template< typename T >
void Aftr::Mat4T<T>::setY( const Aftr::VectorT<T>& yVec ) noexcept
{
   this->m[4] = yVec.x;
   this->m[5] = yVec.y;
   this->m[6] = yVec.z;
}

template< typename T >
void Aftr::Mat4T<T>::setZ( const Aftr::VectorT<T>& zVec ) noexcept
{
   this->m[8]  = zVec.x;
   this->m[9]  = zVec.y;
   this->m[10] = zVec.z;
}

template< typename T >
void Aftr::Mat4T<T>::setXYZ( const VectorT<T>& xCol, const VectorT<T>& yCol, const VectorT<T>& zCol ) noexcept
{
   this->setX( xCol );
   this->setY( yCol );
   this->setZ( zCol );
}

template< typename T >
void Aftr::Mat4T<T>::setPosition( const Aftr::VectorT<T>& trans ) noexcept
{
   this->m[12] = trans.x;
   this->m[13] = trans.y;
   this->m[14] = trans.z;
}

template< typename T >
void Aftr::Mat4T<T>::setVecCol( int i, const VectorT<T>& vec ) noexcept
{
   if( i < 0 || i > 3 )
   {
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << ": cannot insert column " << i << ", out of bounds...\n";
      std::abort();
   }
   if( i == 0 )
      return this->setX(vec);
   else if( i == 1 )
      return this->setY(vec);
   else if( i == 2 )
      return this->setZ(vec);
   else //( i == 3 );
      return this->setPosition(vec);
}

template< typename T >
void Aftr::Mat4T<T>::setScale( const Aftr::VectorT<T>& scale ) noexcept
{
   this->m[0]  = scale.x;
   this->m[5]  = scale.y;
   this->m[10] = scale.z;
}

template< typename T >
void Aftr::Mat4T<T>::multScale(const Aftr::VectorT<T>& scale) noexcept
{
   this->m[0] *= scale.x;
   this->m[5] *= scale.y;
   this->m[10] *= scale.z;
}

template< typename T >
T* Aftr::Mat4T<T>::getPtr() noexcept
{
   return this->m.data();
}

template< typename T >
const T* Aftr::Mat4T<T>::getPtr() const noexcept
{
   return this->m.data();
}

template< typename T >
T* Aftr::Mat4T<T>::data() noexcept
{
   return this->m.data();
}

template< typename T >
const T* Aftr::Mat4T<T>::data() const noexcept
{
   return this->m.data();
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::orthogonalize_3x3() const noexcept
{
   return Aftr::Mat4Impl::orthogonalize_3x3( *this );
}

template< typename T>
Aftr::Mat4T<T> Aftr::Mat4T<T>::toRotationMat_fromSkewSymmetricMat() const noexcept
{
   // |  0 -z  y  0 |  This is the expected form of the skew symmetric matrix.
   // |  z  0 -x  0 |  If the matrix does *not* have this form, this
   // | -y  x  0  0 |  function ***WILL NOT RETURN A VALID VALUE***.
   // |  0  0  0  1 |

   // |  0  4  8 12 |
   // |  1  5  9 13 |
   // |  2  6 10 14 |
   // |  3  7 11 15 |

   if( !this->isSkewSymmetric() )
   {
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << ": Called on non-skewsymmetric matrix will produce garbage... Opting to crash here for fun...\n";
      std::abort();
   }

   T x( this->m[6] );
   T y( this->m[8] );
   T z( this->m[1] );
   VectorT<T> axis{ x,y,z };
   if( axis.isEqual( { 0,0,0 } ) )
      return {};//return identity if passed no angular rotation

   T mag_rad = axis.magnitude();
   Mat4T<T> r = Mat4T<T>::rotateIdentityMat( axis.normalizeMe(), mag_rad );
   return r;
}

template< typename T>
Aftr::Mat4T<T> Aftr::Mat4T<T>::toSkewSymmetricMat_fromRotationMat() const noexcept
{
   // |  0 -z  y  0 |  This is the expected form of the skew symmetric matrix.
   // |  z  0 -x  0 |  If the matrix does *not* have this form, this
   // | -y  x  0  0 |  function ***WILL NOT RETURN A VALID VALUE***.
   // |  0  0  0  1 |

   return this->toRodriguesVec_fromRotationMat().toSkewSymmetric3x3();
}


template< typename T>
Aftr::VectorT<T> Aftr::Mat4T<T>::toRodriguesVec_fromSkewSymmetricMat() const noexcept
{
   // |  0 -z  y  0 |  This is the expected form of the skew symmetric matrix.
   // |  z  0 -x  0 |  If the matrix does *not* have this form, this
   // | -y  x  0  0 |  function ***WILL NOT RETURN A VALID VALUE***.
   // |  0  0  0  1 |

   // |  0  4  8 12 |
   // |  1  5  9 13 |
   // |  2  6 10 14 |
   // |  3  7 11 15 |
   if( !this->isSkewSymmetric() )
   {
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << ": Called on non-skewsymmetric matrix will produce garbage... Opting to crash here for fun...\n";
      std::abort();
   }

   T x( this->m[6] );
   T y( this->m[8] );
   T z( this->m[1] );
   return VectorT<T>{x, y, z};
}

template< typename T>
bool Aftr::Mat4T<T>::isSkewSymmetric() const noexcept
{
   // |  0 -z  y  0 |  This is the expected form of the skew symmetric matrix.
   // |  z  0 -x  0 |  If the matrix does *not* have this form, this
   // | -y  x  0  0 |  function ***WILL NOT RETURN A VALID VALUE***.
   // |  0  0  0  1 |

   // |  0  4  8 12 |
   // |  1  5  9 13 |
   // |  2  6 10 14 |
   // |  3  7 11 15 |

   return Mat4Impl::isSkewSymmetric( *this );
}

template< typename T>
Aftr::VectorT<T> Aftr::Mat4T<T>::toRodriguesVec_fromRotationMat() const noexcept
{

   // |  0  4  8 12 |
   // |  1  5  9 13 |
   // |  2  6 10 14 |
   // |  3  7 11 15 |
   return Mat4Impl::toRodriguesVec_fromRotationMat( *this );


   //https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
   //https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac
   // |  0 -z  y  0 |  This is the expected form of the skew symmetric matrix.
   // |  z  0 -x  0 |  If the matrix does *not* have this form, this
   // | -y  x  0  0 |  function ***WILL NOT RETURN A VALID VALUE***.
   // |  0  0  0  1 |
   Mat4T<T> A = ( (*this) - this->transpose4x4()) / T( 2.0 );
   VectorT<T> rho( A[6], A[8], A[1] ); //ax,ay,az from matrix A
   T s = rho.length(); //length in radians of rho
   T c = (m[0] + m[5] + m[10] - T( 1.0 )) / T( 2.0 ); //trace of original R matrix minus 1, divided by two

   if( std::abs( s ) <= T( 0.00001 ) &&
       std::abs( c - T( 1.0 ) ) <= T( 0.00001 ) )
   { //if s == 0 and c == 1, then r = 0 (rotMat is identity)
      VectorT<T> r( 0, 0, 1 );//identity
      return r; //sln not thoroguly tested, if you hit this line of code, I bet your input is not a valid ortho DCM
   }

   //if( std::abs( s ) <= T( 0.00001 ) &&
   //    std::abs( c + T( 1.0 ) ) <= T( 0.00001 ) )
   //{ //if s == 0 and c == -1, then r = 0 (rotMat is identity)
   //   const Mat4T<T> reflectionMat = ((*this) + Mat4T<T>());
   //   VectorT<T> u = reflectionMat.getX().normalizeMe();
   //   if( u.magnitudeSquared() == 0 ) //normalizeMe() returns EXACTLY 0,0,0 when vector is empty!
   //      u = reflectionMat.getY().normalizeMe();
   //   if( u.magnitudeSquared() == 0 ) //normalizeMe() returns EXACTLY 0,0,0 when vector is empty!
   //      u = reflectionMat.getZ().normalizeMe(); //Z *must* be non-zero if X and Y were zero

   //   if( ( u.x == u.y && u.z < 0 ) || //flips the sign of u based on location in half-hemisphere
   //       ( u.x == 0 && u.y < 0 ) ||
   //       ( u.x < 0 ) )
   //   {
   //      u = -u; //sln not thoroguly tested, if you hit this line of code, I bet your input is not a valid ortho DCM
   //   }

   //   VectorT<T> r = u * T( Aftr::PId ); //normalize unit length u to length PI
   //   return r;
   //}

   //finally, if sin theta != 0
   VectorT<T> u = rho / s;
   T theta = std::atan2( s, c );
   VectorT<T> r = u * theta;
   return r;
}

template< typename T >
T Aftr::Mat4T<T>::getAngleOfRotationAboutAxisOfRotationRads() const noexcept
{
   T cos_theta = (m[0] + m[5] + m[10] - T( 1.0 )) / T( 2.0 ); //trace of original R matrix minus 1, divided by two
   T theta = std::acos( cos_theta );
   return theta;
}

template< typename T >
Aftr::VectorT<T> Aftr::Mat4T<T>::getAxisOfRotation_fromRotationMat() const noexcept
{
   return Aftr::Mat4Impl::getAxisOfRotation_fromRotationMat( *this );
}

template< typename T >
Aftr::AxisAngleT<T> Aftr::Mat4T<T>::toAxisAngle_fromRotationMat() const noexcept
{
   return Aftr::Mat4Impl::toAxisAngle_fromRotationMat( *this );
}

template< typename T >
Aftr::AxisAngleT<T> Aftr::Mat4T<T>::toAxisAngle_fromSkewSymmetricMat() const noexcept
{
   return Aftr::Mat4Impl::toAxisAngle_fromSkewSymmetricMat( *this );
}

template<typename T>
Aftr::QuatT<T> Aftr::Mat4T<T>::toQuat_fromRotationMat() const noexcept
{
   if( this->isSkewSymmetric() )
   {
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " This current Mat4 is *not* a rotation matrix -- it\n"
         "looks like a skew symmetric, perhaps you meant to call toQuat_fromSkewSymmetricMat() or try\n"
         "reorthogonalizing the Mat4 prior to calling this method... Perhaps numerical accuracy is\n"
         "creeping into this matrix... Gonna crash here for your inspection / callstack... Have fun...\n";
      //matrix, this quaternion will not represent the proper orientation.
      std::abort();
   }
   Aftr::QuatT<T> q( *this );
   return q;
}

template<typename T>
Aftr::QuatT<T> Aftr::Mat4T<T>::toQuat_fromSkewSymmetricMat() const noexcept
{
   auto aa = this->toAxisAngle_fromSkewSymmetricMat();
   Aftr::QuatT<T> q( aa );
   return q;
}

template< typename T >
Aftr::VectorD Aftr::Mat4T<T>::getRPY_deg_clockwise() const noexcept
{
   return Aftr::Mat4Impl::getRPY_deg_clockwise( *this );
}

template< typename T >
Aftr::VectorD Aftr::Mat4T<T>::getRPY_deg_counter_clockwise() const noexcept
{
   return Aftr::Mat4Impl::getRPY_deg_counter_clockwise( *this );
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::fromRPY_deg_clockwise( const VectorT<T>& rpyDeg ) noexcept
{
   return Aftr::Mat4Impl::fromRPY_deg_clockwise( rpyDeg );
}

template< typename T >
Aftr::Mat4T<T> Aftr::Mat4T<T>::swizzle_upper_3x3( swizzle_to const swiz ) const noexcept
{
   Mat4 cpy{ *this };
   switch( swiz )
   {
   case swizzle_to::xyz: cpy.setXYZ( getX(), getY(), getZ() ); return cpy;
   case swizzle_to::xzy: cpy.setXYZ( getX(), getZ(), getY() ); return cpy;

   case swizzle_to::yxz: cpy.setXYZ( getY(), getX(), getZ() ); return cpy;
   case swizzle_to::yzx: cpy.setXYZ( getY(), getZ(), getX() ); return cpy;

   case swizzle_to::zxy: cpy.setXYZ( getZ(), getX(), getY() ); return cpy;
   case swizzle_to::zyx: cpy.setXYZ( getZ(), getY(), getX() ); return cpy;
   default:
      std::cout << "ERROR: " << AFTR_FILE_LINE_STR << " INVALID swizzle_to param. Exiting. \n";
      std::abort();
   }
}