#include <J3ML/LinearAlgebra/Matrix3x3.h>
#include <cmath>

namespace J3ML::LinearAlgebra {

    const Matrix3x3 Matrix3x3::Zero = Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
    const Matrix3x3 Matrix3x3::Identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
    const Matrix3x3 Matrix3x3::NaN = Matrix3x3(NAN);

    Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21,
                         float m22) {
        this->elems[0][0] = m00;
        this->elems[0][1] = m01;
        this->elems[0][2] = m02;

        this->elems[1][0] = m10;
        this->elems[1][1] = m11;
        this->elems[1][2] = m12;

        this->elems[2][0] = m20;
        this->elems[2][1] = m21;
        this->elems[2][2] = m22;
    }

    Vector3 Matrix3x3::GetRow(int index) const {
        float x = this->elems[index][0];
        float y = this->elems[index][1];
        float z = this->elems[index][2];
        return {x, y, z};
    }

    Vector3 Matrix3x3::GetColumn(int index) const {
        float x = this->elems[0][index];
        float y = this->elems[1][index];
        float z = this->elems[2][index];

        return {x, y, z};
    }


    float Matrix3x3::At(int x, int y) const {
        return this->elems[x][y];
    }

    void Matrix3x3::SetAt(int x, int y, float value)
    {
        this->elems[x][y] = value;
    }

    Vector3 Matrix3x3::operator*(const Vector3 &rhs) const {
        return {
                At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
                At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
                At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z
        };
    }

    Matrix3x3 Matrix3x3::operator*(const Matrix3x3 &rhs) const {
        //Matrix3x3 r;
        auto m00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
        auto m01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
        auto m02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);

        auto m10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
        auto m11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
        auto m12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);

        auto m20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
        auto m21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
        auto m22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);

        return Matrix3x3({m00, m01, m02}, {m10, m11, m12}, {m20, m21, m22});
    }

    Matrix3x3::Matrix3x3(float val) {
        this->elems[0][0] = val;
        this->elems[0][1] = val;
        this->elems[0][2] = val;

        this->elems[1][0] = val;
        this->elems[1][1] = val;
        this->elems[1][2] = val;

        this->elems[2][0] = val;
        this->elems[2][1] = val;
        this->elems[2][2] = val;

    }

    Matrix3x3::Matrix3x3(const Vector3 &col0, const Vector3 &col1, const Vector3 &col2) {
        SetColumn(0, col0);
        SetColumn(1, col1);
        SetColumn(2, col2);

        //this->elems[0][0] = r1.x;
        //this->elems[0][1] = r1.y;
        //this->elems[0][2] = r1.z;

        //this->elems[1][0] = r2.x;
        //this->elems[1][1] = r2.y;
        //this->elems[1][2] = r2.z;

        //this->elems[2][0] = r3.x;
        //this->elems[2][1] = r3.y;
        //this->elems[2][2] = r3.z;
    }

    Matrix3x3::Matrix3x3(const Quaternion &orientation) {

    }

    float Matrix3x3::Determinant() const {
        const float a = elems[0][0];
        const float b = elems[0][1];
        const float c = elems[0][2];
        const float d = elems[1][0];
        const float e = elems[1][1];
        const float f = elems[1][2];
        const float g = elems[2][0];
        const float h = elems[2][1];
        const float i = elems[2][2];

        // Aight, what the fuck?
        return a*(e*i - f*h) + b*(f*g - d*i) + c*(d*h - e*g);
    }

    Matrix3x3 Matrix3x3::Inverted() const {
        // Compute the inverse directly using Cramer's rule
        // Warning: This method is numerically very unstable!
        float d = Determinant();

        d = 1.f / d;

        Matrix3x3 i = {
                d * (At(1, 1) * At(2, 2) - At(1, 2) * At(2, 1)),
                d * (At(0, 2) * At(2, 1) - At(0, 1) * At(2, 2)),
                d * (At(0, 1) * At(1, 2) - At(0, 2) * At(1, 1)),

                d * (At(1, 2) * At(2, 0) - At(1, 0) * At(2, 2)),
                d * (At(0, 0) * At(2, 2) - At(0, 2) * At(2, 0)),
                d * (At(0, 2) * At(1, 0) - At(0, 0) * At(1, 2)),

                d * (At(1, 0) * At(2, 1) - At(1, 1) * At(2, 0)),
                d * (At(2, 0) * At(0, 1) - At(0, 0) * At(2,1)),
                d * (At(0,0) * At(1, 1) - At(0, 1) * At(1, 0))
        };

        return i;
    }

    Matrix3x3 Matrix3x3::Transposed() const {
        auto m00 = this->elems[0][0];
        auto m01 = this->elems[0][1];
        auto m02 = this->elems[0][2];

        auto m10 = this->elems[1][0];
        auto m11 = this->elems[1][1];
        auto m12 = this->elems[1][2];

        auto m20 = this->elems[2][0];
        auto m21 = this->elems[2][1];
        auto m22 = this->elems[2][2];

        // NO: This is correct order for transposition!
        return {
                m00, m10, m20,
                m01, m11, m21,
                m02, m12, m22
        };
    }

    Vector2 Matrix3x3::Transform(const Vector2 &rhs) const {
        return {
                At(0,0) * rhs.x + At(0, 1) * rhs.y,
                At(1,0) * rhs.x + At(1, 1) * rhs.y
        };
    }

    Vector3 Matrix3x3::Transform(const Vector3 &rhs) const {
        return {
                At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
                At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
                At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z
        };
    }

    Quaternion Matrix3x3::ToQuat() const {
        auto m00 = At(0,0);
        auto m01 = At(0, 1);
        auto m02 = At(0, 2);
        auto m10 = At(1,0);
        auto m11 = At(1, 1);
        auto m12 = At(1, 2);
        auto m20 = At(2,0);
        auto m21 = At(2, 1);
        auto m22 = At(2, 2);

        auto w = std::sqrt(1.f + m00 + m11 + m22) / 2.f;
        float w4 = (4.f * w);
        return {
                (m21 - m12) / w4,
                (m02 - m20) / w4,
                (m10 - m01) / w4,
                w
        };
    }

    void Matrix3x3::SetRotatePart(const Vector3 &a, float angle) {
        float s = std::sin(angle);
        float c = std::cos(angle);

        const float c1 = 1.f - c;

        elems[0][0] = c+c1*a.x*a.x;
        elems[1][0] = c1*a.x*a.y+s*a.z;
        elems[2][0] = c1*a.x*a.z-s*a.y;

        elems[0][1] = c1*a.x*a.y-s*a.z;
        elems[1][1] = c+c1*a.y*a.y;
        elems[2][1] = c1*a.y*a.z+s*a.x;

        elems[0][2] = c1*a.x*a.z+s*a.y;
        elems[1][2] = c1*a.y*a.z-s*a.x;
        elems[2][2] = c+c1*a.z*a.z;
    }

    Matrix3x3 Matrix3x3::RotateAxisAngle(const Vector3 &axis, float angleRadians) {
        Matrix3x3 r;
        r.SetRotatePart(axis, angleRadians);
        return r;
    }

    void Matrix3x3::SetRow(int i, const Vector3 &vec) {
        elems[i][0] = vec.x;
        elems[i][1] = vec.y;
        elems[i][2] = vec.z;
    }

    void Matrix3x3::SetColumn(int i, const Vector3& vec)
    {
        elems[0][i] = vec.x;
        elems[1][i] = vec.y;
        elems[2][i] = vec.z;
    }

    Matrix3x3
    Matrix3x3::LookAt(const Vector3 &forward, const Vector3 &target, const Vector3 &localUp, const Vector3 &worldUp) {
        // User must input proper normalized input direction vectors
        // In the local space, the forward and up directions must be perpendicular to be well-formed.
        // In the world space, the target direction and world up cannot be degenerate (co-linear)
        // Generate the third basis vector in the local space;
        Vector3 localRight = localUp.Cross(forward).Normalize();
        // A. Now we have an orthonormal linear basis {localRight, localUp, forward} for the object local space.
        // Generate the third basis vector for the world space
        Vector3 worldRight = worldUp.Cross(target).Normalize();
        // Since the input worldUp vector is not necessarily perpendicular to the target direction vector
        // We need to compute the real world space up vector that the "head" of the object will point
        // towards when the model is looking towards the desired target direction
        Vector3 perpWorldUp = target.Cross(worldRight).Normalize();
        // B. Now we have an orthonormal linear basis {worldRight, perpWorldUp, targetDirection } for the desired target orientation.
        // We want to build a matrix M that performs the following mapping:
        // 1. localRight must be mapped to worldRight.        (M * localRight = worldRight)
        // 2. localUp must be mapped to perpWorldUp.          (M * localUp = perpWorldUp)
        // 3. localForward must be mapped to targetDirection. (M * localForward = targetDirection)
        // i.e. we want to map the basis A to basis B.

        // This matrix M exists, and it is an orthonormal rotation matrix with a determinant of +1, because
        // the bases A and B are orthonormal with the same handedness.

        // Below, use the notation that (a,b,c) is a 3x3 matrix with a as its first column, b second, and c third.

        // By algebraic manipulation, we can rewrite conditions 1, 2 and 3 in a matrix form:
        //        M * (localRight, localUp, localForward) = (worldRight, perpWorldUp, targetDirection)
        // or     M = (worldRight, perpWorldUp, targetDirection) * (localRight, localUp, localForward)^{-1}.
        // or     M = m1 * m2, where

        // m1 equals (worldRight, perpWorldUp, target):
        Matrix3x3 m1(worldRight, perpWorldUp, target);

        // and m2 equals (localRight, localUp, localForward)^{-1}:
        Matrix3x3 m2;
        m2.SetRow(0, localRight);
        m2.SetRow(1, localUp);
        m2.SetRow(2, forward);
        // Above we used the shortcut that for an orthonormal matrix M, M^{-1} = M^T. So set the rows
        // and not the columns to directly produce the transpose, i.e. the inverse of (localRight, localUp, localForward).

        // Compute final M.
        m2 = m1 * m2;

        // And fix any numeric stability issues by re-orthonormalizing the result.
        m2.Orthonormalize(0, 1, 2);
        return m2;
    }

    Vector3 Matrix3x3::Diagonal() const {
        return {elems[0][0],
                elems[1][1],
                elems[2][2]
        };
    }

    float &Matrix3x3::At(int row, int col) {
        return elems[row][col];
    }

    Vector3 Matrix3x3::WorldZ() const {
        return GetColumn(2);
    }

    Vector3 Matrix3x3::WorldY() const {
        return GetColumn(1);
    }

    Vector3 Matrix3x3::WorldX() const {
        return GetColumn(0);
    }

    Matrix3x3 Matrix3x3::FromRS(const Quaternion &rotate, const Vector3 &scale) {
        return Matrix3x3(rotate) * Matrix3x3::FromScale(scale);
    }

    Matrix3x3 Matrix3x3::FromRS(const Matrix3x3 &rotate, const Vector3 &scale) {
        return rotate * Matrix3x3::FromScale(scale);
    }

    Matrix3x3 Matrix3x3::FromQuat(const Quaternion &orientation) {
        return Matrix3x3(orientation);
    }

    Matrix3x3 Matrix3x3::ScaleBy(const Vector3 &rhs) {
        return *this * FromScale(rhs);
    }

    Vector3 Matrix3x3::GetScale() const {
        return Vector3(GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length());
    }

    Vector3 Matrix3x3::operator[](int row) const {
        return Vector3{elems[row][0], elems[row][1], elems[row][2]};
    }

    Matrix3x3 Matrix3x3::FromScale(const Vector3 &scale) {
        Matrix3x3 m;
        m.At(0,0) = scale.x;
        m.At(1,1) = scale.y;
        m.At(2,2) = scale.z;
        return m;
    }

    Matrix3x3 Matrix3x3::FromScale(float sx, float sy, float sz) {
        Matrix3x3 m;
        m.At(0,0) = sx;
        m.At(1,1) = sy;
        m.At(2,2) = sz;
        return m;
    }

    void Matrix3x3::Orthonormalize(int c0, int c1, int c2) {
        Vector3 v0 = GetColumn(c0);
        Vector3 v1 = GetColumn(c1);
        Vector3 v2 = GetColumn(c2);
        Vector3::Orthonormalize(v0, v1, v2);
        SetColumn(c0, v0);
        SetColumn(c1, v1);
        SetColumn(c2, v2);
    }

    Matrix3x3::Matrix3x3(const float *data) {
        assert(data);
        At(0, 0) = data[0];
        At(0, 1) = data[1];
        At(0, 2) = data[2];

        At(1, 0) = data[4];
        At(1, 1) = data[5];
        At(1, 2) = data[6];

        At(2, 0) = data[8];
        At(2, 1) = data[9];
        At(2, 2) = data[10];
    }

    Vector4 Matrix3x3::Mul(const Vector4 &rhs) const {
        return {Mul(rhs.XYZ()), rhs.GetW()};
    }

    Vector3 Matrix3x3::Mul(const Vector3 &rhs) const {
        return *this * rhs;
    }

    Vector2 Matrix3x3::Mul(const Vector2 &rhs) const {
        return *this * rhs;
    }

    Matrix4x4 Matrix3x3::Mul(const Matrix4x4 &rhs) const {
        return *this * rhs;
    }

    Matrix3x3 Matrix3x3::Mul(const Matrix3x3 &rhs) const {
        return *this * rhs;
    }

    bool Matrix3x3::IsRowOrthogonal(float epsilon) const
    {
        return GetRow(0).IsPerpendicular(GetRow(1), epsilon)
            && GetRow(0).IsPerpendicular(GetRow(2), epsilon)
            && GetRow(1).IsPerpendicular(GetRow(2), epsilon);
    }

    bool Matrix3x3::IsColOrthogonal(float epsilon) const
    {
        return GetColumn(0).IsPerpendicular(GetColumn(1), epsilon)
            && GetColumn(0).IsPerpendicular(GetColumn(2), epsilon)
            && GetColumn(1).IsPerpendicular(GetColumn(2), epsilon);
    }



    bool Matrix3x3::HasUniformScale(float epsilon) const {
        Vector3 scale = ExtractScale();
        return Math::EqualAbs(scale.x, scale.y, epsilon) && Math::EqualAbs(scale.x, scale.z, epsilon);
    }

    Vector3 Matrix3x3::GetRow3(int index) const {
        return GetRow(index);
    }

    Vector3 Matrix3x3::GetColumn3(int index) const {
        return GetColumn(index);
    }

    Matrix4x4 Matrix3x3::operator*(const Matrix4x4 &rhs) const {
        auto lhs = *this;
        Matrix4x4 r;

        r[0][0] = lhs.At(0, 0) * rhs.At(0, 0) + lhs.At(0, 1) * rhs.At(1, 0) + lhs.At(0, 2) * rhs.At(2, 0);
        r[0][1] = lhs.At(0, 0) * rhs.At(0, 1) + lhs.At(0, 1) * rhs.At(1, 1) + lhs.At(0, 2) * rhs.At(2, 1);
        r[0][2] = lhs.At(0, 0) * rhs.At(0, 2) + lhs.At(0, 1) * rhs.At(1, 2) + lhs.At(0, 2) * rhs.At(2, 2);
        r[0][3] = lhs.At(0, 0) * rhs.At(0, 3) + lhs.At(0, 1) * rhs.At(1, 3) + lhs.At(0, 2) * rhs.At(2, 3);

        r[1][0] = lhs.At(1, 0) * rhs.At(0, 0) + lhs.At(1, 1) * rhs.At(1, 0) + lhs.At(1, 2) * rhs.At(2, 0);
        r[1][1] = lhs.At(1, 0) * rhs.At(0, 1) + lhs.At(1, 1) * rhs.At(1, 1) + lhs.At(1, 2) * rhs.At(2, 1);
        r[1][2] = lhs.At(1, 0) * rhs.At(0, 2) + lhs.At(1, 1) * rhs.At(1, 2) + lhs.At(1, 2) * rhs.At(2, 2);
        r[1][3] = lhs.At(1, 0) * rhs.At(0, 3) + lhs.At(1, 1) * rhs.At(1, 3) + lhs.At(1, 2) * rhs.At(2, 3);

        r[2][0] = lhs.At(2, 0) * rhs.At(0, 0) + lhs.At(2, 1) * rhs.At(1, 0) + lhs.At(2, 2) * rhs.At(2, 0);
        r[2][1] = lhs.At(2, 0) * rhs.At(0, 1) + lhs.At(2, 1) * rhs.At(1, 1) + lhs.At(2, 2) * rhs.At(2, 1);
        r[2][2] = lhs.At(2, 0) * rhs.At(0, 2) + lhs.At(2, 1) * rhs.At(1, 2) + lhs.At(2, 2) * rhs.At(2, 2);
        r[2][3] = lhs.At(2, 0) * rhs.At(0, 3) + lhs.At(2, 1) * rhs.At(1, 3) + lhs.At(2, 2) * rhs.At(2, 3);

        r[3][0] = rhs.At(3, 0);
        r[3][1] = rhs.At(3, 1);
        r[3][2] = rhs.At(3, 2);
        r[3][3] = rhs.At(3, 3);
        return r;
    }

    Vector2 Matrix3x3::operator*(const Vector2 &rhs) const {
        return Transform(rhs);
    }

    Matrix3x3 Matrix3x3::RotateX(float radians) {
        Matrix3x3 r;
        r.SetRotatePartX(radians);
        return r;
    }

    Matrix3x3 Matrix3x3::RotateY(float radians) {
        Matrix3x3 r;
        r.SetRotatePartY(radians);
        return r;
    }

    Matrix3x3 Matrix3x3::RotateZ(float radians) {
        Matrix3x3 r;
        r.SetRotatePartZ(radians);
        return r;
    }

    void Matrix3x3::SetRotatePartX(float angle) {
        Set3x3PartRotateX(*this, angle);
    }

    void Matrix3x3::SetRotatePartY(float angle) {
        Set3x3PartRotateY(*this, angle);
    }

    void Matrix3x3::SetRotatePartZ(float angle) {
        Set3x3RotatePartZ(*this, angle);
    }

    Vector3 Matrix3x3::ExtractScale() const {
        return {GetColumn(0).Length(), GetColumn(1).Length(), GetColumn(2).Length()};
    }

    // TODO: Finish implementation
    Matrix3x3 Matrix3x3::RotateFromTo(const Vector3 &source, const Vector3 &direction) {
        assert(source.IsNormalized());
        assert(source.IsNormalized());

        // http://cs.brown.edu/research/pubs/pdfs/1999/Moller-1999-EBA.pdf
        Matrix3x3 r;
        float dot = source.Dot(direction);
        if (std::abs(dot) > 0.999f)
        {
            Vector3 s = source.Abs();
            Vector3 unit = s.x < s.y && s.x < s.z ? Vector3::UnitX : (s.y < s.z ? Vector3::UnitY : Vector3::UnitZ);
        }
    }

    Vector3 &Matrix3x3::Row(int row) {
        assert(row >= 0);
        assert(row < Rows);
        return reinterpret_cast<Vector3 &> (elems[row]);
    }

    Vector3 Matrix3x3::Column(int index) const { return GetColumn(index);}

    Vector3 Matrix3x3::Col(int index) const { return Column(index);}

    Vector3 &Matrix3x3::Row3(int index) {
        return reinterpret_cast<Vector3 &>(elems[index]);
    }

    Vector3 Matrix3x3::Row3(int index) const { return GetRow3(index);}

    void Matrix3x3::Set(const Matrix3x3 &x3) {

    }

}