/*
 * Author: Andrey Pokidov
 * License: Apache-2.0
 * Date: 20 Oct 2024
 */

namespace BGC
{
    public struct VersorFP32
    {
        private float s0 = 1.0f;
        private float x1 = 0.0f;
        private float x2 = 0.0f;
        private float x3 = 0.0f;

        public VersorFP32(float s0, float x1, float x2, float x3)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;

            float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
            
            if (!UtilityFP32.IsSqareUnit(squareModulus))
            {
                this.Normalize(squareModulus);
            }
        }

        public VersorFP32(in VersorFP32 versor)
        {
            this.s0 = versor.s0;
            this.x1 = versor.x1;
            this.x2 = versor.x2;
            this.x3 = versor.x3;
        }

        public VersorFP32(in VersorFP64 versor)
        {
            this.s0 = (float)versor.GetScalar();
            this.x1 = (float)versor.GetX1();
            this.x2 = (float)versor.GetX2();
            this.x3 = (float)versor.GetX3();

            float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
            
            if (!UtilityFP32.IsSqareUnit(squareModulus))
            {
                this.Normalize(squareModulus);
            }
        }

        public readonly float GetScalar()
        {
            return this.s0;
        }

        public readonly float GetX1()
        {
            return this.x1;
        }

        public readonly float GetX2()
        {
            return this.x2;
        }

        public readonly float GetX3()
        {
            return this.x3;
        }

        public readonly float GetAngle(AngleUnit unit)
        {
            if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) {
                return 0.0f;
            }

            if (UtilityFP32.IsZero(this.s0))
            {
                return AngleFP32.GetHalfCircle(unit);
            }

            return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit);
        }

        public readonly bool IsIdle()
        {
            return this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || (1.0f - UtilityFP32.EPSYLON) <= this.s0;
        }

        public void Reset()
        {
            this.s0 = 1.0f;
            this.x1 = 0.0f;
            this.x2 = 0.0f;
            this.x3 = 0.0f;
        }

        public void MakeOpposite()
        {
            this.s0 = -this.s0;
            this.x1 = -this.x1;
            this.x2 = -this.x2;
            this.x3 = -this.x3;
        }

        public void Shorten()
        {
            if (this.s0 < 0.0f)
            {
                this.s0 = -this.s0;
                this.x1 = -this.x1;
                this.x2 = -this.x2;
                this.x3 = -this.x3;
            }
        }

        public void Invert()
        {
            this.x1 = -this.x1;
            this.x2 = -this.x2;
            this.x3 = -this.x3;
        }

        public void SetValues(float s0, float x1, float x2, float x3)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;

            float squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
            
            if (!UtilityFP32.IsSqareUnit(squareModulus))
            {
                this.Normalize(squareModulus);
            }
        }

        public void Set(in VersorFP32 versor)
        {
            this.s0 = versor.s0;
            this.x1 = versor.x1;
            this.x2 = versor.x2;
            this.x3 = versor.x3;
        }

        public void Set(in VersorFP64 versor)
        {
            this.SetValues((float) versor.GetScalar(), (float) versor.GetX1(), (float) versor.GetX2(), (float) versor.GetX3());
        }

        private void Normalize(float squareModulus)
        {
            if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || !float.IsFinite(squareModulus))
            {
                this.Reset();
                return;
            }

            float multiplier = MathF.Sqrt(1.0f / squareModulus);

            this.s0 *= multiplier;
            this.x1 *= multiplier;
            this.x2 *= multiplier;
            this.x3 *= multiplier;
        }

        public static void Combine(in VersorFP32 second, in VersorFP32 first, out VersorFP32 result)
        {
            LoadValues(
                (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3),
                (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3),
                (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1),
                (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2),
                out result
            );
        }

        public static void Combine(in VersorFP32 third, in VersorFP32 second, in VersorFP32 first, out VersorFP32 result)
        {
            float s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3);
            float x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3);
            float x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1);
            float x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2);

            LoadValues(
                (third.s0 * s0 - third.x1 * x1) - (third.x2 * x2 + third.x3 * x3),
                (third.x1 * s0 + third.s0 * x1) - (third.x3 * x2 - third.x2 * x3),
                (third.x2 * s0 + third.s0 * x2) - (third.x1 * x3 - third.x3 * x1),
                (third.x3 * s0 + third.s0 * x3) - (third.x2 * x1 - third.x1 * x2),
                out result
            );
        }

        public static void Exclude(in VersorFP32 basic, in VersorFP32 excludant, out VersorFP32 result)
        {
            LoadValues(
                (basic.s0 * excludant.s0 + basic.x1 * excludant.x1) + (basic.x2 * excludant.x2 + basic.x3 * excludant.x3),
                (basic.x1 * excludant.s0 + basic.x3 * excludant.x2) - (basic.s0 * excludant.x1 + basic.x2 * excludant.x3),
                (basic.x2 * excludant.s0 + basic.x1 * excludant.x3) - (basic.s0 * excludant.x2 + basic.x3 * excludant.x1),
                (basic.x3 * excludant.s0 + basic.x2 * excludant.x1) - (basic.s0 * excludant.x3 + basic.x1 * excludant.x2),
                out result
            );
        }

        public static void GetInverted(in VersorFP32 versor, out VersorFP32 conjugate)
        {
            conjugate.s0 = versor.s0;
            conjugate.x1 = -versor.x1;
            conjugate.x2 = -versor.x2;
            conjugate.x3 = -versor.x3;
        }

        public static void GetShortened(in VersorFP32 versor, out VersorFP32 shortened)
        {
            if (versor.s0 < 0.0f) {
                shortened.s0 = -versor.s0;
                shortened.x1 = -versor.x1;
                shortened.x2 = -versor.x2;
                shortened.x3 = -versor.x3;
            }
            else {
                shortened.s0 = versor.s0;
                shortened.x1 = versor.x1;
                shortened.x2 = versor.x2;
                shortened.x3 = versor.x3;
            }
        }

        public static void GetRotationMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix)
        {
            float s0s0 = versor.s0 * versor.s0;
            float x1x1 = versor.x1 * versor.x1;
            float x2x2 = versor.x1 * versor.x2;
            float x3x3 = versor.x1 * versor.x3;

            float s0x1 = versor.s0 * versor.x1;
            float s0x2 = versor.s0 * versor.x2;
            float s0x3 = versor.s0 * versor.x3;

            float x1x2 = versor.x1 * versor.x2;
            float x1x3 = versor.x1 * versor.x3;
            float x2x3 = versor.x2 * versor.x3;

            matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
            matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
            matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);

            matrix.r1c2 = 2.0f * (x1x2 - s0x3);
            matrix.r2c3 = 2.0f * (x2x3 - s0x1);
            matrix.r3c1 = 2.0f * (x1x3 - s0x2);

            matrix.r2c1 = 2.0f * (x1x2 + s0x3);
            matrix.r3c2 = 2.0f * (x2x3 + s0x1);
            matrix.r1c3 = 2.0f * (x1x3 + s0x2);
        }

        public static void GetReverseMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix)
        {
            float s0s0 = versor.s0 * versor.s0;
            float x1x1 = versor.x1 * versor.x1;
            float x2x2 = versor.x1 * versor.x2;
            float x3x3 = versor.x1 * versor.x3;

            float s0x1 = versor.s0 * versor.x1;
            float s0x2 = versor.s0 * versor.x2;
            float s0x3 = versor.s0 * versor.x3;

            float x1x2 = versor.x1 * versor.x2;
            float x1x3 = versor.x1 * versor.x3;
            float x2x3 = versor.x2 * versor.x3;

            matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
            matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
            matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);

            matrix.r1c2 = 2.0f * (x1x2 + s0x3);
            matrix.r2c3 = 2.0f * (x2x3 + s0x1);
            matrix.r3c1 = 2.0f * (x1x3 + s0x2);

            matrix.r2c1 = 2.0f * (x1x2 - s0x3);
            matrix.r3c2 = 2.0f * (x2x3 - s0x1);
            matrix.r1c3 = 2.0f * (x1x3 - s0x2);
        }

        public static void GetBothMatrices(in VersorFP32 versor, out Matrix3x3FP32 matrix, out Matrix3x3FP32 reverse)
        {
            GetReverseMatrix(versor, out reverse);
            Matrix3x3FP32.MakeTransposed(reverse, out matrix);
        }

        public static void Turn(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result)
        {
            float tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
            float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
            float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.x2 * vector.x1);

            float x1 = vector.x1 + tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
            float x2 = vector.x2 + tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
            float x3 = vector.x3 + tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);

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

        public static void TurnBack(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result)
        {
            float tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
            float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
            float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.x2 * vector.x1);

            float x1 = vector.x1 - tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
            float x2 = vector.x2 - tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
            float x3 = vector.x3 - tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);

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

        public static void Reset(out VersorFP32 result)
        {
            result.s0 = 1.0f;
            result.x1 = 0.0f;
            result.x2 = 0.0f;
            result.x3 = 0.0f;
        }

        public static void LoadValues(float s0, float x1, float x2, float x3, out VersorFP32 result)
        {
            float squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3);

            result.s0 = s0;
            result.x1 = x1;
            result.x2 = x2;
            result.x3 = x3;

            if (!UtilityFP32.IsSqareUnit(squareModulus))
            {
                result.Normalize(squareModulus);
            }
        }
    }
}