#pragma once

#include "QuatImpl.h"
#include "Vector.h"
#include <cassert>
#include <cmath>
#include <sstream>
#include <cstdint>
#include "Mat4.h"
#include "AxisAngle.h"

namespace Aftr
{

template< typename T >
class QuatT
{
public:
   using value_type = T;

   QuatT() noexcept;

   /**
      a - the scalar component of the quaternion
      b,c,d - the vector components of the quaternion
   */
   QuatT( T scalarA, T vecCompX, T vecCompY, T vecCompZ ) noexcept;
   QuatT( const T* m4x4 ); ///< convert a rotation matrix to a quaternion
   QuatT( const Mat4T<T>& dcm ) noexcept; ///< convert a rotation matrix to a quaternion
   QuatT( const QuatT& toCopy ) noexcept;
   QuatT( AxisAngleT<T> const&  ) noexcept;
   ~QuatT() = default;
   ///Flips all 4 signs, so a,b,c,d becomes (-a,-b,-c,-d).
   QuatT<T> negation() const noexcept;
   ///Same as DCM's transpose, but for a quaternion. Flips the signs on the b,c,d vector component.
   QuatT<T> conjugate() const noexcept;
   QuatT<T> operator+( const QuatT& q ) const;
   QuatT<T> operator-( const QuatT& q ) const;
   QuatT<T> operator*( const QuatT& q ) const;
   QuatT<T>& operator=( const QuatT& q );
   QuatT<T> grassmanProduct( const QuatT& q ) const;
   T innerProduct( const QuatT& q )const;
   //QuatT<T> outerProduct( const QuatT& q )const; //sln commented out, not tested yet
   T magnitude()const;
   //QuatT<T> crossProduct( const QuatT& q )const; //sln commented out, not tested yet
   //QuatT<T> evenProduct( const QuatT& q )const;  //sln commented out, not tested yet
   //void toRepresentationMatrix( T m[16] )const;  //sln commented out, not tested yet

   bool isEqual( QuatT const& a, T epsilon = 0.00001 ) const noexcept;

   //-------------------------------------------------------------------------
   //Factory function:
   //If the input str can be parsed to a 4 component w,x,y,z via its internal
   //regular expression, a valid QuatT<T> will be returned in the optional,
   //otherwise nullopt. The expected input will be 4 numbers, the *first*
   //number is the quaternion's scalar component, and the subsequent *three*
   //numbers are the quaternion's vector component.
   //-------------------------------------------------------------------------
   static std::optional< QuatT<T> > fromString_opt( const std::string& str );
   
   ///------------------------------------------------------------------------
   /// Returns a rotation matrix (SO(3)) from this unit quaternion. The
   /// returned Mat4 is column major (0,1,2) = +x, (4,5,6) = +y, (8,9,10)= +z.
   /// Bottom row and last column will be identity (0,0,0,1).
   /// 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 |
   ///------------------------------------------------------------------------
   Mat4T<T> toRotationMatrix() const;
   void toRotationMatrix( T m[16] ) const;
   std::string toString( size_t length = 3) const;
   [[nodiscard]]
   QuatT<T> normalize() const;
   [[nodiscard]]
   T length() const noexcept; ///< Returns the length of the quaternion sqrt of square of all 4 components
   VectorT<T> getVectorXYZ() const noexcept; ///< Returns the x,y,z components of this quat as a Vector
   VectorT<T> getAxisOfRotation() const noexcept; ///< Returns the 3D axis of rotation, assuming this is a unit quaternion representing a rotation
   T getAngleOfRotationRads() const noexcept; ///< Returns the radian counter clockwise rotation about the axis represented by this quaternion.
   AxisAngleT<T> toAxisAngle() const noexcept; ///< Returns the axis and angle representation of this Quaternion's orientation.

   [[nodiscard]]
   T& operator[]( uint8_t i ) noexcept;
   [[nodiscard]]
   T operator[]( uint8_t i ) const noexcept;

   /**
      Spherical Linear Interpolation parametricDistance [0..1] between src
      and dst.
      Returned Quaternion is interpolated orientation between src and dst.
      */
   static QuatT slerp( const QuatT& src, const QuatT& dst, T t );

   T a = T(1.0); ///< Scalar component of quaternion
   T b = 0; ///< Vector 'x' component
   T c = 0; ///< Vector 'y' component
   T d = 0; ///< Vector 'z' component

private:
   //To keep our axis / angle and Q -> DCM -> Q symmetric, we enforce
   //that our Quaternion always has a positive imaginary component.
   //This eliminates the ambiguity that arises when converting from
   // Q -> DCM -> Q and observing sign changes. This is automatically
   //called when a Quaternion is created.
   void enforce_imaginary_comp_is_always_positive() noexcept;
};

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

