/*
 * 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 Geometry
{
    public struct SPVector3
    {
        public static readonly SPVector3 ZERO = new SPVector3(0.0f, 0.0f, 0.0f);

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

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

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

        public SPVector3(in DPVector3 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 - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON)
            {
                return 1;
            }

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

            float module = MathF.Sqrt(squareModule);

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

            return 1;
        }

        public readonly SPVector3 GetNormalized()
        {
            float squareModule = this.GetSquareModule();

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

            if (squareModule <= SPUtility.SQUARE_EPSYLON)
            {
                return ZERO;
            }

            float module = MathF.Sqrt(squareModule);

            return new SPVector3(
                this.x1 / module,
                this.x2 / module,
                this.x3 / module
            );
        }

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

        public readonly SPVector3 GetInverted()
        {
            return new SPVector3(-this.x1, -this.x2, -this.x3);
        }

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

        public readonly bool IsUnit()
        {
            float squareModule = this.GetSquareModule();
            return 1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= SPUtility.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 DPVector3 vector)
        {
            this.x1 = (float)vector.x1;
            this.x2 = (float)vector.x2;
            this.x3 = (float)vector.x3;
        }

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

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

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

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

        public static void Add4(
            in SPVector3 vector1,
            in SPVector3 vector2,
            in SPVector3 vector3,
            in SPVector3 vector4,
            out SPVector3 sum
            )
        {
            sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1);
            sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2);
            sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3);
        }

        public static void Add5(
            in SPVector3 vector1,
            in SPVector3 vector2,
            in SPVector3 vector3,
            in SPVector3 vector4,
            in SPVector3 vector5,
            out SPVector3 sum
            )
        {
            sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1;
            sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2;
            sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3;
        }

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

        public static void GetWeightedSum2(
            float weight1, in SPVector3 vector1,
            float weight2, in SPVector3 vector2,
            out SPVector3 sum
            )
        {
            sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2;
            sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2;
            sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2;
        }

        public static void GetWeightedSum3(
            float weight1, in SPVector3 vector1,
            float weight2, in SPVector3 vector2,
            float weight3, in SPVector3 vector3,
            out SPVector3 sum
            )
        {
            sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3;
            sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3;
            sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2 + vector3.x3 * weight3;
        }

        public static void GetWeightedSum4(
            float weight1, in SPVector3 vector1,
            float weight2, in SPVector3 vector2,
            float weight3, in SPVector3 vector3,
            float weight4, in SPVector3 vector4,
            out SPVector3 sum
            )
        {
            sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4);
            sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4);
            sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4);
        }

        public static void GetWeightedSum5(
            float weight1, in SPVector3 vector1,
            float weight2, in SPVector3 vector2,
            float weight3, in SPVector3 vector3,
            float weight4, in SPVector3 vector4,
            float weight5, in SPVector3 vector5,
            out SPVector3 sum
            )
        {
            sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4) + vector5.x1 * weight5;
            sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4) + vector5.x2 * weight5;
            sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4) + vector5.x3 * weight5;
        }

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

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

        public static void GetMean2(
            in SPVector3 vector1,
            in SPVector3 vector2,
            out SPVector3 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 SPVector3 vector1,
            in SPVector3 vector2,
            in SPVector3 vector3,
            out SPVector3 result
            )
        {
            result.x1 = (vector1.x1 + vector2.x1 + vector3.x1)  * SPUtility.ONE_THIRD;
            result.x2 = (vector1.x2 + vector2.x2 + vector3.x2)  * SPUtility.ONE_THIRD;
            result.x3 = (vector1.x3 + vector2.x3 + vector3.x3)  * SPUtility.ONE_THIRD;
        }

        public static void GetMean4(
            in SPVector3 vector1,
            in SPVector3 vector2,
            in SPVector3 vector3,
            in SPVector3 vector4,
            out SPVector3 result
            )
        {
            result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1)) * 0.25f;
            result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2)) * 0.25f;
            result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3)) * 0.25f;
        }

        public static void GetMean5(
            in SPVector3 vector1,
            in SPVector3 vector2,
            in SPVector3 vector3,
            in SPVector3 vector4,
            in SPVector3 vector5,
            out SPVector3 result
            )
        {
            result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1) * 0.2f;
            result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2) * 0.2f;
            result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3) * 0.2f;
        }

        public static float GetScalarProduct(in SPVector3 vector1, in SPVector3 vector2)
        {
            return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3;
        }
 
        public static void GetCrossProduct(in SPVector3 vector1, in SPVector3 vector2, out SPVector3 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 SPVector3 vector1, in SPVector3 vector2, in SPVector3 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 SPVector3 vector1, in SPVector3 vector2, in SPVector3 vector3, out SPVector3 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 SPVector3 vector1, in SPVector3 vector2, AngleUnit unit)
        {
    		float squareModule1 = vector1.GetSquareModule();

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

    		float squareModule2 = vector2.GetSquareModule();

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

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

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

    		if (cosine <= -(1.0f - SPUtility.EPSYLON))
            {
                return SPAngle.GetHalfCircle(unit);
		    }
    
    		return SPAngle.ConvertFromRadians(MathF.Acos(cosine), unit);
        }
    
        public static float GetSquareDistance(in SPVector3 vector1, in SPVector3 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 SPVector3 vector1, in SPVector3 vector2)
        {
            return MathF.Sqrt(GetSquareDistance(vector1, vector2));
        }
    
        public static bool AreEqual(in SPVector3 vector1, in SPVector3 vector2)
        {
            float squareModule1 = vector1.GetSquareModule();
            float squareModule2 = vector2.GetSquareModule();
            float squareModule3 = GetSquareDistance(vector1, vector2);

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

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

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