/*
 * Author: Andrey Pokidov
 * License: Apache-2.0
 * Date: 10 Feb 2019
 */

namespace BGC
{
    public struct Matrix3x3FP32
    {
        public float r1c1 = 0.0f, r1c2 = 0.0f, r1c3 = 0.0f;
        public float r2c1 = 0.0f, r2c2 = 0.0f, r2c3 = 0.0f;
        public float r3c1 = 0.0f, r3c2 = 0.0f, r3c3 = 0.0f;

        public Matrix3x3FP32(float d1, float d2, float d3)
        {
            this.r1c1 = d1;
            this.r2c2 = d2;
            this.r3c3 = d3;
        }

        public Matrix3x3FP32(in Matrix3x3FP32 matrix)
        {
            this.r1c1 = matrix.r1c1;
            this.r1c2 = matrix.r1c2;
            this.r1c3 = matrix.r1c3;

            this.r2c1 = matrix.r2c1;
            this.r2c2 = matrix.r2c2;
            this.r2c3 = matrix.r2c3;

            this.r3c1 = matrix.r3c1;
            this.r3c2 = matrix.r3c2;
            this.r3c3 = matrix.r3c3;
        }

        public Matrix3x3FP32(in Matrix3x3FP64 matrix)
        {
            this.r1c1 = (float)matrix.r1c1;
            this.r1c2 = (float)matrix.r1c2;
            this.r1c3 = (float)matrix.r1c3;

            this.r2c1 = (float)matrix.r2c1;
            this.r2c2 = (float)matrix.r2c2;
            this.r2c3 = (float)matrix.r2c3;

            this.r3c1 = (float)matrix.r3c1;
            this.r3c2 = (float)matrix.r3c2;
            this.r3c3 = (float)matrix.r3c3;
        }

        public readonly float GetDeterminant()
        {
            return this.r1c1 * (this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2)
                 + this.r1c2 * (this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3)
                 + this.r1c3 * (this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1);
        }

        public readonly bool IsSingular()
        {
            return UtilityFP32.IsZero(this.GetDeterminant());
        }

        public void Transpose()
        {
            (this.r1c2, this.r2c1) = (this.r2c1, this.r1c2);
            (this.r1c3, this.r3c1) = (this.r3c1, this.r1c3);
            (this.r2c3, this.r3c2) = (this.r3c2, this.r2c3);
        }

        public bool Invert()
        {
            float determinant = this.GetDeterminant();
    
            if (UtilityFP32.IsZero(determinant))
            {
                return false;
            }

            float r1c1 = this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2;
            float r1c2 = this.r1c3 * this.r3c2 - this.r1c2 * this.r3c3;
            float r1c3 = this.r1c2 * this.r2c3 - this.r1c3 * this.r2c2;

            float r2c1 = this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3;
            float r2c2 = this.r1c1 * this.r3c3 - this.r1c3 * this.r3c1;
            float r2c3 = this.r1c3 * this.r2c1 - this.r1c1 * this.r2c3;

            float r3c1 = this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1;
            float r3c2 = this.r1c2 * this.r3c1 - this.r1c1 * this.r3c2;
            float r3c3 = this.r1c1 * this.r2c2 - this.r1c2 * this.r2c1;

            float mutiplier = 1.0f / determinant;

            this.r1c1 = r1c1 * mutiplier;
            this.r1c2 = r1c2 * mutiplier;
            this.r1c3 = r1c3 * mutiplier;

            this.r2c1 = r2c1 * mutiplier;
            this.r2c2 = r2c2 * mutiplier;
            this.r2c3 = r2c3 * mutiplier;

            this.r3c1 = r3c1 * mutiplier;
            this.r3c2 = r3c2 * mutiplier;
            this.r3c3 = r3c3 * mutiplier;

            return true;
        }

        public void Reset()
        {
            this.r1c1 = 0.0f;
            this.r1c2 = 0.0f;
            this.r1c3 = 0.0f;

            this.r2c1 = 0.0f;
            this.r2c2 = 0.0f;
            this.r2c3 = 0.0f;

            this.r3c1 = 0.0f;
            this.r3c2 = 0.0f;
            this.r3c3 = 0.0f;
        }

        public void SetToIdentity()
        {
            this.r1c1 = 1.0f;
            this.r1c2 = 0.0f;
            this.r1c3 = 0.0f;

            this.r2c1 = 0.0f;
            this.r2c2 = 1.0f;
            this.r2c3 = 0.0f;

            this.r3c1 = 0.0f;
            this.r3c2 = 0.0f;
            this.r3c3 = 1.0f;
        }

        public void SetToDiagonal(float d1, float d2, float d3)
        {
            this.r1c1 = d1;
            this.r1c2 = 0.0f;
            this.r1c3 = 0.0f;

            this.r2c1 = 0.0f;
            this.r2c2 = d2;
            this.r2c3 = 0.0f;

            this.r2c1 = 0.0f;
            this.r2c2 = 0.0f;
            this.r2c3 = d3;
        }

        public void Set(in Matrix3x3FP32 matrix)
        {
            this.r1c1 = matrix.r1c1;
            this.r1c2 = matrix.r1c2;
            this.r1c3 = matrix.r1c3;

            this.r2c1 = matrix.r2c1;
            this.r2c2 = matrix.r2c2;
            this.r2c3 = matrix.r2c3;

            this.r3c1 = matrix.r3c1;
            this.r3c2 = matrix.r3c2;
            this.r3c3 = matrix.r3c3;
        }

        public void Set(in Matrix3x3FP64 matrix)
        {
            this.r1c1 = (float)matrix.r1c1;
            this.r1c2 = (float)matrix.r1c2;
            this.r1c3 = (float)matrix.r1c3;

            this.r2c1 = (float)matrix.r2c1;
            this.r2c2 = (float)matrix.r2c2;
            this.r2c3 = (float)matrix.r2c3;

            this.r3c1 = (float)matrix.r3c1;
            this.r3c2 = (float)matrix.r3c2;
            this.r3c3 = (float)matrix.r3c3;
        }

        public void SetRow1(float c1, float c2, float c3)
        {
            this.r1c1 = c1;
            this.r1c2 = c2;
            this.r1c3 = c3;
        }

        public void SetRow2(float c1, float c2, float c3)
        {
            this.r2c1 = c1;
            this.r2c2 = c2;
            this.r2c3 = c3;
        }

        public void SetRow3(float c1, float c2, float c3)
        {
            this.r3c1 = c1;
            this.r3c2 = c2;
            this.r3c3 = c3;
        }

        public void SetColumn1(float r1, float r2, float r3)
        {
            this.r1c1 = r1;
            this.r2c1 = r2;
            this.r3c1 = r3;
        }

        public void SetColumn2(float r1, float r2, float r3)
        {
            this.r1c2 = r1;
            this.r2c2 = r2;
            this.r3c2 = r3;
        }

        public void SetColumn3(float r1, float r2, float r3)
        {
            this.r1c3 = r1;
            this.r2c3 = r2;
            this.r3c3 = r3;
        }

        public static void MakeTransposed(in Matrix3x3FP32 matrix, out Matrix3x3FP32 transposed)
        {
            transposed.r1c1 = matrix.r1c1;
            transposed.r2c2 = matrix.r2c2;
            transposed.r3c3 = matrix.r3c3;

            (transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2);
            (transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3);
            (transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3);
        }

        public static bool MakeInverted(in Matrix3x3FP32 matrix, out Matrix3x3FP32 inverted)
        {
            float determinant = matrix.GetDeterminant();
    
            if (UtilityFP32.IsZero(determinant)) {
                LoadZero(out inverted);
                return false;
            }

            float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2;
            float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3;
            float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2;

            float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3;
            float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1;
            float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3;

            float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1;
            float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2;
            float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1;

            float mutiplier = 1.0f / determinant;

            inverted.r1c1 = r1c1 * mutiplier;
            inverted.r1c2 = r1c2 * mutiplier;
            inverted.r1c3 = r1c3 * mutiplier;

            inverted.r2c1 = r2c1 * mutiplier;
            inverted.r2c2 = r2c2 * mutiplier;
            inverted.r2c3 = r2c3 * mutiplier;

            inverted.r3c1 = r3c1 * mutiplier;
            inverted.r3c2 = r3c2 * mutiplier;
            inverted.r3c3 = r3c3 * mutiplier;

            return true;
        }

        public static void Add(in Matrix3x3FP32 matrix1, in Matrix3x3FP32 matrix2, out Matrix3x3FP32 sum)
        {
            sum.r1c1 = matrix1.r1c1 + matrix2.r1c1;
            sum.r1c2 = matrix1.r1c2 + matrix2.r1c2;
            sum.r1c3 = matrix1.r1c3 + matrix2.r1c3;

            sum.r2c1 = matrix1.r2c1 + matrix2.r2c1;
            sum.r2c2 = matrix1.r2c2 + matrix2.r2c2;
            sum.r2c3 = matrix1.r2c3 + matrix2.r2c3;

            sum.r3c1 = matrix1.r3c1 + matrix2.r3c1;
            sum.r3c2 = matrix1.r3c2 + matrix2.r3c2;
            sum.r3c3 = matrix1.r3c3 + matrix2.r3c3;
        }

        public static void AddScaled(in Matrix3x3FP32 basicMatrix, in Matrix3x3FP32 scalableMatrix, float scale, out Matrix3x3FP32 sum)
        {
            sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale;
            sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale;
            sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale;

            sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale;
            sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale;
            sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale;

            sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale;
            sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale;
            sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale;
        }

        public static void Subtract(in Matrix3x3FP32 minuend, in Matrix3x3FP32 subtrahend, out Matrix3x3FP32 difference)
        {
            difference.r1c1 = minuend.r1c1 - subtrahend.r1c1;
            difference.r1c2 = minuend.r1c2 - subtrahend.r1c2;
            difference.r1c3 = minuend.r1c3 - subtrahend.r1c3;

            difference.r2c1 = minuend.r2c1 - subtrahend.r2c1;
            difference.r2c2 = minuend.r2c2 - subtrahend.r2c2;
            difference.r2c3 = minuend.r2c3 - subtrahend.r2c3;

            difference.r3c1 = minuend.r3c1 - subtrahend.r3c1;
            difference.r3c2 = minuend.r3c2 - subtrahend.r3c2;
            difference.r3c3 = minuend.r3c3 - subtrahend.r3c3;
        }

        public static void Multiply(in Matrix3x3FP32 multiplicand, float multiplier, out Matrix3x3FP32 product)
        {
            product.r1c1 = multiplicand.r1c1 * multiplier;
            product.r1c2 = multiplicand.r1c2 * multiplier;
            product.r1c3 = multiplicand.r1c3 * multiplier;

            product.r2c1 = multiplicand.r2c1 * multiplier;
            product.r2c2 = multiplicand.r2c2 * multiplier;
            product.r2c3 = multiplicand.r2c3 * multiplier;

            product.r3c1 = multiplicand.r3c1 * multiplier;
            product.r3c2 = multiplicand.r3c2 * multiplier;
            product.r3c3 = multiplicand.r3c3 * multiplier;
        }

        public static void Divide(in Matrix3x3FP32 dividend, float divisor, out Matrix3x3FP32 quotient)
        {
            Multiply(dividend, 1.0f / divisor, out quotient);
        }

        public static void RightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result)
        {
            float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3;
            float x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3;
            float x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2 + matrix.r3c3 * vector.x3;

            result.x1 = x1;
            result.x2 = x2;
            result.x3 = x3;
        }

        public static void LeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result)
        {
            float x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1;
            float x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2;
            float x3 = vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3 + vector.x3 * matrix.r3c3;

            result.x1 = x1;
            result.x2 = x2;
            result.x3 = x3;
        }

        public static void LoadZero(out Matrix3x3FP32 matrix)
        {
            matrix.r1c1 = 0.0f;
            matrix.r1c2 = 0.0f;
            matrix.r1c3 = 0.0f;

            matrix.r2c1 = 0.0f;
            matrix.r2c2 = 0.0f;
            matrix.r2c3 = 0.0f;

            matrix.r3c1 = 0.0f;
            matrix.r3c2 = 0.0f;
            matrix.r3c3 = 0.0f;
        }

        public static void LoadIdentity(out Matrix3x3FP32 matrix)
        {
            matrix.r1c1 = 1.0f;
            matrix.r1c2 = 0.0f;
            matrix.r1c3 = 0.0f;

            matrix.r2c1 = 0.0f;
            matrix.r2c2 = 1.0f;
            matrix.r2c3 = 0.0f;

            matrix.r3c1 = 0.0f;
            matrix.r3c2 = 0.0f;
            matrix.r3c3 = 1.0f;
        }

        public static void LoadDiagonal(float d1, float d2, float d3, out Matrix3x3FP32 matrix)
        {
            matrix.r1c1 = d1;
            matrix.r1c2 = 0.0f;
            matrix.r1c3 = 0.0f;

            matrix.r2c1 = 0.0f;
            matrix.r2c2 = d2;
            matrix.r2c3 = 0.0f;

            matrix.r3c1 = 0.0f;
            matrix.r3c2 = 0.0f;
            matrix.r3c3 = d3;
        }
    }
}