using System;
using System.Numerics;

namespace LinearAlgebra {

    /*
    public struct Vector2Int {
        public int horizontal;
        public int vertical;

        public Vector2Int(int horizontal, int vertical) {
            this.horizontal = horizontal;
            this.vertical = vertical;
        }

        /// <summary>
        /// A vector with zero for all axis
        /// </summary>
        public static readonly Vector2Int zero = new(0, 0);
        /// <summary>
        /// A vector with values (1, 1)
        /// </summary>
        public static readonly Vector2Int one = new(1, 1);
        /// <summary>
        /// A vector with values (0, 1)
        /// </summary>
        public static readonly Vector2Int up = new(0, 1);
        /// <summary>
        /// A vector with values (0, -1)
        /// </summary>
        public static readonly Vector2Int down = new(0, -1);
        /// <summary>
        /// A vector with values (0, 1)
        /// </summary>
        public static readonly Vector2Int forward = new(0, 1);
        /// <summary>
        /// A vector with values (0, -1)
        /// </summary>
        public static readonly Vector2Int back = new(0, -1);
        /// <summary>
        /// A vector3 with values (-1, 0)
        /// </summary>
        public static readonly Vector2Int left = new(-1, 0);
        /// <summary>
        /// A vector with values (1, 0)
        /// </summary>
        public static readonly Vector2Int right = new(1, 0);

        /// <summary>
        /// Tests if the vector has equal values as the given vector
        /// </summary>
        /// <param name="v1">The vector to compare to</param>
        /// <returns><em>true</em> if the vector values are equal</returns>
        public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical;

        /// <summary>
        /// Tests if the vector is equal to the given object
        /// </summary>
        /// <param name="obj">The object to compare to</param>
        /// <returns><em>false</em> when the object is not a Vector2 or does not have equal values</returns>
        public override readonly bool Equals(object obj) {
            if (obj is not Vector2Int v)
                return false;

            return (this.horizontal == v.horizontal && this.vertical == v.vertical);
        }

        /// <summary>
        /// Tests if the two vectors have equal values
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns><em>true</em>when the vectors have equal values</returns>
        /// Note that this uses a Float equality check which cannot be not exact in all cases.
        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
        public static bool operator ==(Vector2Int v1, Vector2Int v2) {
            return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical);
        }
        /// <summary>
        /// Tests if two vectors have different values
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns><em>true</em>when the vectors have different values</returns>
        /// Note that this uses a Float equality check which cannot be not exact in all case.
        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
        public static bool operator !=(Vector2Int v1, Vector2Int v2) {
            return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
        }
        public readonly float magnitude {
            get {
                int h = this.horizontal;
                int v = this.vertical;
                return MathF.Sqrt(h * h + v * v);
            }
        }

        public static float MagnitudeOf(Vector2Int v) {
            return v.magnitude;
        }

        public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) {
            return new Vector2Int(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical);
        }
        public static Vector2Int operator +(Vector2Int v1, Vector2Int v2) {
            return new Vector2Int(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical);
        }

        public static float Distance(Vector2Int v1, Vector2Int v2) {
            return (v1 - v2).magnitude;
        }
    }

    public struct Vector2Float {
        public float horizontal;
        public float vertical;

        public Vector2Float(float horizontal, float vertical) {
            this.horizontal = horizontal;
            this.vertical = vertical;
        }

        public readonly float magnitude {
            get {
                float h = this.horizontal;
                float v = this.vertical;
                return MathF.Sqrt(h * h + v * v);
            }
        }

        public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) {
            return new Vector2Float(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical);
        }

        public static float Distance(Vector2Float v1, Vector2Float v2) {
            return (v1 - v2).magnitude;
        }
    }
    */

    /// <summary>
    /// 2-dimensional vectors
    /// </summary>
    public struct Vector2Float {

        /// <summary>
        /// The right axis of the vector
        /// </summary>
        public float horizontal; // left/right
        /// <summary>
        /// The upward/forward axis of the vector
        /// </summary>
        public float vertical; // forward/backward
                               // directions are to be inline with Vector3 as much as possible...

        /// <summary>
        /// Create a new 2-dimensional vector
        /// </summary>
        /// <param name="x">x axis value</param>
        /// <param name="y">y axis value</param>
        public Vector2Float(float x, float y) {
            this.horizontal = x;
            this.vertical = y;
        }

        /// <summary>
        /// Convert a Vector2Int into a Vector2Float
        /// </summary>
        /// <param name="v">The Vector2Int</param>
        public Vector2Float(Vector2Int v) {
            this.horizontal = v.horizontal;
            this.vertical = v.vertical;
        }

        /// <summary>
        /// A vector with zero for all axis
        /// </summary>
        public static readonly Vector2Float zero = new Vector2Float(0, 0);
        /// <summary>
        /// A vector with values (1, 1)
        /// </summary>
        public static readonly Vector2Float one = new Vector2Float(1, 1);
        /// <summary>
        /// A vector with values (0, 1)
        /// </summary>
        public static readonly Vector2Float up = new Vector2Float(0, 1);
        /// <summary>
        /// A vector with values (0, -1)
        /// </summary>
        public static readonly Vector2Float down = new Vector2Float(0, -1);
        /// <summary>
        /// A vector with values (0, 1)
        /// </summary>
        public static readonly Vector2Float forward = new Vector2Float(0, 1);
        /// <summary>
        /// A vector with values (0, -1)
        /// </summary>
        public static readonly Vector2Float back = new Vector2Float(0, -1);
        /// <summary>
        /// A vector3 with values (-1, 0)
        /// </summary>
        public static readonly Vector2Float left = new Vector2Float(-1, 0);
        /// <summary>
        /// A vector with values (1, 0)
        /// </summary>
        public static readonly Vector2Float right = new Vector2Float(1, 0);

        /// <summary>
        /// The squared length of this vector
        /// </summary>
        /// <returns>The squared length</returns>
        /// The squared length is computationally simpler than the real length.
        /// Think of Pythagoras A^2 + B^2 = C^2.
        /// This leaves out the calculation of the squared root of C.
        public readonly float sqrMagnitude => horizontal * horizontal + vertical * vertical;
        public static float SqrMagnitudeOf(Vector2Float v) {
            return v.sqrMagnitude;
        }

        /// <summary>
        /// The length of this vector
        /// </summary>
        /// <returns>The length of this vector</returns>
        public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical);
        public static float MagnitudeOf(Vector2Float v) {
            return v.magnitude;
        }

        /// <summary>
        /// Convert the vector to a length of a 1
        /// </summary>
        /// <returns>The vector with length 1</returns>
        public Vector2Float normalized {
            get {
                float l = magnitude;
                Vector2Float v = zero;
                if (l > Float.epsilon)
                    v = this / l;
                return v;
            }
        }
        public static Vector2Float Normalize(Vector2Float v) {
            return v.normalized;
        }

        /// <summary>
        /// Add two vectors
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns>The result of adding the two vectors</returns>
        public static Vector2Float operator +(Vector2Float v1, Vector2Float v2) {
            Vector2Float v = new Vector2Float(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical);
            return v;
        }

        /// <summary>
        /// Subtract two vectors
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns>The result of adding the two vectors</returns>
        public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) {
            Vector2Float v = new Vector2Float(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical);
            return v;
        }

        /// <summary>
        /// Negate the vector
        /// </summary>
        /// <param name="v1">The vector to negate</param>
        /// <returns>The negated vector</returns>
        /// This will result in a vector pointing in the opposite direction
        public static Vector2Float operator -(Vector2Float v1) {
            Vector2Float v = new Vector2Float(-v1.horizontal, -v1.vertical);
            return v;
        }

        /// <summary>
        /// Scale a vector uniformly down
        /// </summary>
        /// <param name="v">The vector to scale</param>
        /// <param name="f">The scaling factor</param>
        /// <returns>The scaled vector</returns>
        /// Each component of the vector will be devided by the same factor.
        public static Vector2Float operator /(Vector2Float v, float f) {
            Vector2Float r = new(v.horizontal / f, v.vertical / f);
            return r;
        }


        /// <summary>
        /// Scale a vector uniformly up
        /// </summary>
        /// <param name="v1">The vector to scale</param>
        /// <param name="f">The scaling factor</param>
        /// <returns>The scaled vector</returns>
        /// Each component of the vector will be multipled with the same factor.
        public static Vector2Float operator *(Vector2Float v1, float f) {
            Vector2Float v = new Vector2Float(v1.horizontal * f, v1.vertical * f);
            return v;
        }

        /// <summary>
        /// Scale a vector uniformly up
        /// </summary>
        /// <param name="f">The scaling factor</param>
        /// <param name="v1">The vector to scale</param>
        /// <returns>The scaled vector</returns>
        /// Each component of the vector will be multipled with the same factor.
        public static Vector2Float operator *(float f, Vector2Float v1) {
            Vector2Float v = new Vector2Float(f * v1.horizontal, f * v1.vertical);
            return v;
        }

        /// @brief Scale the vector using another vector
        /// @param v1 The vector to scale
        /// @param v2 A vector with the scaling factors
        /// @return The scaled vector
        /// @remark Each component of the vector v1 will be multiplied with the
        /// matching component from the scaling vector v2.
        public static Vector2Float Scale(Vector2Float v1, Vector2Float v2) {
            return new Vector2Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical);
        }
        
        /// <summary>
        /// Tests if the vector has equal values as the given vector
        /// </summary>
        /// <param name="v1">The vector to compare to</param>
        /// <returns><em>true</em> if the vector values are equal</returns>
        //public readonly bool Equals(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical;

        /// <summary>
        /// Tests if the two vectors have equal values
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns><em>true</em>when the vectors have equal values</returns>
        /// Note that this uses a Float equality check which cannot be not exact in all cases.
        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
        public static bool operator ==(Vector2Float v1, Vector2Float v2) {
            return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical);
        }

        /// <summary>
        /// Tests if two vectors have different values
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns><em>true</em>when the vectors have different values</returns>
        /// Note that this uses a Float equality check which cannot be not exact in all case.
        /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
        /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
        public static bool operator !=(Vector2Float v1, Vector2Float v2) {
            return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
        }

        /// <summary>
        /// Tests if the vector is equal to the given object
        /// </summary>
        /// <param name="obj">The object to compare to</param>
        /// <returns><em>false</em> when the object is not a Vector2 or does not have equal values</returns>
        public override readonly bool Equals(object obj) {
            if (obj is not Vector2Float v)
                return false;

            return (horizontal == v.horizontal && vertical == v.vertical);
        }        

        /// <summary>
        /// Get an hash code for the vector
        /// </summary>
        /// <returns>The hash code</returns>
        public override readonly int GetHashCode() {
            return HashCode.Combine(horizontal, vertical);
        }

        /// <summary>
        /// Get the distance between two vectors
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns>The distance between the two vectors</returns>
        public static float Distance(Vector2Float v1, Vector2Float v2) {
            float x = v1.horizontal - v2.horizontal;
            float y = v1.vertical - v2.vertical;
            float d = (float)Math.Sqrt(x * x + y * y);
            return d;
        }

        /// <summary>
        /// The dot product of two vectors
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns>The dot product of the two vectors</returns>
        public static float Dot(Vector2Float v1, Vector2Float v2) {
            return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical;
        }

        /// <summary>
        /// Calculate the signed angle between two vectors.
        /// </summary>
        /// <param name="from">The starting vector</param>
        /// <param name="to">The ending vector</param>
        /// <param name="axis">The axis to rotate around</param>
        /// <returns>The signed angle in degrees</returns>
        public static float SignedAngle(Vector2Float from, Vector2Float to) {
            //float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y);
            //return Vector2.Angle(v1, v2) * sign;

            float sqrMagFrom = from.sqrMagnitude;
            float sqrMagTo = to.sqrMagnitude;

            if (sqrMagFrom == 0 || sqrMagTo == 0)
                return 0;
            //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
            //    return nanf("");

            float angleFrom = (float)Math.Atan2(from.vertical, from.horizontal);
            float angleTo = (float)Math.Atan2(to.vertical, to.horizontal);
            return -(angleTo - angleFrom) * AngleFloat.Rad2Deg;
        }

        public static float UnsignedAngle(Vector2Float from, Vector2Float to) {
            return MathF.Abs(SignedAngle(from, to));
        }

        /// <summary>
        /// Rotates the vector with the given angle
        /// </summary>
        /// <param name="v1">The vector to rotate</param>
        /// <param name="angle">The angle in degrees</param>
        /// <returns></returns>
        public static Vector2Float Rotate(Vector2Float v1, AngleFloat angle) {
            float sin = (float)Math.Sin(angle.inRadians);
            float cos = (float)Math.Cos(angle.inRadians);
            // float sin = AngleFloat.Sin(angle);
            // float cos = AngleFloat.Cos(angle);

            float tx = v1.horizontal;
            float ty = v1.vertical;
            Vector2Float v = new Vector2Float() {
                horizontal = (cos * tx) - (sin * ty),
                vertical = (sin * tx) + (cos * ty)
            };
            return v;
        }

        /// <summary>
        /// Lerp between two vectors
        /// </summary>
        /// <param name="v1">The from vector</param>
        /// <param name="v2">The to vector</param>
        /// <param name="f">The interpolation distance [0..1]</param>
        /// <returns>The lerped vector</returns>
        /// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1
        /// matches the *v2* vector Value -1 is *v1* vector minus the difference
        /// between *v1* and *v2* etc.
        public static Vector2Float Lerp(Vector2Float v1, Vector2Float v2, float f) {
            Vector2Float v = v1 + (v2 - v1) * f;
            return v;
        }

        /// <summary>
        /// Map interval of angles between vectors [0..Pi] to interval [0..1]
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns>The resulting factor in interval [0..1]</returns>
        /// Vectors a and b must be normalized
        public static float ToFactor(Vector2Float v1, Vector2Float v2) {
            return (1 - Vector2Float.Dot(v1, v2)) / 2;
        }
    }
}