/*
 * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System;

/*
 * Author: Andrey Pokidov
 * Date: 1 Feb 2019
 */

namespace BGC
{
    public struct FP32Vector3
    {
        public static readonly FP32Vector3 ZERO = new FP32Vector3(0.0f, 0.0f, 0.0f);

        public float x1;
        public float x2;
        public float x3;

        public FP32Vector3(float x1, float x2, float x3)
        {
            this.x1 = x1;
            this.x2 = x2;
            this.x3 = x3;
        }

        public FP32Vector3(in FP32Vector3 vector)
        {
            this.x1 = vector.x1;
            this.x2 = vector.x2;
            this.x3 = vector.x3;
        }

        public FP32Vector3(in FP64Vector3 vector)
        {
            this.x1 = (float)vector.x1;
            this.x2 = (float)vector.x2;
            this.x3 = (float)vector.x3;
        }

        public readonly float GetSquareModule()
        {
            return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3;
        }

        public readonly float GetModule()
        {
            return MathF.Sqrt(this.GetSquareModule());
        }

        public int Normalize()
        {
            float squareModule = this.GetSquareModule();

            if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON)
            {
                return 1;
            }

            if (squareModule <= FP32Utility.SQUARE_EPSYLON)
            {
                this.Reset();
                return 0;
            }

            float module = MathF.Sqrt(squareModule);

            this.x1 /= module;
            this.x2 /= module;
            this.x3 /= module;

            return 1;
        }

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

        public readonly bool IsZero()
        {
            return this.GetSquareModule() <= FP32Utility.SQUARE_EPSYLON;
        }

        public readonly bool IsUnit()
        {
            float squareModule = this.GetSquareModule();
            return 1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= FP32Utility.EPSYLON;
        }

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

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

        public void SetValues(in FP64Vector3 vector)
        {
            this.x1 = (float)vector.x1;
            this.x2 = (float)vector.x2;
            this.x3 = (float)vector.x3;
        }

        public void SetValues(in FP32Vector3 vector)
        {
            this.x1 = vector.x1;
            this.x2 = vector.x2;
            this.x3 = vector.x3;
        }

        public void SetReverseOf(in FP32Vector3 vector)
        {
            this.x1 = -vector.x1;
            this.x2 = -vector.x2;
            this.x3 = -vector.x3;
        }

        public void SetReverseOf(in FP64Vector3 vector)
        {
            this.x1 = -(float)vector.x1;
            this.x2 = -(float)vector.x2;
            this.x3 = -(float)vector.x3;
        }

        public void AppendScaled(FP32Vector3 summand, float scale)
        {
            this.x1 += summand.x1 * scale;
            this.x2 += summand.x2 * scale;
            this.x3 += summand.x3 * scale;
        }

        public readonly override string ToString()
        {
            return String.Format("SPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3);
        }

        public static void Add(in FP32Vector3 vector1, in FP32Vector3 vector2, out FP32Vector3 sum)
        {
            sum.x1 = vector1.x1 + vector2.x1;
            sum.x2 = vector1.x2 + vector2.x2;
            sum.x3 = vector1.x3 + vector2.x3;
        }

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

        public static void Muliply(in FP32Vector3 multiplicand, float multiplier, out FP32Vector3 product)
        {
            product.x1 = multiplicand.x1 * multiplier;
            product.x2 = multiplicand.x2 * multiplier;
            product.x3 = multiplicand.x3 * multiplier;
        }

        public static void Divide(in FP32Vector3 dividend, float divisor, out FP32Vector3 quotient)
        {
            quotient.x1 = dividend.x1 / divisor;
            quotient.x2 = dividend.x2 / divisor;
            quotient.x3 = dividend.x3 / divisor;
        }

        public static void GetMean2(in FP32Vector3 vector1, in FP32Vector3 vector2, out FP32Vector3 result)
        {
            result.x1 = (vector1.x1 + vector2.x1) * 0.5f;
            result.x2 = (vector1.x2 + vector2.x2) * 0.5f;
            result.x3 = (vector1.x3 + vector2.x3) * 0.5f;
        }

        public static void GetMean3(in FP32Vector3 vector1, in FP32Vector3 vector2, in FP32Vector3 vector3, out FP32Vector3 result)
        {
            result.x1 = (vector1.x1 + vector2.x1 + vector3.x1)  * FP32Utility.ONE_THIRD;
            result.x2 = (vector1.x2 + vector2.x2 + vector3.x2)  * FP32Utility.ONE_THIRD;
            result.x3 = (vector1.x3 + vector2.x3 + vector3.x3)  * FP32Utility.ONE_THIRD;
        }

        public static float GetScalarProduct(in FP32Vector3 vector1, in FP32Vector3 vector2)
        {
            return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3;
        }
 
        public static void GetCrossProduct(in FP32Vector3 vector1, in FP32Vector3 vector2, out FP32Vector3 result)
        {
            float x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2;
            float x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3;
            float x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;

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

        public static float GetTripleProduct(in FP32Vector3 vector1, in FP32Vector3 vector2, in FP32Vector3 vector3)
        {
            return vector1.x1 * (vector2.x2 * vector3.x3 - vector2.x3 * vector3.x2)
                 + vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3)
                 + vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1);
        }
 
        public static void GetDoubleCrossProduct(in FP32Vector3 vector1, in FP32Vector3 vector2, in FP32Vector3 vector3, out FP32Vector3 result)
        {
            // [a x [b x c]] = b * (a, c) - c * (a, b)
            float ac = GetScalarProduct(vector1, vector3);
            float ab = GetScalarProduct(vector1, vector2);

            result.x1 = ac * vector2.x1 - ab * vector3.x1;
            result.x2 = ac * vector2.x2 - ab * vector3.x2;
            result.x3 = ac * vector2.x3 - ab * vector3.x3;
        }

        public static float GetAngle(in FP32Vector3 vector1, in FP32Vector3 vector2, AngleUnit unit)
        {
            float squareModule1 = vector1.GetSquareModule();

            if (squareModule1 <= FP32Utility.SQUARE_EPSYLON)
            {
                return 0.0f;
            }

            float squareModule2 = vector2.GetSquareModule();

            if (squareModule2 <= FP32Utility.SQUARE_EPSYLON)
            {
                return 0.0f;
            }

            float cosine = FP32Vector3.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2);

            if (1.0f - FP32Utility.EPSYLON <= cosine)
            {
                return 0.0f;
            }

            if (cosine <= -(1.0f - FP32Utility.EPSYLON))
            {
                return FP32Angle.GetHalfCircle(unit);
            }
    
            return FP32Radians.ToUnits(MathF.Acos(cosine), unit);
        }
    
        public static float GetSquareDistance(in FP32Vector3 vector1, in FP32Vector3 vector2)
        {
            float dx1 = vector1.x1 - vector2.x1;
            float dx2 = vector1.x2 - vector2.x2;
            float dx3 = vector1.x3 - vector2.x3;
        
            return dx1 * dx1 + dx2 * dx2 + dx3 * dx3;
        }
    
        public static float GetDistance(in FP32Vector3 vector1, in FP32Vector3 vector2)
        {
            return MathF.Sqrt(GetSquareDistance(vector1, vector2));
        }
    
        public static bool AreEqual(in FP32Vector3 vector1, in FP32Vector3 vector2)
        {
            float squareModule1 = vector1.GetSquareModule();
            float squareModule2 = vector2.GetSquareModule();
            float squareModule3 = GetSquareDistance(vector1, vector2);

            // 3.0f means dimension amount
            if (squareModule1 < FP32Utility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < FP32Utility.EPSYLON_EFFECTIVENESS_LIMIT)
            {
                return squareModule3 < (3.0f * FP32Utility.SQUARE_EPSYLON);
            }

            if (squareModule1 <= squareModule2)
            {
                return squareModule3 <= (3.0f * FP32Utility.SQUARE_EPSYLON) * squareModule2;
            }

            return squareModule3 <= (3.0f * FP32Utility.SQUARE_EPSYLON) * squareModule1;
        }
    }
}