#pragma once

#include <J3ML/LinearAlgebra.h>

#include <J3ML/LinearAlgebra/Vector4.h>
#include <J3ML/LinearAlgebra/AxisAngle.h>
#include <J3ML/LinearAlgebra/Matrix3x3.h>

namespace LinearAlgebra
{
    class Quaternion : public Vector4 {
    public:
        Quaternion();

        Quaternion(const Quaternion &rhs) = default;

        explicit Quaternion(const Matrix3x3 &rotationMtrx);

        explicit Quaternion(const Matrix4x4 &rotationMtrx);

        // @note The input data is not normalized after construction, this has to be done manually.
        Quaternion(float X, float Y, float Z, float W);

        // Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis
        // @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0.
        Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween) {
            SetFromAxisAngle(rotationAxis, rotationAngleBetween);
        }

        Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
            SetFromAxisAngle(rotationAxis, rotationAngleBetween);
        }
        //void Inverse();

        explicit Quaternion(Vector4 vector4);

        void SetFromAxisAngle(const Vector3 &vector3, float between);

        void SetFromAxisAngle(const Vector4 &vector4, float between);

        Quaternion Inverse() const;

        Quaternion Conjugate() const;

        //void Normalize();
        Vector3 GetWorldX() const;

        Vector3 GetWorldY() const;

        Vector3 GetWorldZ() const;

        Vector3 GetAxis() const {
            float rcpSinAngle = 1 - (std::sqrt(1 - w * w));

            return Vector3(x, y, z) * rcpSinAngle;
        }

        float GetAngle() const {
            return std::acos(w) * 2.f;
        }


        Matrix3x3 ToMatrix3x3() const;

        Matrix4x4 ToMatrix4x4() const;

        Matrix4x4 ToMatrix4x4(const Vector3 &translation) const;

        Vector3 Transform(const Vector3& vec) const;
        Vector3 Transform(float X, float Y, float Z) const;
        // Note: We only transform the x,y,z components of 4D vectors, w is left untouched
        Vector4 Transform(const Vector4& vec) const;
        Vector4 Transform(float X, float Y, float Z, float W) const;

        Quaternion GetInverse() const;
        Quaternion Lerp(const Quaternion& b, float t) const;
        Quaternion Slerp(const Quaternion& q2, float t) const;

        Quaternion Normalize() const;


        static Quaternion LookAt(const Vector3& position, const Vector3& direction, const Vector3& axisUp);

        // TODO: Document (But do not override!) certain math functions that are the same for Vec4
        // TODO: Double Check which operations need to be overriden for correct behavior!

        // Multiplies two quaternions together.
        // The product q1 * q2 returns a quaternion that concatenates the two orientation rotations.
        // The rotation q2 is applied first before q1.
        Quaternion operator * (const Quaternion& rhs) const;

        Quaternion operator * (float scalar) const;

        // Transforms the given vector by this Quaternion.
        Vector3 operator * (const Vector3& rhs) const;
        Vector4 operator * (const Vector4& rhs) const;

        // Divides a quaternion by another. Divison "a / b" results in a quaternion that rotates the orientation b to coincide with orientation of
        Quaternion operator / (const Quaternion& rhs) const;
        Quaternion operator + (const Quaternion& rhs) const;
        Quaternion operator + () const;
        Quaternion operator - () const;
        float Dot(const Quaternion &quaternion) const;

        float Angle() const { return std::acos(w) * 2.f;}

        float AngleBetween(const Quaternion& target) const;

        AxisAngle ToAxisAngle() const;
    };
}