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

namespace 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];

    }

    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 &r1, const Vector3 &r2, const Vector3 &r3) {
        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::Inverse() 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::Transpose() 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]
        };
    }

}