//----------------------------------------------------------------------------
//Description: The Mat4T class is a utility class for reference frame / matrix
//rotations, scalings, and translations. This matrix class is column major and
//assumes all in passed Vectors are also column major. As a result, this class
//performs post multiplication against other Mat4s and Vectors.
//
//Useful operations are also provided to manipulate a Matrices and verticies.
//Additionally, the BIG 3 are implemented.
//
//This class is defined as a templated class that has a user-specified
//storage type. The most expected user specified types are float, double,
//and other floating point types (perhaps long double)?. Either way,
//since float and double are considered the "most common" usages, this
//Mat4T class declares two aliases as defined in MatrixFwd.h. In this file
//Matrix is an alias to Mat4T<float> and MatrixD is an alias to 
//Mat4T<double>.
//
//To PREDECLARE this class in a header file, simply #include "MatrixFwd.h"
//Otherwise, the user will have to retype the alias definitions made
//within MatrixFwd.h.
//
//Author: Scott Nykl
//----------------------------------------------------------------------------
#pragma once
#include "AftrConfig.h"
#include "AftrGlobals.h"
#include "Mat4Fwd.h"
#include "VectorFwd.h"
#include "Vector_Mat4_Common.h"
#include "QuatFwd.h"
#include "Vector.h"
#include "Mat4Impl.h"
#include "Quat.h"

#include <string>
#include <cmath>
#include <iostream>
#include <array>
#include <algorithm>

namespace Aftr
{

template< typename T >
class Mat4T
{
public:

   using value_type = T;
   static constexpr size_t NUM_ELEMS = 16;

   static void setToIdentity4x4( std::array<T, NUM_ELEMS>& m ) noexcept;

   ///------------------------------------------------------------------------
   /// Column-major 4x4 matrix default constructor;  initializes to identity.
   ///------------------------------------------------------------------------
   Mat4T() noexcept;
   
   ///------------------------------------------------------------------------
   /// Copy constructor creates a copy of the passed in Matrix.
   /// const Mat4T& m: the Matrix to be copied.
   ///------------------------------------------------------------------------
   Mat4T( const Mat4T& m ) noexcept;

   ///-------------------------------------------------------------------------
   /// Constructor which creates a Matrix from the first 16 elements from the
   /// in passed T* array. T* must contain at least 16 elements or the behavior
   /// is undefined. The in passed T* is only copied so ownership is 
   /// NOT passed to this instance.
   ///-------------------------------------------------------------------------
   explicit Mat4T( const T* Mat4ToBeCopied ) noexcept;

   ///-------------------------------------------------------------------------
   /// Constructor which creates a Rotation Matrix (DCM / SO(3)) from the in
   /// passed AxisAngle instance. Note this does NOT create a skew symmetric
   /// matrix, but a normal Rotation Matrix.
   ///-------------------------------------------------------------------------
   Mat4T( AxisAngleT<T> const& a ) noexcept;

   ///-------------------------------------------------------------------------
   /// Constructor which creates a Rotation Matrix (DCM / SO(3)) from the in
   /// passed Quaternion instance. Note this does NOT create a skew symmetric
   /// matrix, but a normal Rotation Matrix. Same as calling 
   /// q.toRotationMatrix().
   ///-------------------------------------------------------------------------
   Mat4T( QuatT<T> const& q ) noexcept;

   ///------------------------------------------------------------------------
   /// Constructor which creates a Mat4T from the passed in string.
   /// String str: The str which is parsed to create the Mat4T.
   /// The input format is a string containing at least 16 numbers. A number may
   /// look like: 1.2 0.2 2 .3 1.2e+3 -.3 45.334E-32, etc.
   /// The numbers can be separated by any separator or set of separators that
   /// do not include a format matched by above.
   /// Specifically, given a string, the first 16 matches found by the regular 
   /// expression: "[-+]?[0-9]*\\.?[0-9]+([eE][-+]?[0-9]+)?"
   /// will be used as input.
   /// 
   /// If the input string contains less than 2 non-starting, non-terminating,
   /// non-adjacent NEWLINES ('\n') then the input string is considering one
   /// contiguous list of numbers AND:
   ///   -If the second parameter is true (default), the INPUT STRING is
   ///    interpreted as COLUMN major. Since the Mat4 is also column major,
   ///    the **columns** of the input string will be placed in the corresponding
   ///    columns of the Mat4. See examples below.
   /// 
   ///   -If the second parameter is false, the INPUT STRING is interpreted as
   ///    ROW major. Since Mat4 is column major, the constructed Mat4 will
   ///    place each input **row** into the corresponding column of the Mat4.
   /// 
   /// If the input string contains more than 2 non-starting, non-terminating,
   /// non-adjacent NEWLINES ('\n') then:
   ///   -If the second parameter is true, the each \n-terminated substring
   ///    will be interpreted a one COLUMN vector, See m_b in example below.
   ///   -If the second parameter is false, the each \n terminated substring
   ///    will be interpreted as one ROW vector.
   /// 
   /// However, the user must determine if the input string is specified in
   /// ROW major or COLUMN major form. Naturally, this isn't as simple as it
   /// may sound -- consider the following example:
   /// 
   /// std::string m_a = "1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16\n";
   /// std::string m_b = R"( 1 5  9 13\n   // notice more than 2 interleaved
   ///                       2 6 10 14\n   // \n. Each \n denotes one vector
   ///                       3 7 11 15\n   // that will be come a column (2nd param == true)
   ///                       4 8 12 16 )"; // or become a row (2nd param == false)
   /// Both m_a and m_b will produce the same Mat4 with following values:
   /// Mat4 a{ m_a, true }; //a and b have identical values
   /// Mat4 b{ m_b, true }; //a and b have identical values
   /// a.toString() -> "1 5  9 13 //X col is 1 2 3 4, Y col is 5 6 7 8
   ///                  2 6 10 14 //Z col is 9 10 11 12, pos is 13 14 15. 
   ///                  3 7 11 15
   ///                  4 8 12 16";
   /// 
   /// Now consider passing false as the second argument:
   /// Mat4 a{ m_a, false }; //a and b have identical values
   /// Mat4 b{ m_b, false }; //a and b have identical values
   /// a.toString() -> "1   2  3  4 //X col is 1 5 9 13, Y col is 2 6 10 14
   ///                  5   6  7  8 //Z col is 3 7 11 15, pos is 4 8 12. 
   ///                  9  10 11 12
   ///                  13 14 15 16";
   /// 
   /// Another example, one can also use ; instead of newlines to quickly
   /// create a Mat4:
   /// Mat4 a{ "1 2 3 4; 5 6 7 8; 9 10 11 12; 13 14 15 16", true }; //default
   /// a.toString() -> " 1   2  3  4 //X col is 1 5 9 13, Y col is 2 6 10 14
   ///                   5   6  7  8 //Z col is 3 7 11 15, pos is 4 8 12
   ///                   9  10 11 12
   ///                   13 14 15 16" //cols of input string become cols of Mat4
   /// 
   /// Mat4 b{ "1 2 3 4; 5 6 7 8; 9 10 11 12; 13 14 15 16", false };
   /// b.toString() -> " 1 5  9 13 //X col is 1 2 3 4, Y col is 5 6 7 8
   ///                   2 6 10 14 //Z col is 9 10 11 12, pos is 13 14 15
   ///                   3 7 11 15
   ///                   4 8 12 16" //rows of input string become cols of Mat4
   ///------------------------------------------------------------------------
   Mat4T( const std::string& str, bool inputStringIsColumnMajor = true ) noexcept;

   Mat4T( const VectorT<T>& x, const VectorT<T>& y, const VectorT<T>& z ) noexcept;

   Mat4T( const VectorT<T>& x, const VectorT<T>& y, const VectorT<T>& z, const VectorT<T>& translation ) noexcept;

   /// Performs pair-wise index assignment. That is, this->data[i] = m[i] for
   /// all elements of m. Mat4T are column major, so element 0, 1, 2 should
   /// be interpreted as the first 3 elements of the x column vector.
   Mat4T( std::array<T, NUM_ELEMS> const& m ) noexcept;

   ///------------------------------------------------------------------------
   /// Destructor
   ///------------------------------------------------------------------------
   ~Mat4T() noexcept {};

   ///------------------------------------------------------------------------
   /// Copies 16 values from the input into this Mat4's internal matrix.
   ///------------------------------------------------------------------------
   void set( const T* mat4x4 );
   void set( Mat4T<T> const& m ) noexcept;

   ///------------------------------------------------------------------------
   /// Resets the X,Y,Z values to 0.
   ///------------------------------------------------------------------------
   void setMeToIdentity() noexcept;

   ///------------------------------------------------------------------------
   /// Multiplies the upper 3x3 portion of this matrix by the in passed vector
   /// and returns the resultant Vector. The translation column of this matrix
   /// is NOT added to the result; that is, a Vector (direction only) is
   /// returned. If you would like to transform a vertex, either add the
   /// fourth column of this matrix to the result, or use a Vector4 instance.
   /// Optionally, you may also use the operator * which takes an 
   /// std::pair< Vector<T>, T >, where the second item is the 4th coordinate
   /// (ie, the 'w' in the x,y,z,w in passed values).
   /// Returns: Vector: the column vector transformed by the upper 3x3 of this
   /// submatrix of this matrix; the translation column (4th column) of this
   /// matrix is NOT added to the result.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   VectorT<T> operator *( const VectorT<T>& v ) const noexcept;

   ///------------------------------------------------------------------------
   /// Multiplies a 4 element column major vector by this matrix and returns
   /// a 4 element column major vector. The 4 element vector is broken into 
   /// two parts: A) the first part of the pair, which defines the X,Y,Z
   /// components, and B) the second part of the pair, which defines the X
   /// component.
   /// The resultant 4 element vector is return as an std::pair in the same
   /// format as the input.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   std::pair< VectorT<T>, T > operator *( const std::pair< const VectorT<T>&, T >& homogeniusCoord ) const noexcept;

   ///------------------------------------------------------------------------
   /// Multiplies a 4 element column major vector by this matrix and returns
   /// a 3 
   ///------------------------------------------------------------------------
   ///VectorT<T> multiply4ElementVec( const VectorT<T>& v, T h ) const;
   
   //Scott: contemplate equivalent "translateMe" functions which manipulate the existing object in equivalent ways
   //not sure if this class is implicitly using move semantics, so unsure if this will improve performance in
   //cases where old values are not needed, wasn't immediately obvious as a user that these wouldn't
   //translate the object they were called on
   [[nodiscard]]
   Mat4T<T> translate( const VectorT<T>& t ) const noexcept;
   static Mat4T<T> translateIdentityMat( const VectorT<T>& t ) noexcept;

   ///Scale scales the x, y, and z components of a copy of this by s.x, s.y, and s.z and returns it, to assign the diagonal use setScale()
   [[nodiscard]]
   Mat4T<T> scale( const VectorT<T>& s ) const noexcept;
   [[nodiscard]]
   static Mat4T<T> scaleIdentityMat( const VectorT<T>& s ) noexcept;

   [[nodiscard]]
   Mat4T<T> transposeUpperLeft3x3() const noexcept;
   [[nodiscard]]
   Mat4T<T> transpose4x4() const noexcept;
   /// Identical to calling transpose4x4(), just shorter notation.
   [[nodiscard]]
   Mat4T<T> tp() const noexcept { return this->transpose4x4(); }

   ///------------------------------------------------------------------------
   /// Returns this Matrix rotated about axisNormalied by angleRads radians.
   /// Internally, an identity matrix is rotated by the in passed params and
   /// then is post multiplied by this matrix.
   /// Return: this matrix rotated by the in passed parameters.
   /// returnMat := rotatedIdentity * (*this); (poaftrultiplication)
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> rotate( const VectorT<T>& axisNormalized, T const angleRads ) const noexcept;

   ///------------------------------------------------------------------------
   /// Returns this Matrix rotated about its current relative X axis. Mat4 is
   /// always column major and post multiplied from right to left ( L <- R ).
   /// Equiv to:
   /// returnMat := rotatedIdentity_by_relX * (this_Mat) //Postmultiplied
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> rotateAboutRelX( T const angleRads ) const noexcept;

   ///------------------------------------------------------------------------
   /// Returns this Matrix rotated about its current relative Y axis. Mat4 is
   /// always column major and post multiplied from right to left ( L <- R ).
   /// Equiv to:
   /// returnMat := rotatedIdentity_by_relY * (this_Mat) //Postmultiplied
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> rotateAboutRelY( T const angleRads ) const noexcept;

   ///------------------------------------------------------------------------
   /// Returns this Matrix rotated about its current relative Z axis. Mat4 is
   /// always column major and post multiplied from right to left ( L <- R ).
   /// Equiv to:
   /// returnMat := rotatedIdentity_by_relZ * (this_Mat) //Postmultiplied
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> rotateAboutRelZ( T const angleRads ) const noexcept;

   ///------------------------------------------------------------------------
   /// A static method that always rotates the identity matrix about the in
   /// passed axis by the specified radian amount. This is identical to 
   /// Mat4T<T>::rotate when ::rotate is called on a matrix set to identity.
   /// This method is faster than ::rotate since no internal accumulation 
   /// has to take place; ie, this requires one less matrix multiplication
   /// than rotate.
   /// Returns an identity reference frame rotated about axisNormalized by
   /// angleRad radians.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   static Mat4T<T> rotateIdentityMat( const VectorT<T>& axisNormalized,
                                      T const angleRads ) noexcept;

   ///------------------------------------------------------------------------
   /// Post Multiplies this Matrix by m and returns the result.
   /// Result := thisMatrix * m (column-major using post multiplication).
   /// Returns: Mat4T: the product of this matrix and Matrix m.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> operator*( Mat4T const& m ) const noexcept;

   ///------------------------------------------------------------------------
   /// Scales each element in this Matrix by Scalar m and returns the result.
   /// Result := thisMatrix * m (column-major using post multiplication).
   /// Returns: Mat4T: the product of this matrix and Matrix m.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> operator*( T const scalar ) const noexcept;

   ///------------------------------------------------------------------------
   /// Divides each element in this Matrix by Scalar m and returns the result.
   /// Mat4 containing element-wise division by scalar.
   /// Returns: Mat4T: the product of this matrix and Matrix m.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> operator/( T const scalar ) const noexcept;

   ///------------------------------------------------------------------------
   /// Post Multiplies this Matrix by m and returns the result.
   /// thisMatrix := m * thisMatrix (column-major using post multiplication).
   /// Returns: Mat4T: the product of this Matrix m and this matrix.
   /// This enables rotations like:
   ///
   /// thisMat *= Mat4::rotateIdentityMat( {0,0,1}, 75.0f*Aftr::DEGtoRAD );
   /// Will rotate thisMat about the global +Z axis 75 degrees.
   ///
   /// thisMat *= Mat4::rotateIdentityMat( m.getZ(), 75.0f*Aftr::DEGtoRAD );
   /// Will rotate thisMat about its local +Z axis 75 degrees.
   ///------------------------------------------------------------------------
   Mat4T<T>& operator *=( const Mat4T& m ) noexcept;

   ///------------------------------------------------------------------------
   /// Component wise addition of the two matrices.
   /// Result := thisMatrix + m
   /// Returns: Mat4T: the product of this matrix and Matrix m.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> operator +(const Mat4T& m) const noexcept;

   ///------------------------------------------------------------------------
   /// Component wise subtraction of the two matrices.
   /// Result := thisMatrix - m
   /// Returns: Mat4T: the difference between this matrix and Matrix m.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> operator -( const Mat4T& m ) const noexcept;

   ///------------------------------------------------------------------------
   /// Insertion operator enabling a Mat4T to be read into std::cout << myVec.
   ///------------------------------------------------------------------------
   friend std::ostream& operator <<( std::ostream& out, const Mat4T& m )
   {
      out << m.toString();
      return out;
   }

   ///------------------------------------------------------------------------
   ///Assignment operator assigns the in passed Mat4T's data equal to this 
   ///Mat4T.
   ///m: the Matrix who's data will be used to fill this Mat4T.
   ///------------------------------------------------------------------------
   Mat4T<T>& operator =( const Mat4T& mat ) noexcept;

   ///------------------------------------------------------------------------
   /// Returns true if each component falls within a specified epsilon
   /// to each other.
   /// PARAM const Mat4T& m: the Matrix who's values will be compared to
   /// the values of this Mat4T.
   /// RETURN: True if the X,Y,Z values lie within an epsilon to each other;
   /// false, otherwise.
   ///------------------------------------------------------------------------
   bool operator ==( const Mat4T& m ) const noexcept;

   bool operator !=( const Mat4T<T>& m ) const noexcept;

   T& operator[]( size_t i ) noexcept;
   T operator[]( size_t i ) const noexcept;
   T& at( size_t i ) noexcept;
   T at( size_t i ) const noexcept;
   T& at( int row, int col ) noexcept;
   T at( int row, int col ) const noexcept;

   bool isEqual( const Mat4T& m, T tolerance = static_cast<T>( 0.00001 ) ) const noexcept;
   static bool is_T_Equal( T x, T y, T epsilon ) noexcept;

   ///------------------------------------------------------------------------
   /// Returns true if each element in this 4x4 matrix is not infinite or not
   /// NaN.
   ///------------------------------------------------------------------------
   bool isValidForEachComponent() const noexcept;

   ///------------------------------------------------------------------------
   /// Returns the sum of the diagonal elements of the upper 3x3 matrix. This
   /// is also known as the trace of the matrix. This is elements
   /// [0] + [5] + [10], which is identical to elements 
   /// at(0,0) + at(1,1) + at(2,2).
   /// |  0  4  8 12 | //single idx  |  0,0  0,1  0,2  0,3 | //row,col idx
   /// |  1  5  9 13 |               |  1,0  1,1  1,2  1,3 |
   /// |  2  6 10 14 |               |  2,0  2,1  2,2  2,3 |
   /// |  3  7 11 15 |               |  3,0  3,1  3,2  3,3 |
   ///------------------------------------------------------------------------
   T trace3x3() const noexcept;

   ///------------------------------------------------------------------------
   /// Returns the sum of the diagonal elements of the upper 3x3 matrix. This
   /// is also known as the trace of the matrix. This is elements
   /// [0] + [5] + [10] + [15], which is identical to elements 
   /// at(0,0) + at(1,1) + at(2,2) + at(3,3).
   /// |  0  4  8 12 | //single idx  |  0,0  0,1  0,2  0,3 | //row,col idx
   /// |  1  5  9 13 |               |  1,0  1,1  1,2  1,3 |
   /// |  2  6 10 14 |               |  2,0  2,1  2,2  2,3 |
   /// |  3  7 11 15 |               |  3,0  3,1  3,2  3,3 |
   ///------------------------------------------------------------------------
   T trace4x4() const noexcept;

   ///------------------------------------------------------------------------
   /// Returns a string representation of the current Mat4T.
   /// Return: string: "(X, Y, Z)" representing the current Mat4T.
   ///------------------------------------------------------------------------
   std::string toString() const;
   std::string toString( size_t length ) const;
   std::string toStringWithVecMagnitudes( size_t length ) const;

   ///------------------------------------------------------------------------
   /// Follows exact same semantics as constructor with same input parameters.
   ///------------------------------------------------------------------------
   static Mat4T<T> fromString( const std::string& str, bool inputStringIsColumnMajor = true ) noexcept;
   static std::optional< Mat4T<T> > fromString_opt( const std::string& str, bool inputStringIsColumnMajor = true ) noexcept;


   //Returns a double precision version of this single precision Mat4T.
   Mat4T< double > toMatD() const noexcept;
   Mat4T< float > toMatS() const noexcept;

   [[nodiscard]] VectorT<T> getX() const noexcept;
   [[nodiscard]] VectorT<T> getY() const noexcept;
   [[nodiscard]] VectorT<T> getZ() const noexcept;
   [[nodiscard]] VectorT<T> getPosition() const noexcept; ///< Returns the position column stored in this matrix. That is, the X,Y,Z values in the 4th column (elements 12,13,14)
   [[nodiscard]] VectorT<T> getVecCol( int i ) const noexcept; ///< Returns the ith column vector in this 4x4 matrix (0=X, 1=Y, 2=Z, 3=Pos)

   [[nodiscard]]
   Mat4T<T> getUpperLeft3x3AsMat4() const noexcept;

   void setX( const VectorT<T>& xVec ) noexcept;
   void setY( const VectorT<T>& yVec ) noexcept;
   void setZ( const VectorT<T>& zVec ) noexcept;
   void setXYZ( const VectorT<T>& xCol, const VectorT<T>& yCol, const VectorT<T>& zCol ) noexcept;
   void setPosition( const VectorT<T>& transVec ) noexcept; ///< Sets the X,Y,Z values of the 4th column to in passed value, does not transform through upper 3x3
   void setVecCol( int i, const VectorT<T>& vec ) noexcept; ///< Sets the ith column vector in this 4x4 matrix (0=X, 1=Y, 2=Z, 3=Pos)
   void setScale( const VectorT<T>& scaleVec ) noexcept; ///< Sets the Diagonal values (matrix's trace) of the first 3 columns to in passed value. It overwrites the x,y,z scales.
   void multScale(const VectorT<T>& scaleVec) noexcept;///< Multiplies the diagonal values of the first 3 columns to in passed value.  Effectively the operation of now depricated glScale*()

   ///------------------------------------------------------------------------
   // Assuming the upper 3x3 of this MatT<T> is a DCM representing an 
   // orthogonal basis set spanning 3 dimensions, this method will normalize 
   // each vector and ensure they are perpendicular to each other. This is 
   // helpful when many accumulations occur on the same matrix and numerical 
   // errors lead to de-orthogonalization. Interally, this method converts the
   // Upper 3x3 into a Quaternion, normalizes the quaternion, and then 
   // converts the quaternion to a Mat4T<T>. The 4th column (position) is 
   // simply passed through to the return value -- it does not affect 
   // orthogonalization.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> orthogonalize_3x3() const noexcept;

   ///------------------------------------------------------------------------
   // Assuming the upper 3x3 of this MatT<T> is a skew symmetric matrix
   // representing a infintesimal rotation about an axis of rotation, this
   // method returns the corresponding rotation matrix.
   // The magnitude of the 3 values
   // defines the radian counter clockwise (right handed) rotation of the
   // resultant rotation matrix. The 4th row and column are ignored and the
   // returned rotation matrix has identity 4th row/col.
   // 
   // |  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 |
   // 
   // This skew symmetric matrix is *also* the derivative of a rotation matrix.
   // Thus exponentiating the skew symmetric matrix is the same as computing
   // its integral which results in a rotation matrix.
   // 
   // This is the same as exponentiating a SO3 skew matrix matrix. which is the
   // same as taking the "derivating"
   // 
   // This is the same a OpenCV's rodrigues ( cv::rodrigues ) function.
   // https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> toRotationMat_fromSkewSymmetricMat() const noexcept;
   [[nodiscard]]
   Mat4T<T> toSkewSymmetricMat_fromRotationMat() const noexcept;
   [[nodiscard]]
   Mat4T<T> exponentiate_toSkewSymmetric_fromRotMat() const noexcept { return this->toSkewSymmetric_fromRotationMat(); }
   
   ///------------------------------------------------------------------------
   // Returns true iff this is a valid skew symmetric (also known as
   // antisymmetric matrix). Specifically a skew symmetric corresponding to
   // the log of an SO(3) Rotation matrix. The exponentiation of a skew
   // symmetric matrix is an SO(3) Rotation Matrix (a 3D DCM) / Rotation
   // matrix.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   bool isSkewSymmetric() const noexcept;

   ///------------------------------------------------------------------------
   // Assuming the upper 3x3 of this MatT<T> is a proper ROTATION matrix,
   // this method returns the corresponding rotation vector as an axis with
   // length corresponding to the radian counter-clockwise rotation about
   // that axis.
   // 
   // This is the same a OpenCV's rodrigues ( cv::rodrigues ) function.
   // https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac
   ///------------------------------------------------------------------------
   [[nodiscard]]
   VectorT<T> toRodriguesVec_fromSkewSymmetricMat() const noexcept;
   [[nodiscard]]
   VectorT<T> toRodriguesVec_fromRotationMat() const noexcept;

   ///------------------------------------------------------------------------
   // Assuming the upper 3x3 of this MatT<T> is a proper ROTATION matrix,
   // this method returns the Radian angle this DCM is rotated about its
   // current axis of rotation. This is the same as getting the Rodrigues
   // vector and inspecting the length of the returned X,Y,Z. This method,
   // however, performs less work that creating a full rodrigues vector.
   // 
   // Returns radian rotation distance about the single rotation axis 
   // contained in this rotation matrix.
   // 
   // https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac
   ///------------------------------------------------------------------------
   [[nodiscard]]
   T getAngleOfRotationAboutAxisOfRotationRads() const noexcept;

   [[nodiscard]]
   VectorT<T> getAxisOfRotation_fromRotationMat() const noexcept;


   ///------------------------------------------------------------------------
   /// Assumes this is a ROTATION Matrix (NOT A SKEW SYMMETRIC Matrix).
   /// If this is called on a skew symmetric matrix, an improper result will
   /// be returned.
   /// Returns an Axis/Angle corresponding to the orientation defined within
   /// the current SO(3) rotation matrix (DCM, Display Matrix). The last
   /// column / bottom row of the 4x4 are ignored, only the upper 3x3 is
   /// used.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   AxisAngleT<T> toAxisAngle_fromRotationMat() const noexcept;
   [[nodiscard]]
   AxisAngleT<T> toAxisAngle_fromSkewSymmetricMat() const noexcept;

   ///------------------------------------------------------------------------
   /// Assumes this is a ROTATION Matrix (NOT A SKEW SYMMETRIC Matrix).
   /// If this is called on a skew symmetric matrix, an improper result will
   /// be returned.
   /// Returns a Quaternion corresponding to the orientation defined within
   /// the current SO(3) rotation matrix (DCM, Display Matrix). The last
   /// column / bottom row of the 4x4 are ignored, only the upper 3x3 is
   /// used.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   QuatT<T> toQuat_fromRotationMat() const noexcept; //sln do me
   [[nodiscard]]
   QuatT<T> toQuat_fromSkewSymmetricMat() const noexcept; //sln do me

   ///------------------------------------------------------------------------
   /// Returns the Euler angles represented by the column major X,Y,Z axes,
   /// respectively. Assumes the YAW rotation is clockwise (mapping to compass
   /// heading).
   ///    YAW of 0 is parallel to +x axis, 
   ///    YAW of 90 is parallel to -y axis (EAST)
   /// 
   /// The returned vector's .x -> roll, .y -> pitch, and .z -> yaw (clockwise)
   /// 
   /// RETURN VALUE of each component of the VectorT<T>:
   /// -YAW degrees(.z): Compass heading from due north.
   ///                     +90 is east (parallel to -Y axis).
   ///                     -90 / 270 is west (parallel to +Y Axis).
   ///                     +x is assumed to point due north.
   /// -PITCH degrees(.y): How far above horizon the nose is pointing.
   ///                     +30 deg is thirty degrees above the horizon. 
   ///                     0 deg is level and -24 would be twenty four degrees
   ///                     below horizon.
   /// -ROLL degrees(.x): How far clockwise have the wingtips rolled?
   ///                    0 is flat, 180 is upside down.
   ///                    If flying due north w/ a pitch of zero (flying towards
   ///                    horizon), at 90 deg roll would have to cockpit's
   ///                    normal vector pointing east.
   /// 
   /// The Givens order of the rotation is Z -> Y -> X. In an
   /// aircraft's frame that is YAW, PITCH, ROLL. HOWEVER!!! Should Yaw be
   /// mapped to the compass heading (clockwise / left handed) or should
   /// Yaw map to a right handed coordinate frame (counter-clockwise /
   /// right handed)? To clarify, two methods are provided.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   VectorD getRPY_deg_clockwise() const noexcept;

   ///------------------------------------------------------------------------
   /// Returns the Euler angle rotations about the column major X,Y,Z axes,
   /// respectively. Assumes the YAW rotation is counter-clockwise 
   ///    YAW of 0 is parallel to +x axis, YAW of 90 is parallel to +y axis
   /// 
   /// The returned vector's .x -> roll, .y -> pitch, and 
   /// .z -> yaw (counter-clockwise)
   /// 
   ///    /// RETURN VALUE of each component of the VectorT<T>:
   /// -YAW degrees(.z): Right handed counter clockwise rotation from +x axis.
   ///                     +90 is parallel to +Y axis (0,1,0).
   ///                     -90 / 270 is 
   ///                     aligned w/ -Y axis (0,-1,0).
   /// -PITCH degrees(.y): How far above horizon the nose is pointing.
   ///                     +30 deg is thirty degrees above the horizon. 
   ///                     0 deg is level and -24 would be twenty four degrees
   ///                     below horizon.
   /// -ROLL degrees(.x): How far clockwise have the wingtips rolled?
   ///                    0 is flat, 180 is upside down.
   ///                    If flying due north w/ a pitch of zero (flying towards
   ///                    horizon), at 90 deg roll would have to cockpit's
   ///                    normal vector pointing east.
   /// 
   /// The Givens order of the rotation is Z -> Y -> X. In an
   /// aircraft's frame that is YAW, PITCH, ROLL. HOWEVER!!! Should Yaw be
   /// mapped to the compass heading (clockwise / left handed) or should
   /// Yaw map to a right handed coordinate frame (counter-clockwise /
   /// right handed)? To clarify, two methods are provided. Use 
   /// counter_clockwise for right handed yaw. Use clockwise for left handed /
   /// compass heading yaw.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   VectorD getRPY_deg_counter_clockwise() const noexcept;


   ///------------------------------------------------------------------------
   /// This methods takes as input a Vector who's X,Y, and Z components hold
   /// the roll, pitch, and yaw, respectively, expressed in degrees. This 
   /// method sets the outDispMat to a 4x4 orthonormal display matrix 
   /// matching that orientation.The outDispMat is a typical Mat4 DCM that 
   /// specifies the orientation of a Model.
   /// 
   /// So passing in VectorT<T>{ 20, 45, 90 }. Would be a DCM that has it's
   /// +x column pointing due east (along global -Y). It is pitched up 45
   /// degrees above the horizon (the global XY plane). And it is rolled
   /// 20 degrees rightward (ie, a flat wing  like this - would tilt
   ///  like this \ ( - -> \).
   ///
   /// The order of evaluation for these euler angles is yaw, then pitch, and
   /// finally roll.
   ///
   /// Returns the a DCM built by Euler angle rotation of Yaw (clockwise / 
   /// compass heading), then Pitch (above horizon is positive, below is neg),
   /// then roll (clockwise roll is positive where left wing moves up and 
   /// right wing moves down).
   /// 
   /// Assumes the YAW rotation is clockwise (mapping to compass
   /// heading). BUT the INPUT Vector is x->roll, y->pitch, z->yaw.
   ///    YAW of 0 is parallel to +x axis, 
   ///    YAW of 90 is parallel to -y axis (EAST)
   /// 
   /// The returned Mat4 is a DCM that first applied the .z->yaw, then 
   /// .y->pitch, and finally .x -> roll (clockwise / COMPASS heading).
   /// 
   /// Internally, this simply invokes
   /// AftrCoordinateTransforms::transformFromRollPitchYawToDisplayMatrix( rpy );
   /// 
   /// The Givens order of the rotation is Z -> Y -> X. In an
   /// aircraft's frame that is YAW, PITCH, ROLL. HOWEVER!!! Should Yaw be
   /// mapped to the compass heading (clockwise / left handed) or should
   /// Yaw map to a right handed coordinate frame (counter-clockwise /
   /// right handed)? To clarify, two methods are provided.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   static Mat4T<T> fromRPY_deg_clockwise( const VectorT<T>& rpyDeg ) noexcept;


   ///------------------------------------------------------------------------
   /// Swizzle creates a new Mat4 containing the same column vectors as the
   /// original Mat4, but moved to new column locations specified by the
   /// input parameter. For example passing calling:
   /// Mat4 m; m.swizzle_to( yzx ), will return a new mat4 with the following
   /// values:
   /// | 0 0 1 U |   U denotes UNchanged from original.
   /// | 1 0 0 U |
   /// | 0 1 0 U |
   /// | U U U U |   
   /// That is,
   /// The original y col vec is moved to the first column in the new mat.
   /// The original z col vec is moved to the second column in the new mat.
   /// The original x col vec is moved to the third column in the new mat.
   /// The last column and bottom row of the 4x4 are unchanged from the 
   /// original mat4.
   /// So, using xyz returns a copy of the original.
   ///------------------------------------------------------------------------
   [[nodiscard]]
   Mat4T<T> swizzle_upper_3x3( Aftr::swizzle_to const swiz ) const noexcept;

   ///------------------------------------------------------------------------
   ///Returns a pointer to an array of 16 values, corresponding to this Mat4
   ///internal matrix. Column Major layout as follows:
   ///   0 4  8 12        x0 x1 x2 0
   ///   1 5  9 13  ===>  y0 y1 y2 0
   ///   2 6 10 14  ===>  z0 z1 z2 0
   ///   3 7 11 15         0  0  0 1
   ///
   /// Where Column 0 is the X vector, Column 1 is the Y vector, and
   /// Column 2 is the Z vector in this Mat4.
   ///------------------------------------------------------------------------
   T* getPtr() noexcept;
   const T* getPtr() const noexcept;

   T* data() noexcept;
   const T* data() const noexcept;


   std::array<T, NUM_ELEMS> m = 
                         { 1,0,0,0, 
                           0,1,0,0, 
                           0,0,1,0, 
                           0,0,0,1 }; //The 4x4 matrix

   constexpr T ZERO_THRESHOLD() const noexcept { return static_cast<T>( 0.001 ); }

   using iterator = typename decltype(m)::iterator;
   using const_iterator = typename decltype(m)::const_iterator;

   iterator begin() noexcept { return this->m.begin(); }
   const_iterator cbegin() const noexcept { return this->m.begin(); }

   iterator end() noexcept { return this->m.end(); }
   const_iterator cend() const noexcept { return this->m.end(); }
};

//template< typename T >
//typename Mat4T<T>::iterator begin( Mat4T<T>& m ) { return m.begin(); }
//
//template< typename T >
//typename Mat4T<T>::const_iterator begin( const Mat4T<T>& m ) { return m.begin(); }
//
//template< typename T >
//typename Mat4T<T>::const_iterator end( Mat4T<T>& m ) { return m.end(); }
//
//template< typename T >
//typename Mat4T<T>::iterator end( const Mat4T<T>& m ) { return m.end(); }
//
//template< typename T >
//typename Mat4T<T>::const_iterator cbegin( const Mat4T<T>& m ) { return m.cbegin(); }
//
//template< typename T >
//typename Mat4T<T>::const_iterator cend( const Mat4T<T>& m ) { return m.cend(); }

using Mat4 = Mat4T< float >;
using Mat4D = Mat4T< double >;

} //namespace Aftr
#include "Mat4.cpptemplate.h"
