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

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

        public float x1 = 0.0f;
        public float x2 = 0.0f;

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

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

        public Vector2FP32(in Vector2FP64 vector)
        {
            this.x1 = (float)vector.x1;
            this.x2 = (float)vector.x2;
        }

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

        public readonly float GetModulus()
        {
            return MathF.Sqrt(this.GetSquareModulus());
        }

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

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

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

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

        public readonly Vector2FP32 GetOpposite()
        {
            return new Vector2FP32(-this.x1, -this.x2);
        }

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

            if (UtilityFP32.IsSqareUnit(squareModulus))
            {
                return true;
            }

            if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
            {
                return false;
            }

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

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

            return true;
        }

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

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

        public void SetValues(in Vector2FP64 vector)
        {
            this.x1 = (float)vector.x1;
            this.x2 = (float)vector.x2;
        }
        
        public readonly override string ToString()
        {
            return String.Format("Vector2FP32({0}, {1})", this.x1, this.x2);
        }

        public static void Swap(ref Vector2FP32 vector1, ref Vector2FP32 vector2)
        {
            float x1 = vector1.x1;
            float x2 = vector1.x2;

            vector1.x1 = vector2.x1;
            vector1.x2 = vector2.x2;

            vector2.x1 = x1;
            vector2.x2 = x2;
        }

        public static void Reset(out Vector2FP32 vector)
        {
            vector.x1 = 0.0f;
            vector.x2 = 0.0f;
        }

        public static void GetOpposite(in Vector2FP32 vector, out Vector2FP32 reverted)
        {
            reverted.x1 = -vector.x1;
            reverted.x2 = -vector.x2;
        }

        public static bool GetNormalized(in Vector2FP32 vector, out Vector2FP32 normalized)
        {
            float squareModulus = vector.GetSquareModulus();

            if (UtilityFP32.IsSqareUnit(squareModulus))
            {
                normalized.x1 = vector.x1;
                normalized.x2 = vector.x2;
                return true;
            }

            if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
            {
                normalized.x1 = 0.0f;
                normalized.x2 = 0.0f;
                return false;
            }

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

            normalized.x1 = vector.x1 * multiplier;
            normalized.x2 = vector.x2 * multiplier;
            return true;
        }

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

        public static void AddScaled(in Vector2FP32 basicVector, in Vector2FP32 scalableVector, float scale, out Vector2FP32 sum)
        {
            sum.x1 = basicVector.x1 + scalableVector.x1 * scale;
            sum.x2 = basicVector.x2 + scalableVector.x2 * scale;
        }

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

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

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

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

        public static void GetMeanOfThree(in Vector2FP32 vector1, in Vector2FP32 vector2, in Vector2FP32 vector3, out Vector2FP32 mean)
        {
            mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD;
            mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD;
        }

        public static void Interpolate(in Vector2FP32 vector1, in Vector2FP32 vector2, float phase, out Vector2FP32 interpolation)
        {
            float counterphase = 1.0f - phase;

            interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase;
            interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase;
        }

        public static float GetScalarProduct(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2;
        }
 
        public static float GetCrossProduct(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            return vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
        }

        public static float GetAngle(in Vector2FP32 vector1, in Vector2FP32 vector2, AngleUnit unit)
        {
            float squareModulus1 = vector1.GetSquareModulus();

            if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus1))
            {
                return 0.0f;
            }

            float squareModulus2 = vector2.GetSquareModulus();

            if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus2))
            {
                return 0.0f;
            }

            float multiplier = MathF.Sqrt(1.0f / (squareModulus1 * squareModulus2));

            float x = GetScalarProduct(vector1, vector2);
            float y = GetCrossProduct(vector1, vector2);

            return RadianFP32.ToUnits(MathF.Atan2(y * multiplier, x * multiplier), unit);
        }
    
        public static float GetSquareDistance(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            float dx1 = vector1.x1 - vector2.x1;
            float dx2 = vector1.x2 - vector2.x2;
        
            return dx1 * dx1 + dx2 * dx2;
        }
    
        public static float GetDistance(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            return MathF.Sqrt(GetSquareDistance(vector1, vector2));
        }

        public static bool AreCloseEnough(in Vector2FP32 vector1, in Vector2FP32 vector2, float distanceLimit)
        {
            return 0.0f <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit;
        }

        public static bool AreClose(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            float squareModulus1 = vector1.GetSquareModulus();
            float squareModulus2 = vector2.GetSquareModulus();
            float squareDistance = GetSquareDistance(vector1, vector2);

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

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

        public static bool AreParallel(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            float squareModulus1 = vector1.GetSquareModulus();
            float squareModulus2 = vector2.GetSquareModulus();

            if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
            {
                return true;
            }

            float crossProduct = GetCrossProduct(vector1, vector2);

            return crossProduct * crossProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
        }

        public static bool AreOrthogonal(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            float squareModulus1 = vector1.GetSquareModulus();
            float squareModulus2 = vector2.GetSquareModulus();

            if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
            {
                return true;
            }

            float scalarProduct = GetScalarProduct(vector1, vector2);

            return scalarProduct * scalarProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
        }

        public static Attitude GetAttitude(in Vector2FP32 vector1, in Vector2FP32 vector2)
        {
            float squareModulus1 = vector1.GetSquareModulus();
            float squareModulus2 = vector2.GetSquareModulus();

            if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
            {
                return Attitude.ZERO;
            }

            float squareLimit = UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
            float scalarProduct = GetScalarProduct(vector1, vector2);

            if (scalarProduct * scalarProduct <= squareLimit)
            {
                return Attitude.ORTHOGONAL;
            }

            float crossProduct = GetCrossProduct(vector1, vector2);

            if (crossProduct * crossProduct > squareLimit)
            {
                return Attitude.ANY;
            }

            return scalarProduct > 0.0f ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL;
        }
    }
}