#include <J3ML/LinearAlgebra/Vector3.h>
#include <J3ML/LinearAlgebra/Vector4.h>
#include <J3ML/LinearAlgebra/Matrix3x3.h>
#include <J3ML/LinearAlgebra/Matrix4x4.h>
#include <J3ML/LinearAlgebra/Quaternion.h>

namespace LinearAlgebra {
    Quaternion Quaternion::operator-() const
    {
        return {-x, -y, -z, -w};
    }

    Quaternion::Quaternion(const Matrix3x3 &rotationMtrx) {}

    Quaternion::Quaternion(const Matrix4x4 &rotationMtrx) {}

    Vector3 Quaternion::GetWorldX() const { return Transform(1.f, 0.f, 0.f); }

    Vector3 Quaternion::GetWorldY() const { return Transform(0.f, 1.f, 0.f); }

    Vector3 Quaternion::GetWorldZ() const { return Transform(0.f, 0.f, 1.f); }

    Vector3 Quaternion::Transform(const Vector3 &vec) const {
        Matrix3x3 mat = this->ToMatrix3x3();
        return mat * vec;
    }

    Vector3 Quaternion::Transform(float X, float Y, float Z) const {
        return Transform(Vector3{X, Y, Z});
    }

    Vector4 Quaternion::Transform(const Vector4 &vec) const {
        return Vector4(Transform(vec.x, vec.y, vec.z), vec.w);
    }

    Vector4 Quaternion::Transform(float X, float Y, float Z, float W) const {
        return Transform(Vector4(X, Y, Z, W));
    }

    Quaternion Quaternion::Lerp(const Quaternion &b, float t) const {
        float angle = this->Dot(b);
        if (angle >= 0.f) // Make sure we rotate the shorter arc
            return (*this * (1.f - t) + b * t).Normalize();
        else
            return (*this * (t - 1.f) + b * t).Normalize();
    }

    void Quaternion::SetFromAxisAngle(const Vector3 &axis, float angle) {
        float sinz, cosz;

    }

    Quaternion Quaternion::operator*(float scalar) const {
        return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
    }

    Quaternion Quaternion::operator+() const { return *this; }

    Quaternion::Quaternion() {}

    Quaternion::Quaternion(float X, float Y, float Z, float W) : Vector4(X,Y,Z,W) {}

    // TODO: implement
    float Quaternion::Dot(const Quaternion &rhs) const {
        return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
    }

    Quaternion::Quaternion(Vector4 vector4) {

    }

    Quaternion Quaternion::Normalize() const {
        float length = Length();
        if (length < 1e-4f)
            return {0,0,0,0};
        float reciprocal = 1.f / length;
        return {
                x * reciprocal,
                y * reciprocal,
                z * reciprocal,
                w * reciprocal
        };
    }

    Quaternion Quaternion::Conjugate() const {
        return { -x, -y, -z, w };
    }

    Quaternion Quaternion::Inverse() const {
        return Conjugate();
    }

    Quaternion Quaternion::Slerp(const Quaternion &q2, float t) const {
        float angle = this->Dot(q2);
        float sign = 1.f;
        if (angle < 0.f)
        {
            angle = -angle;
            sign = -1.f;
        }

        float a;
        float b;
        if (angle < 0.999)
        {
            // angle = Acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
            angle = (-0.69813170079773212f * angle * angle - 0.87266462599716477f) * angle + 1.5707963267948966f;

            float ta = t*angle;
            // Not using a lookup table, manually compute the two sines by using a very rough approximation.
            float ta2 = ta*ta;
            b = ((5.64311797634681035370e-03f * ta2 - 1.55271410633428644799e-01f) * ta2 + 9.87862135574673806965e-01f) * ta;
            a = angle - ta;
            float a2 = a*a;
            a = ((5.64311797634681035370e-03f * a2 - 1.55271410633428644799e-01f) * a2 + 9.87862135574673806965e-01f) * a;
        }
        else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
        {
            a = 1.f - t;
            b = t;
        }
        // Lerp and renormalize.
        return (*this * (a * sign) + q2 * b).Normalize();
    }

    AxisAngle Quaternion::ToAxisAngle() const {
        float halfAngle = std::acos(w);
        float angle = halfAngle * 2.f;
        // TODO: Can Implement Fast Inverse Sqrt Here
        float reciprocalSinAngle = 1.f / std::sqrt(1.f - w*w);

        Vector3 axis = {
                x*reciprocalSinAngle,
                y*reciprocalSinAngle,
                z*reciprocalSinAngle
        };
        return AxisAngle(axis, angle);
    }

    float Quaternion::AngleBetween(const Quaternion &target) const {
        Quaternion delta = target / *this;
        return delta.Normalize().Angle();
    }

    Quaternion Quaternion::operator/(const Quaternion &rhs) const {
        return {
                x*rhs.w - y*rhs.z + z*rhs.y - w*rhs.x,
                x*rhs.z + y*rhs.w - z*rhs.x - w*rhs.y,
                -x*rhs.y + y*rhs.x + z*rhs.w - w*rhs.z,
                x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w
        };
    }

    Matrix3x3 Quaternion::ToMatrix3x3() const {
        return {
                1 - 2 *(y*y) - 2*(z*z), 2*x*y - 2*z*w, 2*x*z + 2*y*w,
                2*x*y + 2*z*w, 1-2*x*x - 2*z*z, 2*y*z - 2*x*w,
                2*x*z - 2*y*w, 2*y*z + 2*x*w, 1-2*x*x - 2*y*y
        };
    }

    Quaternion Quaternion::operator+(const Quaternion &rhs) const {
        return {
                x + rhs.x, y + rhs.y, z + rhs.z,w + rhs.w
        };
    }

    Matrix4x4 Quaternion::ToMatrix4x4() const {
        return Matrix4x4(*this);
    }

    Matrix4x4 Quaternion::ToMatrix4x4(const Vector3 &translation) const {
        return {*this, translation};
    }
}