/*
 * Author: Andrey Pokidov
 * License: Apache-2.0
 * Date: 4 Dec 2024
 */

namespace BGC
{
    public struct QuaternionFP64
    {
        public double s0 = 0.0, x1 = 0.0, x2 = 0.0, x3 = 0.0;

        public QuaternionFP64(double s0, double x1, double x2, double x3)
        {
            this.s0 = s0;
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;
        }

        public QuaternionFP64(in QuaternionFP64 quaternion)
        {
            this.s0 = quaternion.s0;
            this.x1 = quaternion.x1;
            this.x2 = quaternion.x2;
            this.x3 = quaternion.x3;
        }

        public QuaternionFP64(in QuaternionFP32 quaternion)
        {
            this.s0 = quaternion.s0;
            this.x1 = quaternion.x1;
            this.x2 = quaternion.x2;
            this.x3 = quaternion.x3;
        }

        public readonly double GetSquareModulus()
        {
            return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
        }

        public readonly double GetModulus()
        {
            return Math.Sqrt(this.GetSquareModulus());
        }

        public readonly bool IsZero()
        {
            return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
        }

        public readonly bool IsUnit()
        {
            return UtilityFP64.IsSqareUnit(this.GetSquareModulus());
        }

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

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

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

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

        public bool Invert()
        {
            double squareModulus = this.GetSquareModulus();

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
            {
                return false;
            }

            double multiplicand = 1.0 / squareModulus;

            this.s0 =  this.s0 * multiplicand;
            this.x1 = -this.x1 * multiplicand;
            this.x2 = -this.x2 * multiplicand;
            this.x3 = -this.x3 * multiplicand;

            return true;
        }

        public bool Normalize()
        {
            double squareModulus = this.GetSquareModulus();

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
            {
                return false;
            }

            double multiplier = Math.Sqrt(1.0 / squareModulus);

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

            return true;
        }

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

        public void SetValues(in QuaternionFP64 quaternion)
        {
            this.s0 = quaternion.s0;
            this.x1 = quaternion.x1;
            this.x2 = quaternion.x2;
            this.x3 = quaternion.x3;
        }

        public void SetValues(in QuaternionFP32 quaternion)
        {
            this.s0 = quaternion.s0;
            this.x1 = quaternion.x1;
            this.x2 = quaternion.x2;
            this.x3 = quaternion.x3;
        }

        public static void Reset(out QuaternionFP64 quaternion)
        {
            quaternion.s0 = 0.0;
            quaternion.x1 = 0.0;
            quaternion.x2 = 0.0;
            quaternion.x3 = 0.0;
        }

        public static void MakeUnit(out QuaternionFP64 quaternion)
        {
            quaternion.s0 = 1.0;
            quaternion.x1 = 0.0;
            quaternion.x2 = 0.0;
            quaternion.x3 = 0.0;
        }

        public static void Swap(ref QuaternionFP64 quaternion1, ref QuaternionFP64 quaternion2)
        {
            double s0 = quaternion1.s0;
            double x1 = quaternion1.x1;
            double x2 = quaternion1.x2;
            double x3 = quaternion1.x3;

            quaternion1.s0 = quaternion2.s0;
            quaternion1.x1 = quaternion2.x1;
            quaternion1.x2 = quaternion2.x2;
            quaternion1.x3 = quaternion2.x3;

            quaternion2.s0 = s0;
            quaternion2.x1 = x1;
            quaternion2.x2 = x2;
            quaternion2.x3 = x3;
        }

        public static void Add(in QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, out QuaternionFP64 sum)
        {
            sum.s0 = quaternion1.s0 + quaternion2.s0;
            sum.x1 = quaternion1.x1 + quaternion2.x1;
            sum.x2 = quaternion1.x2 + quaternion2.x2;
            sum.x3 = quaternion1.x3 + quaternion2.x3;
        }

        public static void AddScaled(in QuaternionFP64 basic_quaternion, in QuaternionFP64 scalable_quaternion, double scale, out QuaternionFP64 sum)
        {
            sum.s0 = basic_quaternion.s0 + scale * scalable_quaternion.s0;
            sum.x1 = basic_quaternion.x1 + scale * scalable_quaternion.x1;
            sum.x2 = basic_quaternion.x2 + scale * scalable_quaternion.x2;
            sum.x3 = basic_quaternion.x3 + scale * scalable_quaternion.x3;
        }

        public static void Subtract(in QuaternionFP64 minuend, in QuaternionFP64 subtrahend, out QuaternionFP64 difference)
        {
            difference.s0 = minuend.s0 - subtrahend.s0;
            difference.x1 = minuend.x1 - subtrahend.x1;
            difference.x2 = minuend.x2 - subtrahend.x2;
            difference.x3 = minuend.x3 - subtrahend.x3;
        }

        public static void Multiply(in QuaternionFP64 left, in QuaternionFP64 right, out QuaternionFP64 product)
        {
            double s0 = left.s0 * right.s0 - left.x1 * right.x1 - (left.x2 * right.x2 + left.x3 * right.x3);
            double x1 = left.x1 * right.s0 + left.s0 * right.x1 - (left.x3 * right.x2 - left.x2 * right.x3);
            double x2 = left.x2 * right.s0 + left.s0 * right.x2 - (left.x1 * right.x3 - left.x3 * right.x1);
            double x3 = left.x3 * right.s0 + left.s0 * right.x3 - (left.x2 * right.x1 - left.x1 * right.x2);

            product.s0 = s0;
            product.x1 = x1;
            product.x2 = x2;
            product.x3 = x3;
        }

        public static void Multiply(in QuaternionFP64 quaternion, double multiplier, out QuaternionFP64 product)
        {
            product.s0 = quaternion.s0 * multiplier;
            product.x1 = quaternion.x1 * multiplier;
            product.x2 = quaternion.x2 * multiplier;
            product.x3 = quaternion.x3 * multiplier;
        }

        public static bool Divide(in QuaternionFP64 divident, in QuaternionFP64 divisor, out QuaternionFP64 quotient)
        {
            double squareModulus = divisor.GetSquareModulus();

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
            {
                Reset(out quotient);
                return false;
            }

            double s0 = (divident.s0 * divisor.s0 + divident.x1 * divisor.x1) + (divident.x2 * divisor.x2 + divident.x3 * divisor.x3);
            double x1 = (divident.x1 * divisor.s0 + divident.x3 * divisor.x2) - (divident.s0 * divisor.x1 + divident.x2 * divisor.x3);
            double x2 = (divident.x2 * divisor.s0 + divident.x1 * divisor.x3) - (divident.s0 * divisor.x2 + divident.x3 * divisor.x1);
            double x3 = (divident.x3 * divisor.s0 + divident.x2 * divisor.x1) - (divident.s0 * divisor.x3 + divident.x1 * divisor.x2);

            double multiplicand = 1.0 / squareModulus;

            quotient.s0 = s0 * multiplicand;
            quotient.x1 = x1 * multiplicand;
            quotient.x2 = x2 * multiplicand;
            quotient.x3 = x3 * multiplicand;

            return true;
        }

        public static void Divide(in QuaternionFP64 divident, double divisor, out QuaternionFP64 quotient)
        {
            Multiply(divident, 1.0 / divisor, out quotient);
        }

        public static void GetMeanOfTwo(in QuaternionFP64 vector1, in QuaternionFP64 vector2, out QuaternionFP64 mean)
        {
            mean.s0 = (vector1.s0 + vector2.s0) * 0.5;
            mean.x1 = (vector1.x1 + vector2.x1) * 0.5;
            mean.x2 = (vector1.x2 + vector2.x2) * 0.5;
            mean.x3 = (vector1.x3 + vector2.x3) * 0.5;
        }

        public static void GetMeanOfThree(in QuaternionFP64 vector1, in QuaternionFP64 vector2, in QuaternionFP64 vector3, out QuaternionFP64 mean)
        {
            mean.s0 = (vector1.s0 + vector2.s0 + vector3.s0) * UtilityFP64.ONE_THIRD;
            mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD;
            mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD;
            mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP64.ONE_THIRD;
        }

        public static void Interpolate(in QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, double phase, out QuaternionFP64 interpolation)
        {
            double counterphase = 1.0 - phase;

            interpolation.s0 = quaternion1.s0 * counterphase + quaternion2.s0 * phase;
            interpolation.x1 = quaternion1.x1 * counterphase + quaternion2.x1 * phase;
            interpolation.x2 = quaternion1.x2 * counterphase + quaternion2.x2 * phase;
            interpolation.x3 = quaternion1.x3 * counterphase + quaternion2.x3 * phase;
        }

        public static void GetConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate)
        {
            conjugate.s0 = quaternion.s0;
            conjugate.x1 = -quaternion.x1;
            conjugate.x2 = -quaternion.x2;
            conjugate.x3 = -quaternion.x3;
        }

        public static void GetOpposite(in QuaternionFP64 quaternion, out QuaternionFP64 opposit)
        {
            opposit.s0 = -quaternion.s0;
            opposit.x1 = -quaternion.x1;
            opposit.x2 = -quaternion.x2;
            opposit.x3 = -quaternion.x3;
        }

        public static bool GetInverse(in QuaternionFP64 quaternion, out QuaternionFP64 inverse)
        {
            double squareModulus = quaternion.GetSquareModulus();

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
            {
                Reset(out inverse);
                return false;
            }

            double multiplicand = 1.0 / squareModulus;

            inverse.s0 =  quaternion.s0 * multiplicand;
            inverse.x1 = -quaternion.x1 * multiplicand;
            inverse.x2 = -quaternion.x2 * multiplicand;
            inverse.x3 = -quaternion.x3 * multiplicand;

            return true;
        }

        public static bool GetNormalized(in QuaternionFP64 quaternion, out QuaternionFP64 normalized)
        {
            double squareModulus = quaternion.GetSquareModulus();

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
            {
                Reset(out normalized);
                return false;
            }

            double multiplier = Math.Sqrt(1.0 / squareModulus);

            normalized.s0 = quaternion.s0 * multiplier;
            normalized.x1 = quaternion.x1 * multiplier;
            normalized.x2 = quaternion.x2 * multiplier;
            normalized.x3 = quaternion.x3 * multiplier;

            return true;
        }

        public static bool GetExponation(in QuaternionFP64 quaternion, double exponent, out QuaternionFP64 power)
        {
            double s0s0 = quaternion.s0 * quaternion.s0;
            double x1x1 = quaternion.x1 * quaternion.x1;
            double x2x2 = quaternion.x2 * quaternion.x2;
            double x3x3 = quaternion.x3 * quaternion.x3;

            double squareVector = x1x1 + (x2x2 + x3x3);
            double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);

            if (double.IsNaN(squareModulus))
            {
                Reset(out power);
                return false;
            }

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON)
            {
                Reset(out power);
                return true;
            }

            if (squareVector <= UtilityFP64.SQUARE_EPSYLON)
            {
                if (quaternion.s0 < 0.0f)
                {
                    Reset(out power);
                    return false;
                }

                power.s0 = Math.Pow(quaternion.s0, exponent);
                power.x1 = 0.0f;
                power.x2 = 0.0f;
                power.x3 = 0.0f;

                return true;
            }

            double sine = Math.Sqrt(squareVector / squareModulus);
            double power_angle = Math.Atan2(sine, quaternion.s0 / Math.Sqrt(squareModulus)) * exponent;
            double power_modulus = Math.Pow(squareModulus, 0.5f * exponent);
            double multiplier = power_modulus * Math.Sin(power_angle) / sine;

            power.s0 = power_modulus * Math.Cos(power_angle);
            power.x1 = quaternion.x1 * multiplier;
            power.x2 = quaternion.x2 * multiplier;
            power.x3 = quaternion.x3 * multiplier;

            return true;
        }

        public static bool GetRotationMatrix(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix)
        {
            double s0s0 = quaternion.s0 * quaternion.s0;
            double x1x1 = quaternion.x1 * quaternion.x1;
            double x2x2 = quaternion.x2 * quaternion.x2;
            double x3x3 = quaternion.x3 * quaternion.x3;

            double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
            {
                Matrix3x3FP64.LoadIdentity(out matrix);
                return false;
            }

            double corrector1 = 1.0 / squareModulus;

            double s0x1 = quaternion.s0 * quaternion.x1;
            double s0x2 = quaternion.s0 * quaternion.x2;
            double s0x3 = quaternion.s0 * quaternion.x3;
            double x1x2 = quaternion.x1 * quaternion.x2;
            double x1x3 = quaternion.x1 * quaternion.x3;
            double x2x3 = quaternion.x2 * quaternion.x3;

            double corrector2 = 2.0 * corrector1;

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

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

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

            return true;
        }

        public static bool GetReverseMatrix(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix)
        {
            double s0s0 = quaternion.s0 * quaternion.s0;
            double x1x1 = quaternion.x1 * quaternion.x1;
            double x2x2 = quaternion.x2 * quaternion.x2;
            double x3x3 = quaternion.x3 * quaternion.x3;

            double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);

            if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
            {
                Matrix3x3FP64.LoadIdentity(out matrix);
                return false;
            }

            double corrector1 = 1.0 / squareModulus;

            double s0x1 = quaternion.s0 * quaternion.x1;
            double s0x2 = quaternion.s0 * quaternion.x2;
            double s0x3 = quaternion.s0 * quaternion.x3;
            double x1x2 = quaternion.x1 * quaternion.x2;
            double x1x3 = quaternion.x1 * quaternion.x3;
            double x2x3 = quaternion.x2 * quaternion.x3;

            double corrector2 = 2.0 * corrector1;

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

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

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

            return true;
        }

        public static bool GetBothMatrices(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix, out Matrix3x3FP64 reverse)
        {
            if (GetReverseMatrix(quaternion, out reverse))
            {
                Matrix3x3FP64.GetTransposed(reverse, out matrix);
                return true;
            }

            Matrix3x3FP64.LoadIdentity(out matrix);
            return false;
        }

        public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2)
        {
            double ds0 = quaternion1.s0 - quaternion2.s0;
            double dx1 = quaternion1.x1 - quaternion2.x1;
            double dx2 = quaternion1.x2 - quaternion2.x2;
            double dx3 = quaternion1.x3 - quaternion2.x3;

            double squareModulus1 = quaternion1.GetSquareModulus();
            double squareModulus2 = quaternion2.GetSquareModulus();
            double squareDistance = (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3);

            if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) {
                return squareDistance <= UtilityFP64.SQUARE_EPSYLON;
            }

            return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2;
        }
    }
}